Merge pull request #156 from BlueAndi/dev/speed_units_turtle

Turtle Speeds

lib/APPTurtle/src/App.cpp

@@ -140,7 +140,7 @@ void App::setup()
         else
         {
             /* Trigger immediately. */
-            m_turtleStepTimer.start(0U);
+            m_turtleMovementTimer.start(0U);
             isSuccessful = true;
         }
     }
@@ -274,10 +274,9 @@ bool App::setupSerialMuxProtServer()
     bool isSuccessful = false;
 
     m_serialMuxProtChannelIdStatus = m_smpServer.createChannel(STATUS_CHANNEL_NAME, STATUS_CHANNEL_DLC);
-    m_serialMuxProtChannelIdMotorSpeeds =
-        m_smpServer.createChannel(SPEED_SETPOINT_CHANNEL_NAME, SPEED_SETPOINT_CHANNEL_DLC);
+    m_serialMuxProtChannelIdTurtle = m_smpServer.createChannel(TURTLE_CHANNEL_NAME, TURTLE_CHANNEL_DLC);
 
-    if ((0U == m_serialMuxProtChannelIdStatus) || (0U == m_serialMuxProtChannelIdMotorSpeeds))
+    if ((0U == m_serialMuxProtChannelIdStatus) || (0U == m_serialMuxProtChannelIdTurtle))
     {
         LOG_ERROR("Failed to create SerialMuxProt channels.");
     }
@@ -295,57 +294,39 @@ void App::handleTurtle()
     /* Check for new data. */
     if (true == m_isNewTurtleSpeedSetpoint)
     {
-        /*
-        Max speed is currently unknown to DCS, but using a big value will ensure that
-        RU drives as fast as possible as it will cap the speed to its own maximum.
-        */
-        const int16_t MOTOR_MAX_SPEED = 5000;
-        SpeedData     payload;
-
-        /*
-        Move turtle regardless of the exact speed setpoint received.
-        */
-        if (m_turtleSpeedSetpoint.linear.x > 0.0F)
-        {
-            payload.left  = MOTOR_MAX_SPEED;
-            payload.right = MOTOR_MAX_SPEED;
-        }
-        /* Turn left. */
-        else if (m_turtleSpeedSetpoint.angular.z > 0.0F)
-        {
-            payload.left  = -MOTOR_MAX_SPEED;
-            payload.right = MOTOR_MAX_SPEED;
-        }
-        /* Turn right. */
-        else if (m_turtleSpeedSetpoint.angular.z < 0.0F)
-        {
-            payload.left  = MOTOR_MAX_SPEED;
-            payload.right = -MOTOR_MAX_SPEED;
-        }
+        TurtleSpeed   payload;
+        const int32_t MILLI_CONVERSION_FACTOR = 1000;
+        int32_t       linearSpeed = m_turtleSpeedSetpoint.linear.x * MILLI_CONVERSION_FACTOR; /* Linear speed in mm/s */
+        int32_t angularSpeed = m_turtleSpeedSetpoint.angular.z * MILLI_CONVERSION_FACTOR; /* Angular speed in mrad/s */
+
+        payload.linearCenter = linearSpeed;
+        payload.angular      = angularSpeed;
+
+        LOG_DEBUG("Linear speed: %d mm/s, Angular speed: %d mrad/s", payload.linearCenter, payload.angular);
 
-        if (false == m_smpServer.sendData(m_serialMuxProtChannelIdMotorSpeeds, &payload, sizeof(payload)))
+        if (false == m_smpServer.sendData(m_serialMuxProtChannelIdTurtle, &payload, sizeof(payload)))
         {
             LOG_WARNING("Failed to send motor speeds to RU.");
         }
         else
         {
             m_isNewTurtleSpeedSetpoint = false;
-            m_turtleStepTimer.start(TURTLE_STEP_TIMER_INTERVAL);
+            m_turtleMovementTimer.start(TURTLE_STEP_TIMER_INTERVAL);
         }
     }
 
-    if (true == m_turtleStepTimer.isTimeout())
+    if (true == m_turtleMovementTimer.isTimeout())
     {
-        SpeedData payload;
-        payload.left  = 0;
-        payload.right = 0;
+        TurtleSpeed payload;
+        payload.linearCenter = 0;
+        payload.angular      = 0;
 
-        if (false == m_smpServer.sendData(m_serialMuxProtChannelIdMotorSpeeds, &payload, sizeof(payload)))
+        if (false == m_smpServer.sendData(m_serialMuxProtChannelIdTurtle, &payload, sizeof(payload)))
         {
             LOG_WARNING("Failed to send motor speeds to RU.");
         }
 
-        m_turtleStepTimer.stop();
+        m_turtleMovementTimer.stop();
     }
 }
 

lib/APPTurtle/src/App.h

@@ -68,10 +68,10 @@ class App
     App() :
         m_ros(),
         m_serialMuxProtChannelIdStatus(0U),
-        m_serialMuxProtChannelIdMotorSpeeds(0U),
+        m_serialMuxProtChannelIdTurtle(0U),
         m_smpServer(Board::getInstance().getRobot().getStream(), this),
         m_statusTimer(),
-        m_turtleStepTimer(),
+        m_turtleMovementTimer(),
         m_turtleSpeedSetpoint(),
         m_isNewTurtleSpeedSetpoint(true)
     {
@@ -121,9 +121,9 @@ class App
     uint8_t m_serialMuxProtChannelIdStatus;
 
     /**
-     * SerialMuxProt Channel id for sending motor speeds.
+     * SerialMuxProt Channel id for sending Turtle speeds.
      */
-    uint8_t m_serialMuxProtChannelIdMotorSpeeds;
+    uint8_t m_serialMuxProtChannelIdTurtle;
 
     /**
      * SerialMuxProt Server Instance
@@ -136,9 +136,9 @@ class App
     SimpleTimer m_statusTimer;
 
     /**
-     * Timer for the steps of the turtle.
+     * Timer for the movement of the turtle.
      */
-    SimpleTimer m_turtleStepTimer;
+    SimpleTimer m_turtleMovementTimer;
 
     /**
      * Turtle speed setpoint.

lib/APPTurtle/src/SerialMuxChannels.h

@@ -86,6 +86,12 @@
 /** DLC of Line Sensor Channel */
 #define LINE_SENSOR_CHANNEL_DLC (sizeof(LineSensorData))
 
+/** Name of the Channel to send Turtle Speeds. */
+#define TURTLE_CHANNEL_NAME "TURTLE"
+
+/** DLC of Turtle Channel */
+#define TURTLE_CHANNEL_DLC (sizeof(TurtleSpeed))
+
 /******************************************************************************
  * Types and Classes
  *****************************************************************************/
@@ -173,16 +179,16 @@ typedef struct _CommandResponse
     /** Response Payload. */
     union
     {
-        int16_t maxMotorSpeed; /**< Max speed [steps/s]. */
+        int32_t maxMotorSpeed; /**< Max speed [mm/s]. */
     };
 } __attribute__((packed)) CommandResponse;
 
 /** Struct of the "Speed" channel payload. */
 typedef struct _SpeedData
 {
-    int16_t left;   /**< Left motor speed [steps/s] */
-    int16_t right;  /**< Right motor speed [steps/s] */
-    int16_t center; /**< Center motor speed [steps/s] */
+    int32_t left;   /**< Left motor speed [mm/s] */
+    int32_t right;  /**< Right motor speed [mm/s] */
+    int32_t center; /**< Center motor speed [mm/s] */
 } __attribute__((packed)) SpeedData;
 
 /** Struct of the "Current Vehicle Data" channel payload. */
@@ -191,9 +197,9 @@ typedef struct _VehicleData
     int32_t                  xPos;        /**< X position [mm]. */
     int32_t                  yPos;        /**< Y position [mm]. */
     int32_t                  orientation; /**< Orientation [mrad]. */
-    int16_t                  left;        /**< Left motor speed [steps/s]. */
-    int16_t                  right;       /**< Right motor speed [steps/s]. */
-    int16_t                  center;      /**< Center speed [steps/s]. */
+    int32_t                  left;        /**< Left motor speed [mm/s]. */
+    int32_t                  right;       /**< Right motor speed [mm/s]. */
+    int32_t                  center;      /**< Center speed [mm/s]. */
     SMPChannelPayload::Range proximity;   /**< Range at which object is found [range]. */
 } __attribute__((packed)) VehicleData;
 
@@ -209,6 +215,13 @@ typedef struct _LineSensorData
     uint16_t lineSensorData[5U]; /**< Line sensor data [digits] normalized to max 1000 digits. */
 } __attribute__((packed)) LineSensorData;
 
+/** Struct of the "Turtle" channel payload. */
+typedef struct _TurtleSpeed
+{
+    int32_t linearCenter; /**< Linear speed of the vehicle center. [mm/s] */
+    int32_t angular;      /**< Angular speed. [mrad/s] */
+} __attribute__((packed)) TurtleSpeed;
+
 /******************************************************************************
  * Functions
  *****************************************************************************/