isPositionReached in motor_control class

Author: gbr1

src/definitions/robot_definitions.h

@@ -37,6 +37,7 @@ const float MOTOR_RATIO = MOTOR_CPR*MOTOR_GEAR_RATIO;
 #define POSITION_KD_DEFAULT 0.0001
 #define POSITION_CONTROL_PERIOD 0.02
 #define POSITION_MAX_SPEED 30.0
+#define POSITION_THRESHOLD 2.0
 
 
 

src/motor_control/motor_control.cpp

@@ -35,6 +35,7 @@ MotorControl::MotorControl(DCmotor * _motor, Encoder * _encoder, const float _kp
     pos_controller_period = _pos_controller_period;
     pos_max_velocity = _pos_max_velocity;
     position_control_enabled = false;
+    is_position_reached = false;
 
 
 
@@ -168,6 +169,9 @@ void MotorControl::update(){
     if (position_control_enabled){
         pos_pid->update(position);
         setRPM(round(pos_pid->getControlOutput()/10.0)*10);
+        if (abs(pos_pid->getError())<POSITION_THRESHOLD){
+            is_position_reached = true;
+        }
     }
 }
 
@@ -210,6 +214,7 @@ void MotorControl::enablePositionControl(){
 
 void MotorControl::disablePositionControl(){
     position_control_enabled = false;
+    is_position_reached = false;
 }
 
 bool MotorControl::isPositionControlEnabled(){
@@ -230,5 +235,10 @@ float MotorControl::getPosition(){
 void MotorControl::setPosition(const float degree){
     pos_pid->setReference(degree);
     enablePositionControl();
+    is_position_reached = false;
+}
+
+bool MotorControl::isPositionReached(){
+    return is_position_reached;
 }
 

src/motor_control/motor_control.h

@@ -43,6 +43,7 @@ class MotorControl{
         float pos_controller_period;
         float pos_max_velocity;
         bool position_control_enabled;
+        bool is_position_reached;
 
         float position;
         float angle;
@@ -112,6 +113,7 @@ class MotorControl{
         void setPosition(const float degree);                          // set the reference for position control
         float getPosition();                                            // get the actual angle in degrees of motor
         void  resetPosition(const float p0=0.0);                        // reset/set the position value
+        bool isPositionReached();
 
         float getError();