fix

Author: gbr1

src/Arduino_AlvikCarrier.cpp

@@ -75,10 +75,12 @@ Arduino_AlvikCarrier::Arduino_AlvikCarrier(){
 
     // kinematics
     kinematics = new Kinematics(WHEEL_TRACK_MM, WHEEL_DIAMETER_MM);
-    kinematics_movement = 0;
+    kinematics_movement = MOVEMENT_DISABLED;
     kinematics_achieved = false;
+    previous_travel = 0.0;
+    move_direction = 0.0;
     rotate_pid = new PidController(ROTATE_KP_DEFAULT, ROTATE_KI_DEFAULT, ROTATE_KD_DEFAULT, ROTATE_CONTROL_PERIOD, ROTATE_MAX_SPEED);
-
+    move_pid = new PidController(MOVE_KP_DEFAULT, MOTOR_KI_DEFAULT, MOVE_KD_DEFAULT, MOVE_CONTROL_PERIOD, MOVE_MAX_SPEED);
 }
 
 int Arduino_AlvikCarrier::begin(){
@@ -148,7 +150,8 @@ void Arduino_AlvikCarrier::getVersion(uint8_t &high_byte, uint8_t &mid_byte, uin
 
 int Arduino_AlvikCarrier::beginAPDS(){
     pinMode(APDS_LED, OUTPUT);
-    enableIlluminator();
+    //enableIlluminator();
+    disableIlluminator();
     if (!apds9960->begin()){
         return ERROR_APDS;
     }
@@ -704,7 +707,7 @@ void Arduino_AlvikCarrier::lockingRotate(const float angle){
 void Arduino_AlvikCarrier::rotate(const float angle){
     rotate_pid->reset();
     rotate_pid->setReference(kinematics->getTheta()+angle);
-    kinematics_movement=1;
+    kinematics_movement=MOVEMENT_ROTATE;
     kinematics_achieved=false;
 }
 
@@ -739,25 +742,42 @@ void Arduino_AlvikCarrier::lockingMove(const float distance){
 
 void Arduino_AlvikCarrier::move(const float distance){
     move_pid->reset();
-    //move_pid->setReference()
+    previous_travel=kinematics->getTravel();
+    if (distance<0){
+        move_direction=-1.0;
+    }
+    else{
+        move_direction=1.0;
+    }
+    move_pid->setReference(distance);
+    kinematics_movement=MOVEMENT_MOVE;
+    kinematics_achieved=false;
 }
 
 void Arduino_AlvikCarrier::updateKinematics(){
     kinematics->inverse(motor_control_left->getRPM(), motor_control_right->getRPM());
     kinematics->updatePose();
-    if (kinematics_movement!=0){
-        if (kinematics_movement==1){
+    if (kinematics_movement!=MOVEMENT_DISABLED){
+        if (kinematics_movement==MOVEMENT_ROTATE){
             rotate_pid->update(kinematics->getTheta());
-            drive(0,round(rotate_pid->getControlOutput()/10.0)*10);
+            drive(0, round(rotate_pid->getControlOutput()/10.0)*10);
             if (abs(rotate_pid->getError())<ROTATE_THREASHOLD){
                 kinematics_achieved=true;
             }
         }
+        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;
+            }
+        }
     }
 }
 
 void Arduino_AlvikCarrier::disableKinematicsMovement(){
-    kinematics_movement=0;
+    kinematics_movement=MOVEMENT_DISABLED;
 }
 
 bool Arduino_AlvikCarrier::isTargetReached(){

src/Arduino_AlvikCarrier.h

@@ -70,9 +70,12 @@ class Arduino_AlvikCarrier{
 
 
         uint8_t kinematics_movement;
+        bool kinematics_achieved;
+        float previous_travel;
+        float move_direction;
+
         PidController * rotate_pid;
         PidController * move_pid;
-        bool kinematics_achieved;
 
 
 

src/definitions/robot_definitions.h

@@ -13,6 +13,10 @@
 #ifndef __ROBOT_DEFINITIONS_H__
 #define __ROBOT_DEFINITIONS_H__
 
+// Robot parameters
+#define WHEEL_TRACK_MM 89.0
+#define WHEEL_DIAMETER_MM 34.0
+
 // Motor Control and mechanical parameters
 #define CONTROL_LIMIT 4096              // PWM resolution
 
@@ -34,13 +38,28 @@ const float MOTOR_RATIO = MOTOR_CPR*MOTOR_GEAR_RATIO;
 #define POSITION_CONTROL_PERIOD 0.02
 #define POSITION_MAX_SPEED 30.0
 
+
+
+// Kinematics control
 #define ROTATE_KP_DEFAULT 5.0
 #define ROTATE_KI_DEFAULT 0.0
 #define ROTATE_KD_DEFAULT 0.001
 #define ROTATE_CONTROL_PERIOD 0.02
-#define ROTATE_MAX_SPEED 45
+#define ROTATE_MAX_SPEED 45.0
 #define ROTATE_THREASHOLD 3
 
+#define MOVE_KP_DEFAULT 5.0
+#define MOVE_KI_DEFAULT 0.0
+#define MOVE_KD_DEFAULT 0.001
+#define MOVE_CONTROL_PERIOD 0.02
+#define MOVE_MAX_SPEED 45.0
+#define MOVE_THREADSHOLD 3
+
+#define MOVEMENT_DISABLED 0
+#define MOVEMENT_ROTATE 1
+#define MOVEMENT_MOVE 2
+
+
 
 
 // Sensor fusioning parameters
@@ -56,15 +75,13 @@ const float MOTION_FX_PERIOD = (1000U / MOTION_FX_FREQ);
 #define GBIAS_ACC_TH_SC                 (2.0f*0.000765f)
 #define GBIAS_GYRO_TH_SC                (2.0f*0.002f)
 #define MOTION_FX_DECIMATION                      1U
-//#define STATE_SIZE                      (size_t)(2432)
 
+
+// Library version
 #define VERSION_BYTE_HIGH 0
 #define VERSION_BYTE_MID 1
 #define VERSION_BYTE_LOW 2
 
 
 
-#define WHEEL_TRACK_MM 89
-#define WHEEL_DIAMETER_MM 34
-
 #endif

src/robotics/kinematics.h

@@ -177,6 +177,10 @@ class Kinematics{
 
         }
 
+        void resetTravel(){
+            travel=0.0;
+        }
+
         float getX(){
             return m_to_mm(x);
         }