update kinematics, check units

Author: gbr1

src/Arduino_AlvikCarrier.cpp

@@ -622,39 +622,44 @@ void Arduino_AlvikCarrier::rotate(const float angle){
             error=final_angle-kinematics->getTheta();
             Serial.println(error);
         }
-        if (error>0){
-            drive(0,60);
+        if (angle>0){
+            drive(0,40);
         }else{
-            drive(0,-60);
+            drive(0,-40);
         }
     }
     drive(0,0);
     updateMotors();
+    motor_control_left->brake();
+    motor_control_right->brake();
 }
 
-/*
+
+
 void Arduino_AlvikCarrier::move(const float distance){
-    float initial_distance=kinematics->get();
-    float final_distance=distance+initial_distance;
-    float error=distance;
+    float initial_travel=1000.0*kinematics->getTravel();
+    float final_travel=abs(distance)+initial_travel;
+    float error=abs(distance);
     unsigned long t=millis();
-    while(abs(error)>2){
+
+    while(error>2){
         if (millis()-t>20){
             t=millis();
             updateMotors();
             kinematics->inverse(motor_control_left->getRPM(),motor_control_right->getRPM());
             kinematics->updatePose();
-            error=final_distance-kinematics->getTheta();
-            Serial.println(error);
+            error=final_travel-1000.0*kinematics->getTravel();
+            Serial.println(kinematics->getTravel());
         }
-        if (error>0){
+        if (distance>0){
             drive(40,0);
         }else{
             drive(-40,0);
         }
     }
     drive(0,0);
     updateMotors();
-    */
+    motor_control_left->brake();
+    motor_control_right->brake();
 }
 

src/Arduino_AlvikCarrier.h

@@ -197,7 +197,8 @@ class Arduino_AlvikCarrier{
         // Kinematics
         void drive(const float linear, const float angular);                            // set mm/s and deg/s of the robot
 
-        void rotate(const float angle);                                                 // set degrees
+        void move(const float distance);                                                // move of distance millimeters
+        void rotate(const float angle);                                                 // rotate of angle degrees
 
 
 

src/robotics/kinematics.h

@@ -13,6 +13,7 @@
 #define __KINEMATICS_H__
 
 #include "Arduino.h"
+#include "../definitions/robot_definitions.h"
 
 class Kinematics{
     private:
@@ -35,6 +36,8 @@ class Kinematics{
         float delta_x, delta_y;
         float travel;
 
+        float control_period;
+
 
         float mm_to_m(const float mm){
             return mm*0.001;
@@ -70,7 +73,7 @@ class Kinematics{
 
 
     public:
-        Kinematics(const float _wheel_track, const float _wheel_diameter){
+        Kinematics(const float _wheel_track, const float _wheel_diameter, const float _control_period=MOTOR_CONTROL_PERIOD){
             w=0.0;
             left_velocity=0.0;
             right_velocity=0.0;
@@ -94,6 +97,8 @@ class Kinematics{
             delta_right=0.0;
             delta_travel=0.0;
             travel=0.0;
+
+            control_period=_control_period*0.001;                       // convert ms to s
         }
 
         void forward(const float linear, const float angular){
@@ -148,12 +153,14 @@ class Kinematics{
         */
 
         void updatePose(){
-            delta_theta=angular_velocity*0.02;
-            delta_x=linear_velocity*cos(theta)*0.02;
-            delta_y=linear_velocity*sin(theta)*0.02;
+            delta_theta=angular_velocity*control_period;
+            delta_x=linear_velocity*cos(theta)*control_period;
+            delta_y=linear_velocity*sin(theta)*control_period;
+            delta_travel=sqrt(delta_x*delta_x+delta_y*delta_y);
             x+=delta_x;
             y+=delta_y;
             theta+=delta_theta;
+            travel+=delta_travel;
         }
 
         void resetPose(const float initial_x=0.0, const float initial_y=0.0, const float initial_theta=0.0){
@@ -183,7 +190,7 @@ class Kinematics{
         }
 
         float getTravel(){
-            return travel;
+            return m_to_mm(travel);
         }
 
         float getDeltaX(){