Try to kepp motors quiet on zero velocity

industrial-robotics-lab · May 17, 2021 · 983d920 · 983d920
1 parent 592544e
commit 983d920
Showing 1 changed file with 12 additions and 3 deletions.
diff --git a/lib/SerialTransceiver/SerialTransceiver.cpp b/lib/SerialTransceiver/SerialTransceiver.cpp
@@ -28,9 +28,18 @@ void SerialTransceiver::rx()
         calcChecksum = crc8((uint8_t *)controlVec, 3);
         if (readChecksum == calcChecksum)
         {
-            (*desiredVelocity)(0) = mapUint8ToFloat(controlVec[0], -maxAngSpeed, maxAngSpeed);
-            (*desiredVelocity)(1) = mapUint8ToFloat(controlVec[1], -maxLinSpeed, maxLinSpeed);
-            (*desiredVelocity)(2) = mapUint8ToFloat(controlVec[2], -maxLinSpeed, maxLinSpeed);
+            if (controlVec[0] == controlVec[1] == controlVec[2] == 127)
+            {
+                (*desiredVelocity)(0) = 0.0;
+                (*desiredVelocity)(1) = 0.0;
+                (*desiredVelocity)(2) = 0.0;
+            }
+            else
+            {
+                (*desiredVelocity)(0) = mapUint8ToFloat(controlVec[0], -maxAngSpeed, maxAngSpeed);
+                (*desiredVelocity)(1) = mapUint8ToFloat(controlVec[1], -maxLinSpeed, maxLinSpeed);
+                (*desiredVelocity)(2) = mapUint8ToFloat(controlVec[2], -maxLinSpeed, maxLinSpeed);
+            }
             // memcpy(desiredVelocity, vec, 12);
         }
     }