kinematic pose fix, preliminary rotate

Author: gbr1

examples/kinematics/kinematics.ino

@@ -31,11 +31,16 @@ void loop() {
   if (millis()-tmotor>20){
     tmotor=millis();
     alvik.updateMotors();
-    alvik.kinematics->updatePose(alvik.motor_control_left->getTravel(), alvik.motor_control_right->getTravel());
+    alvik.kinematics->inverse(alvik.motor_control_left->getRPM(),alvik.motor_control_right->getRPM());
+    alvik.kinematics->updatePose();
     Serial.print("\t");
-    Serial.print(alvik.kinematics->getDeltaX());
+    Serial.print(alvik.kinematics->getLinearVelocity());
     Serial.print("\t");
-    Serial.print(alvik.kinematics->getDeltaY());
+    Serial.print(alvik.kinematics->getAngularVelocity());
+    Serial.print("\t");
+    Serial.print(alvik.kinematics->getX());
+    Serial.print("\t");
+    Serial.print(alvik.kinematics->getY());
     Serial.print("\t");
     Serial.print(alvik.kinematics->getTheta());
     Serial.print("\n");
@@ -45,16 +50,16 @@ void loop() {
     ttask=millis();
     switch (task){
       case 0:
-        alvik.drive(50,0);
+        alvik.rotate(90);
         break;
       case 1:
-        alvik.drive(0,0);
+        alvik.drive(40,0);
         break;
       case 2:
-        alvik.drive(0,-90);
+        alvik.rotate(-90);
         break;
       case 3:
-        alvik.drive(0,0);
+        alvik.drive(-40,0);
         break;
     }
     task++;

library.properties

@@ -1,4 +1,4 @@
-name=Arduino Alvik Carrier
+name=Arduino_AlvikCarrier
 version=0.1.0
 author=Arduino, Giovanni di Dio Bruno, Lucio Rossi
 maintainer=Arduino <[email protected]>

src/Arduino_AlvikCarrier.cpp

@@ -610,22 +610,23 @@ void Arduino_AlvikCarrier::drive(const float linear, const float angular){
 
 void Arduino_AlvikCarrier::rotate(const float angle){
     float initial_angle=kinematics->getTheta();
-    float error=angle-initial_angle;
+    float final_angle=angle+initial_angle;
+    float error=angle;
     unsigned long t=millis();
     while(abs(error)>2){
         if (millis()-t>20){
             t=millis();
             updateMotors();
-            kinematics->updatePose(motor_control_left->getAngle(),motor_control_right->getAngle());
-            error=angle-kinematics->getTheta();
+            kinematics->inverse(motor_control_left->getRPM(),motor_control_right->getRPM());
+            kinematics->updatePose();
+            error=final_angle-kinematics->getTheta();
             Serial.println(error);
         }
         if (error>0){
-            drive(0,40);
+            drive(0,60);
         }else{
-            drive(0,-40);
+            drive(0,-60);
         }
-
     }
     drive(0,0);
     updateMotors();

src/Arduino_AlvikCarrier.h

@@ -197,7 +197,7 @@ 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);
+        void rotate(const float angle);                                                 // set degrees
 
 
 

src/robotics/kinematics.h

@@ -24,13 +24,9 @@ class Kinematics{
         float wheel_diameter;
         float wheel_radius;
         float wheel_circumference;
-        float rads_to_rpm;
-        float rpm_to_rads;
-        float rads_to_degs;
-        float degs_to_rads;
-        float rotations_to_rads;
-        float rads_to_rotations;
-        float w;
+
+        float v, w;
+        float left_vel, right_vel;
 
         float theta, delta_theta;
         float x, y;
@@ -40,6 +36,38 @@ class Kinematics{
         float travel;
 
 
+        float mm_to_m(const float mm){
+            return mm*0.001;
+        }
+
+        float m_to_mm(const float m){
+            return m*1000.0;
+        }
+
+        float rads_to_degs(const float rads){
+            return rads*180.0/PI;
+        }
+
+        float degs_to_rads(const float degs){
+            return degs*PI/180.0;
+        }
+
+        float rads_to_rpm(const float rads){
+            return rads*60.0/(2.0*PI);
+        }
+
+        float rpm_to_rads(const float rpm){
+            return rpm*2.0*PI/60.0;
+        }
+
+        float rads_to_rotations(const float rads){
+            return rads/(2.0*PI);
+        }
+
+        float rotations_to_rads(const float rotations){
+            return rotations*2.0*PI;
+        }
+
 
     public:
         Kinematics(const float _wheel_track, const float _wheel_diameter){
@@ -48,19 +76,12 @@ class Kinematics{
             right_velocity=0.0;
             linear_velocity=0.0;
             angular_velocity=0.0;
-            wheel_track=_wheel_track;
-            wheel_diameter=_wheel_diameter;
-            wheel_radius=wheel_diameter/2.0;
-            wheel_circumference=wheel_diameter*PI;
 
-            rads_to_rpm=60.0/(2.0*PI);
-            rpm_to_rads=2.0*PI/60.0;
+            wheel_track=mm_to_m(_wheel_track);
+            wheel_diameter=mm_to_m(_wheel_diameter);
 
-            rads_to_degs=180.0/PI;
-            degs_to_rads=PI/180.0;
-
-            rads_to_rotations=1/(2.0*PI);
-            rotations_to_rads=2.0*PI;
+            wheel_radius=wheel_diameter/2.0;
+            wheel_circumference=wheel_diameter*PI;
 
             theta=0.0;
             x=0.0;
@@ -77,51 +98,69 @@ class Kinematics{
         }
 
         void forward(const float linear, const float angular){
-            w = angular*degs_to_rads;
-            left_velocity=(2*linear-w*wheel_track)/(wheel_diameter);
-            left_velocity=rads_to_rpm*left_velocity;
-            right_velocity=(2*linear+w*wheel_track)/(wheel_diameter);
-            right_velocity=rads_to_rpm*right_velocity;
+            v = mm_to_m(linear);
+            w = degs_to_rads(angular);
+
+            left_velocity=(2*v-w*wheel_track)/(wheel_diameter);
+            right_velocity=(2*v+w*wheel_track)/(wheel_diameter);
         }
 
-        void inverse(const float left_vel, const float right_vel){
+        void inverse(const float left, const float right){
+            left_vel=rpm_to_rads(left);
+            right_vel=rpm_to_rads(right);
+
             linear_velocity=(left_vel+right_vel)*wheel_radius/2.0;
             angular_velocity=(-left_vel+right_vel)*wheel_radius/wheel_track;
         }
 
         float getLeftVelocity(){
-            return left_velocity;
+            return rads_to_rpm(left_velocity);
         }
 
         float getRightVelocity(){
-            return right_velocity;
+            return rads_to_rpm(right_velocity);
         }
 
         float getLinearVelocity(){
-            return linear_velocity;
+            return m_to_mm(linear_velocity);
         }
 
         float getAngularVelocity(){
-            return angular_velocity;
+            return rads_to_degs(angular_velocity);
         }
 
+        /*
         void updatePose(const float left_rotation, const float right_rotation){
             delta_left=left_rotation*wheel_circumference;
             delta_right=right_rotation*wheel_circumference;
             delta_travel=(delta_left+delta_right)/2.0;
-            delta_theta=(-delta_left+delta_right)/(2.0*wheel_track);
+            delta_theta=(-delta_left+delta_right)/(wheel_track);
+            
             delta_x=delta_travel*cos(theta+delta_theta/2.0);
             delta_y=delta_travel*sin(theta+delta_theta/2.0);
+            
+            delta_x=delta_travel*cos(delta_theta);
+            delta_y=delta_travel*sin(delta_theta);
+            x+=delta_x;
+            y+=delta_y;
+            theta+=delta_theta;
+            travel+=delta_travel; 
+        }
+        */
+
+        void updatePose(){
+            delta_theta=angular_velocity*0.02;
+            delta_x=linear_velocity*cos(theta)*0.02;
+            delta_y=linear_velocity*sin(theta)*0.02;
             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){
-            x=initial_x;
-            y=initial_y;
-            theta=degs_to_rads*initial_theta;
+            x=mm_to_m(initial_x);
+            y=mm_to_m(initial_y);
+            theta=degs_to_rads(initial_theta);
             travel=0.0;
             delta_x=0.0;
             delta_y=0.0;
@@ -141,7 +180,7 @@ class Kinematics{
         }
 
         float getTheta(){
-            return rads_to_degs*theta;
+            return rads_to_degs(theta);
         }
 
         float getTravel(){
@@ -157,7 +196,7 @@ class Kinematics{
         }
 
         float getDeltaTheta(){
-            return rads_to_degs*delta_theta;
+            return rads_to_degs(delta_theta);
         }