fixs are required on pose

Author: gbr1

examples/kinematics/kinematics.ino

@@ -7,6 +7,7 @@ unsigned long ttask=0;
 uint8_t task=0;
 
 void setup() {
+  Serial.begin(115200);
   alvik.begin();
   ttask=millis();
   tmotor=millis();
@@ -17,13 +18,21 @@ void loop() {
   if (millis()-tmotor>20){
     tmotor=millis();
     alvik.updateMotors();
+    alvik.kinematics->updatePose(alvik.motor_control_left->getTravel(), alvik.motor_control_right->getTravel());
+    Serial.print("\t");
+    Serial.print(alvik.kinematics->getDeltaX());
+    Serial.print("\t");
+    Serial.print(alvik.kinematics->getDeltaY());
+    Serial.print("\t");
+    Serial.print(alvik.kinematics->getTheta());
+    Serial.print("\n");
   }
 
   if (millis()-ttask>2000){
     ttask=millis();
     switch (task){
       case 0:
-        alvik.drive(0,90);
+        alvik.drive(50,0);
         break;
       case 1:
         alvik.drive(0,0);
@@ -40,4 +49,5 @@ void loop() {
       task=0;
     }
   }
+  
 }

src/Arduino_Alvik_Firmware.cpp

@@ -606,7 +606,35 @@ void Arduino_Alvik_Firmware::errorLed(const int error_code){
 }
 
 
+
+/******************************************************************************************************/
+/*                                             Kinematics                                             */
+/******************************************************************************************************/
+
 void Arduino_Alvik_Firmware::drive(const float linear, const float angular){
     kinematics->forward(linear, angular);
     setRpm(kinematics->getLeftVelocity(),kinematics->getRightVelocity());
 }
+
+void Arduino_Alvik_Firmware::rotate(const float angle){
+    float initial_angle=kinematics->getTheta();
+    float error=angle-initial_angle;
+    unsigned long t=millis();
+    while(abs(error)>0){
+        if (millis()-t>20){
+            t=millis();
+            updateMotors();
+        }
+        if (error>0){
+            drive(0,90);
+        }else{
+            drive(0,-90);
+        }
+        kinematics->updatePose(motor_control_left->getTravel(),motor_control_right->getTravel());
+        error=kinematics->getTheta()-initial_angle;
+        Serial.println(error);
+    }
+    drive(0,0);
+    updateMotors();
+}
+

src/Arduino_Alvik_Firmware.h

@@ -75,11 +75,11 @@ class Arduino_Alvik_Firmware{
         uint8_t version_mid;
         uint8_t version_low;
 
-        Kinematics * kinematics;
 
 
     public:        
-    
+        Kinematics * kinematics;
+
         MotorControl * motor_control_right;
         MotorControl * motor_control_left;
 
@@ -203,6 +203,7 @@ class Arduino_Alvik_Firmware{
         // Kinematics
         void drive(const float linear, const float angular);
 
+        void rotate(const float angle);