added robot velocity into firmware

Author: gbr1

examples/firmware_01/firmware_01.ino

@@ -186,10 +186,16 @@ void loop(){
   if (millis()-tmotor>20){
     tmotor=millis();
     alvik.updateMotors();
+    alvik.updateKinematics();
+    // joint speed
     msg_size = packeter.packetC2F('j', alvik.getRpmLeft(),alvik.getRpmRight());
     alvik.serial->write(packeter.msg,msg_size);
+    // joint position
     msg_size = packeter.packetC2F('w', alvik.getPositionLeft(),alvik.getPositionRight());
-    alvik.serial->write(packeter.msg,msg_size);
+    alvik.serial->write(packeter.msg, msg_size);
+    // robot speed
+    msg_size = packeter.packetC2F('v', alvik.getLinearVelocity(), alvik.getAngularVelocity());
+    alvik.serial->write(packeter.msg, msg_size);
   }
 
   if (millis()-timu>10){

src/Arduino_AlvikCarrier.cpp

@@ -681,6 +681,14 @@ void Arduino_AlvikCarrier::drive(const float linear, const float angular){
     setRpm(kinematics->getLeftVelocity(), kinematics->getRightVelocity());
 }
 
+float Arduino_AlvikCarrier::getLinearVelocity(){
+    return kinematics->getLinearVelocity();
+}
+
+float Arduino_AlvikCarrier::getAngularVelocity(){
+    return kinematics->getAngularVelocity();
+}
+
 
 void Arduino_AlvikCarrier::lockingRotate(const float angle){
     float initial_angle = kinematics->getTheta();

src/Arduino_AlvikCarrier.h

@@ -220,6 +220,8 @@ class Arduino_AlvikCarrier{
         // Kinematics
         void updateKinematics();                                                        // update pose/velocity of the robot and controls
         void drive(const float linear, const float angular);                            // set mm/s and deg/s of the robot
+        float getLinearVelocity();
+        float getAngularVelocity();
 
         void move(const float distance);                                                // move of distance millimeters
         void rotate(const float angle);                                                 // rotate of angle degrees