reset pose

Author: gbr1

examples/firmware_01/firmware_01.ino

@@ -45,6 +45,7 @@ uint8_t sensor_id = 0;
 
 uint8_t pid;
 float kp, ki, kd;
+float x, y, theta;
 
 uint8_t servo_A, servo_B;
 
@@ -150,14 +151,19 @@ void loop(){
         break;
 
       case 'R':
-        packeter.unpacketC1F(code,value);
+        packeter.unpacketC1F(code, value);
         alvik.rotate(value);
         break;
 
       case 'G':
-        packeter.unpacketC1F(code,value);
+        packeter.unpacketC1F(code, value);
         alvik.move(value);
         break;
+
+      case 'Z':
+        packeter.unpacketC3F(code, x, y, theta);
+        alvik.resetPose(x, y, theta);
+        break;
     }
   }
 
@@ -234,4 +240,4 @@ void loop(){
     msg_size = packeter.packetC6F('i', alvik.getAccelerationX(), alvik.getAccelerationY(), alvik.getAccelerationZ(), alvik.getAngularVelocityX(), alvik.getAngularVelocityY(), alvik.getAngularVelocityZ());
     alvik.serial->write(packeter.msg,msg_size);
   }
-}
+}

src/Arduino_AlvikCarrier.cpp

@@ -716,6 +716,10 @@ float Arduino_AlvikCarrier::getTheta(){
     return kinematics->getTheta();
 }
 
+void Arduino_AlvikCarrier::resetPose(const float x0, const float y0, const float theta0){
+    kinematics->resetPose(x0,y0,theta0);
+}
+
 void Arduino_AlvikCarrier::lockingRotate(const float angle){
     float initial_angle = kinematics->getTheta();
     float final_angle = angle+initial_angle;

src/Arduino_AlvikCarrier.h

@@ -225,6 +225,7 @@ class Arduino_AlvikCarrier{
         float getX();                                                                   // absolute position in mm
         float getY();                                                                   // absolute position in mm
         float getTheta();                                                               // angle in deg
+        void resetPose(const float x0=0.0, const float y0=0.0, const float theta0=0.0); // reset pose in kinematics
 
         void move(const float distance);                                                // move of distance millimeters
         void rotate(const float angle);                                                 // rotate of angle degrees