Merge pull request #39 from BCLab-UNM/bug-simulationdriving

Robots able to drive in sim using the PID values in driveController. This fixes a problem introduced in sim when the PIDs were tuned for hardware.

Author: gmfricke

src/behaviours/src/DriveController.h

@@ -29,7 +29,7 @@ class DriveController : virtual Controller
 
   bool interupt = false;
 
-  float rotateOnlyAngleTolerance = 0.1;
+  float rotateOnlyAngleTolerance = 0.15;
   float finalRotationTolerance = 0.2;
   const float waypointTolerance = 0.15; //15 cm tolerance.
 

src/sbridge/src/sbridge.cpp

@@ -24,7 +24,7 @@ void sbridge::cmdHandler(const geometry_msgs::Twist::ConstPtr& message) {
     double left = (message->linear.x);
     double right = (message->angular.z);
 
-    float max_turn_rate = 3.14; //radians per second
+    float max_turn_rate = 4.5; //radians per second
     float max_linear_velocity = 0.6; // meters per second
 
     float turn = 0;
@@ -47,8 +47,8 @@ void sbridge::cmdHandler(const geometry_msgs::Twist::ConstPtr& message) {
         float linearVel = (left + right)/2;
         float angularVel = (right-left)/2;
 
-        turn = angularVel/180;
-        forward = linearVel/400;
+        turn = angularVel/55;
+        forward = linearVel/425;
         if (forward >= 150){
 
             forward -= (abs(turn)/5);