Bug fix: Limited the max motor command values

Fixed a problem where the values sent to the Arduino were not capped. The robot turns violently causing the motors to stop working temporarily. Reduced the cap for manual driving to 120 from 255 and applied the cap to autonomous driving where it wasn't applied before. Tested in two physical rovers and in the sim.

Author: gmfricke

src/abridge/src/abridge.cpp

@@ -161,40 +161,41 @@ void driveCommandHandler(const geometry_msgs::Twist::ConstPtr& message) {
   float left = (message->linear.x); //target linear velocity in meters per second
   float right = (message->angular.z); //angular error in radians
 
-  if (currentMode == 1) //manual control
+  // Cap motor commands at 120. Experimentally determined that high values (tested 180 and 255) can cause 
+  // the hardware to fail when the robot moves itself too violently.
+  int max_motor_cmd = 120;
+
+  // Assumes left and right are always between -1 and 1
+  float linear = left * max_motor_cmd; 
+  float angular = right * max_motor_cmd; 
+
+  left = linear - angular;
+  right = linear + angular;
+
+  if (left > max_motor_cmd)
+  {
+    left = max_motor_cmd;
+  }
+  else if (left < -max_motor_cmd)
   {
+    left = - max_motor_cmd;
+  }
 
-	float linear = left * 255;
-	float angular = right * 255; //scale values between -255 and 255;
-
-	left = linear - angular;
-	right = linear + angular;
-
-	if (left >255)
-	{
-	  left = 255;
-        }
-        else if (left < -255)
-        {
-          left = -255;
- 	}
-
-	if (right >255)
-	{
-	  right = 255;
-        }
-        else if (right < -255)
-        {
-          right = -255;
- 	}
+  if (right > max_motor_cmd)
+  {
+    right = max_motor_cmd;
+  }
+  else if (right < -max_motor_cmd)
+  {
+    right = -max_motor_cmd;
   }
 
   int leftInt = left;
   int rightInt = right;
 
-    sprintf(moveCmd, "v,%d,%d\n", leftInt, rightInt); //format data for arduino into c string
-    usb.sendData(moveCmd);                      //send movement command to arduino over usb
-    memset(&moveCmd, '\0', sizeof (moveCmd));   //clear the movement command string
+  sprintf(moveCmd, "v,%d,%d\n", leftInt, rightInt); //format data for arduino into c string
+  usb.sendData(moveCmd);                      //send movement command to arduino over usb
+  memset(&moveCmd, '\0', sizeof (moveCmd));   //clear the movement command string
 }