added disableKinematics after reaching target

Author: gbr1

src/Arduino_AlvikCarrier.cpp

@@ -679,13 +679,15 @@ void Arduino_AlvikCarrier::updateKinematics(){
             drive(0, round(rotate_pid->getControlOutput()/10.0)*10);
             if (abs(rotate_pid->getError())<ROTATE_THREASHOLD){
                 kinematics_achieved=true;
+                disableKinematicsMovement();
             }
         }
         if (kinematics_movement==MOVEMENT_MOVE){
             move_pid->update((kinematics->getTravel()-previous_travel)*move_direction);
             drive(round(move_pid->getControlOutput()/10.0)*10, 0);
             if (abs(move_pid->getError())<MOVE_THREADSHOLD){
                 kinematics_achieved=true;
+                disableKinematicsMovement();
             }
 
         }

src/definitions/robot_definitions.h

@@ -46,7 +46,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.0
-#define ROTATE_THREASHOLD 3
+#define ROTATE_THREASHOLD 1
 
 #define MOVE_KP_DEFAULT 5.0
 #define MOVE_KI_DEFAULT 0.0