Fixed simulation not driving like real rovers

sBrdige would not map speeds correctly and the max speed was to low for rotation
This has been corrected to allow 4.5 radians per second and 0.6 m/s for respective movments and roughly mapping -255 - +255 onto -4.5 - +4.5 for rotation and the equivalent for linear

Also the maximum alignment error was increased by 0.05 radians to prevent possible oscilliaions.

Known additonal bug, robot may seem to freeze on rare occasion biased towards the start of sim but it will eventually turn to correct heading and drive.

Author: JCHJones

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);