kinematics now returns the state

Author: gbr1

src/Arduino_AlvikCarrier.cpp

@@ -76,6 +76,7 @@ Arduino_AlvikCarrier::Arduino_AlvikCarrier(){
     // kinematics
     kinematics = new Kinematics(WHEEL_TRACK_MM, WHEEL_DIAMETER_MM);
     kinematics_movement = 0;
+    kinematics_achieved = false;
     rotate_pid = new PidController(ROTATE_KP_DEFAULT, ROTATE_KI_DEFAULT, ROTATE_KD_DEFAULT, ROTATE_CONTROL_PERIOD, ROTATE_MAX_SPEED);
 
 }
@@ -704,11 +705,12 @@ void Arduino_AlvikCarrier::rotate(const float angle){
     rotate_pid->reset();
     rotate_pid->setReference(kinematics->getTheta()+angle);
     kinematics_movement=1;
+    kinematics_achieved=false;
 }
 
 
 
-void Arduino_AlvikCarrier::move(const float distance){
+void Arduino_AlvikCarrier::lockingMove(const float distance){
     float initial_travel = kinematics->getTravel();
     float final_travel = abs(distance)+initial_travel;
     float error = abs(distance);
@@ -735,7 +737,10 @@ void Arduino_AlvikCarrier::move(const float distance){
 }
 
 
-
+void Arduino_AlvikCarrier::move(const float distance){
+    move_pid->reset();
+    //move_pid->setReference()
+}
 
 void Arduino_AlvikCarrier::updateKinematics(){
     kinematics->inverse(motor_control_left->getRPM(), motor_control_right->getRPM());
@@ -744,10 +749,17 @@ void Arduino_AlvikCarrier::updateKinematics(){
         if (kinematics_movement==1){
             rotate_pid->update(kinematics->getTheta());
             drive(0,round(rotate_pid->getControlOutput()/10.0)*10);
+            if (abs(rotate_pid->getError())<ROTATE_THREASHOLD){
+                kinematics_achieved=true;
+            }
         }
     }
 }
 
 void Arduino_AlvikCarrier::disableKinematicsMovement(){
     kinematics_movement=0;
 }
+
+bool Arduino_AlvikCarrier::isTargetReached(){
+    return kinematics_achieved;
+}

src/Arduino_AlvikCarrier.h

@@ -72,6 +72,7 @@ class Arduino_AlvikCarrier{
         uint8_t kinematics_movement;
         PidController * rotate_pid;
         PidController * move_pid;
+        bool kinematics_achieved;
 
 
 
@@ -220,8 +221,10 @@ class Arduino_AlvikCarrier{
 
 
         void lockingRotate(const float angle);                                                 
+        void lockingMove(const float distance);                                                // move of distance millimeters
 
         void disableKinematicsMovement();
+        bool isTargetReached();
 
 
 };

src/definitions/robot_definitions.h

@@ -39,6 +39,7 @@ const float MOTOR_RATIO = MOTOR_CPR*MOTOR_GEAR_RATIO;
 #define ROTATE_KD_DEFAULT 0.001
 #define ROTATE_CONTROL_PERIOD 0.02
 #define ROTATE_MAX_SPEED 45
+#define ROTATE_THREASHOLD 3