robot pose

Author: gbr1

examples/firmware_01/firmware_01.ino

@@ -209,6 +209,9 @@ void loop(){
     // robot speed
     msg_size = packeter.packetC2F('v', alvik.getLinearVelocity(), alvik.getAngularVelocity());
     alvik.serial->write(packeter.msg, msg_size);
+    // pose
+    msg_size = packeter.packetC3F('s', alvik.getX(), alvik.getY(), alvik.getTheta());
+    alvik.serial->write(packeter.msg, msg_size);
 
     if (alvik.getKinematicsMovement()!=MOVEMENT_DISABLED){
       if (alvik.isTargetReached()){

src/Arduino_AlvikCarrier.cpp

@@ -704,6 +704,18 @@ float Arduino_AlvikCarrier::getAngularVelocity(){
     return kinematics->getAngularVelocity();
 }
 
+float Arduino_AlvikCarrier::getX(){
+    return kinematics->getX();
+}
+
+float Arduino_AlvikCarrier::getY(){
+    return kinematics->getY();
+}
+
+float Arduino_AlvikCarrier::getTheta(){
+    return kinematics->getTheta();
+}
+
 void Arduino_AlvikCarrier::lockingRotate(const float angle){
     float initial_angle = kinematics->getTheta();
     float final_angle = angle+initial_angle;

src/Arduino_AlvikCarrier.h

@@ -222,6 +222,9 @@ class Arduino_AlvikCarrier{
         void drive(const float linear, const float angular);                            // set mm/s and deg/s of the robot
         float getLinearVelocity();                                                      // get linear velocity of the robot in mm/s
         float getAngularVelocity();                                                     // get angular velocity of the robot in deg/s
+        float getX();                                                                   // absolute position in mm
+        float getY();                                                                   // absolute position in mm
+        float getTheta();                                                               // angle in deg
 
         void move(const float distance);                                                // move of distance millimeters
         void rotate(const float angle);                                                 // rotate of angle degrees