commit 04114d139191ab6a6893f0af9770f0a8ed341778
Author: Alex <azirbel@andrew.cmu.edu>
Date:   Sat Nov 17 03:21:05 2012 -0500

    Fixed teleop with new units, and changes the refresh rate to be closer to true m/s.

diff --git a/scout/motors/msg/set_motors.msg b/scout/motors/msg/set_motors.msg
index d7a66fd..5957eb2 100644
--- a/scout/motors/msg/set_motors.msg
+++ b/scout/motors/msg/set_motors.msg
@@ -6,7 +6,8 @@ bool fr_set
 bool bl_set
 bool br_set
 
-# The absolute motor speeds to set (-255 to 255)
+# The absolute motor speeds to set (-128 to 127)
+# TODO check that this is the correct range
 int8 fl_speed
 int8 fr_speed
 int8 bl_speed
diff --git a/scout/scoutsim/src/scout.cpp b/scout/scoutsim/src/scout.cpp
index 59734a6..803e5c2 100644
--- a/scout/scoutsim/src/scout.cpp
+++ b/scout/scoutsim/src/scout.cpp
@@ -63,8 +63,8 @@ namespace scoutsim
                  wxBitmap *path_bitmap,
                  float orient)
         : path_bitmap(path_bitmap)
-	  , sonar_visual_on(sonar_visual_on)
-	  , node (nh)
+      , sonar_visual_on(sonar_visual_on)
+      , node (nh)
           , scout_image(scout_image)
           , pos(pos)
           , orient(orient)
@@ -150,54 +150,51 @@ namespace scoutsim
             motor_br_speed = absolute_to_mps(msg->br_speed);
         }
     }
-	//scout sonar callback
 
     bool Scout::handle_sonar_toggle(sonar::sonar_toggle::Request  &req,
-	                     sonar::sonar_toggle::Response &res)
+                         sonar::sonar_toggle::Response &res)
     {
         if (req.set_on && !sonar_on)
         {
-	        ROS_INFO("Turning on the sonar");
+            ROS_INFO("Turning on the sonar");
             sonar_on = true;
 
         }
         else if (!req.set_on && sonar_on)
         {
-	        ROS_INFO("Turning off the sonar");
+            ROS_INFO("Turning off the sonar");
             sonar_on = false;
         }
         else
         {
-        ROS_INFO("Sonar state remains unchanged");
+            ROS_INFO("Sonar state remains unchanged");
         }
-
-    res.ack = true;
-    return true;
+        res.ack = true;
+        return true;
     }
 
-
-bool Scout::handle_sonar_set_scan(sonar::sonar_set_scan::Request  &req,
-                           sonar::sonar_set_scan::Response &res)
-{
-    // Code to set the sonar to scan from
-    // req.stop_l to req.stop_r
-    if (req.stop_l>=0 and req.stop_r<=23 and req.stop_l<=req.stop_r)
+    bool Scout::handle_sonar_set_scan(sonar::sonar_set_scan::Request  &req,
+                                      sonar::sonar_set_scan::Response &res)
     {
-    ROS_INFO("Setting sonar scan range to [%i, %i]",
-             req.stop_l,
-             req.stop_r);
-    sonar_stop_l = req.stop_l;
-    sonar_stop_r = req.stop_r;
-    sonar_position = req.stop_l;
-    sonar_direction = 1;
-    res.ack = true;
-    }
-    else
-    {
-    ROS_INFO("Bad Input: Input should be integers 0-23, stop_l<stop_r");
+        // Code to set the sonar to scan from
+        // req.stop_l to req.stop_r
+        if (req.stop_l>=0 and req.stop_r<=23 and req.stop_l<=req.stop_r)
+        {
+            ROS_INFO("Setting sonar scan range to [%i, %i]",
+                     req.stop_l,
+                     req.stop_r);
+            sonar_stop_l = req.stop_l;
+            sonar_stop_r = req.stop_r;
+            sonar_position = req.stop_l;
+            sonar_direction = 1;
+            res.ack = true;
+        }
+        else
+        {
+            ROS_INFO("Bad Input: Input should be integers 0-23, stop_l<stop_r");
+        }
+        return true;
     }
-    return true;
-}
 
     bool Scout::setPenCallback(scoutsim::SetPen::Request& req,
                                scoutsim::SetPen::Response&)
@@ -284,48 +281,49 @@ bool Scout::handle_sonar_set_scan(sonar::sonar_set_scan::Request  &req,
             unsigned char r = walls_image.GetRed(d_x, d_y);
             unsigned char g = walls_image.GetGreen(d_x, d_y);
             unsigned char b = walls_image.GetBlue(d_x, d_y);
-	
+    
             reading = rgb_to_grey(r, g, b);
-	    
+        
 
             d++;
         }
         /// @todo Consider using different cutoffs for different features
         while (reading < 128); /// @todo Get rid of hardcoded stuff like this
         
-	if(sonar_visual_on)
-	{   
-		if(isFront)
-		{
-			// draw a circle at the wall_x, wall_y where reading > 128
-			sonar_dc.SelectObject(*path_bitmap);
-			sonar_dc.SetBrush(*wxRED_BRUSH); //old value
-			sonar_dc.DrawCircle(wxPoint(old_front_dx,old_front_dy), 2);
-			old_front_dx = d_x;
-			old_front_dy = d_y;
-		}
-		else
-		{
-			// draw a circle at the wall_x, wall_y where reading > 128
-			sonar_dc.SelectObject(*path_bitmap);
-			sonar_dc.SetBrush(*wxRED_BRUSH); //old value
-			sonar_dc.DrawCircle(wxPoint(old_back_dx,old_back_dy), 2);
-			old_back_dx = d_x;
-			old_back_dy = d_y;
-		}
-
-		sonar_dc.SetBrush(*wxGREEN_BRUSH);  //newest value
-		sonar_dc.DrawCircle( wxPoint(d_x,d_y), 2);
-		if (isFront) //for some reason isFront = (!isFront) is not working
-		{
-			isFront = FALSE;
-		}
-		else
-		{
-			isFront = TRUE;	
-		}
-			
-	}
+        if (sonar_visual_on)
+        {   
+            if (isFront)
+            {
+                // draw a circle at the wall_x, wall_y where reading > 128
+                sonar_dc.SelectObject(*path_bitmap);
+                sonar_dc.SetBrush(*wxRED_BRUSH); //old value
+                sonar_dc.DrawCircle(wxPoint(old_front_dx,old_front_dy), 2);
+                old_front_dx = d_x;
+                old_front_dy = d_y;
+            }
+            else
+            {
+                // draw a circle at the wall_x, wall_y where reading > 128
+                sonar_dc.SelectObject(*path_bitmap);
+                sonar_dc.SetBrush(*wxRED_BRUSH); //old value
+                sonar_dc.DrawCircle(wxPoint(old_back_dx,old_back_dy), 2);
+                old_back_dx = d_x;
+                old_back_dy = d_y;
+            }
+
+            sonar_dc.SetBrush(*wxGREEN_BRUSH);  //newest value
+            sonar_dc.DrawCircle( wxPoint(d_x,d_y), 2);
+            if (isFront) //for some reason isFront = (!isFront) is not working
+            {
+                isFront = FALSE;
+            }
+            else
+            {
+                isFront = TRUE; 
+            }
+                
+        }
+
         return d;
     }
 
@@ -392,8 +390,9 @@ bool Scout::handle_sonar_set_scan(sonar::sonar_set_scan::Request  &req,
 
     /// Sends back the position of this scout so scoutsim can save
     /// the world state
+    /// TODO remove dt param
     geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc,
-					                    wxMemoryDC& sonar_dc,
+                                        wxMemoryDC& sonar_dc,
                                         bool sonar_visual, 
                                         const wxImage& path_image,
                                         const wxImage& lines_image,
@@ -402,8 +401,8 @@ bool Scout::handle_sonar_set_scan(sonar::sonar_set_scan::Request  &req,
                                         wxColour sonar_color,
                                         world_state state)
     {
-
         sonar_visual_on = sonar_visual;
+
         // Assume that the two motors on the same side will be set to
         // roughly the same speed. Does not account for slip conditions
         // when they are set to different speeds.
@@ -428,7 +427,6 @@ bool Scout::handle_sonar_set_scan(sonar::sonar_set_scan::Request  &req,
 
         orient = fmod((float) (orient + ang_dist), (float) (2.0 * PI));
 
-        // Multiply by dt, the time passed (SCOUTSIM_REFRESH_RATE)
         pos.x += sin(orient + PI/2.0) * lin_dist;
         pos.y += cos(orient + PI/2.0) * lin_dist;
 
diff --git a/scout/scoutsim/src/scout_constants.h b/scout/scoutsim/src/scout_constants.h
index 47167b6..c21b368 100644
--- a/scout/scoutsim/src/scout_constants.h
+++ b/scout/scoutsim/src/scout_constants.h
@@ -48,10 +48,16 @@
 #ifndef _SCOUTSIM_SCOUT_CONSTANTS_
 #define _SCOUTSIM_SCOUT_CONSTANTS_
 
+/// TODO put these in a utility file somewhere?
+#define min(x,y) ((x < y) ? x : y)
+#define max(x,y) ((x > y) ? x : y)
+
 namespace scoutsim
 {
     // Maximum speed which will be sent to scoutsim in absolute units
-    const int MAX_ABSOLUTE_SPEED = 255;
+    const int MAX_ABSOLUTE_SPEED = 127;
+    const int MIN_ABSOLUTE_SPEED = -128;
+
     // Speed in m/s corresponding to maximum absolute speed
     const float MAX_SPEED_MPS = 1.0;
 
@@ -75,7 +81,7 @@ namespace scoutsim
     // Time it takes for the sonar to spin from position 0 to position 23
     const float SONAR_HALF_SPIN_TIME = 0.5;
 
-    const float SCOUTSIM_REFRESH_RATE = 0.01;   // s
+    const float SCOUTSIM_REFRESH_RATE = 0.05;   // s
 }
 
 #endif
diff --git a/scout/scoutsim/src/sim_frame.cpp b/scout/scoutsim/src/sim_frame.cpp
index 7420e40..c4a8a3d 100644
--- a/scout/scoutsim/src/sim_frame.cpp
+++ b/scout/scoutsim/src/sim_frame.cpp
@@ -314,6 +314,7 @@ namespace scoutsim
     // Runs every SCOUTSIM_REFRESH_RATE.
     void SimFrame::onUpdate(wxTimerEvent& evt)
     {
+        cout << frame_count << endl;
         ros::spinOnce();
 
         teleop();
@@ -324,6 +325,8 @@ namespace scoutsim
         {
             Close();
         }
+
+        frame_count++;
     }
 
     void SimFrame::onPaint(wxPaintEvent& evt)
@@ -391,13 +394,13 @@ namespace scoutsim
         }
         else if (wxGetKeyState(WXK_LEFT))
         {
-            teleop_l_speed = -TELEOP_PRECISE_SPEED;
-            teleop_r_speed = TELEOP_PRECISE_SPEED;
+            teleop_l_speed = -TELEOP_PRECISE_SPEED * 2;
+            teleop_r_speed = TELEOP_PRECISE_SPEED * 2;
         }
         else if (wxGetKeyState(WXK_RIGHT))
         {
-            teleop_l_speed = TELEOP_PRECISE_SPEED;
-            teleop_r_speed = -TELEOP_PRECISE_SPEED;
+            teleop_l_speed = TELEOP_PRECISE_SPEED * 2;
+            teleop_r_speed = -TELEOP_PRECISE_SPEED * 2;
         }
     }
 
@@ -411,14 +414,18 @@ namespace scoutsim
         {
             teleop_fluid_speed -= TELEOP_FLUID_INC * 2;
         }
-        else if (teleop_fluid_speed > 0)
+        else if (teleop_fluid_speed > TELEOP_FLUID_INC)
         {
             teleop_fluid_speed -= TELEOP_FLUID_INC;
         }
-        else if (teleop_fluid_speed < 0)
+        else if (teleop_fluid_speed < -TELEOP_FLUID_INC)
         {
             teleop_fluid_speed += TELEOP_FLUID_INC;
         }
+        else
+        {
+            teleop_fluid_speed = 0;
+        }
 
         if (wxGetKeyState(WXK_LEFT))
         {
@@ -441,17 +448,22 @@ namespace scoutsim
         {
             teleop_fluid_speed = -TELEOP_FLUID_MAX_SPEED;
         }
-        if (teleop_fluid_omega > TELEOP_FLUID_MAX_SPEED / 2)
+        if (teleop_fluid_omega > TELEOP_FLUID_MAX_SPEED)
         {
-            teleop_fluid_omega = TELEOP_FLUID_MAX_SPEED / 2;
+            teleop_fluid_omega = TELEOP_FLUID_MAX_SPEED;
         }
-        else if (teleop_fluid_omega < -TELEOP_FLUID_MAX_SPEED / 2)
+        else if (teleop_fluid_omega < -TELEOP_FLUID_MAX_SPEED)
         {
-            teleop_fluid_omega = -TELEOP_FLUID_MAX_SPEED / 2;
+            teleop_fluid_omega = -TELEOP_FLUID_MAX_SPEED;
         }
 
-        teleop_l_speed = teleop_fluid_speed + teleop_fluid_omega;
-        teleop_r_speed = teleop_fluid_speed - teleop_fluid_omega;
+        int l_speed = teleop_fluid_speed + teleop_fluid_omega;
+        int r_speed = teleop_fluid_speed - teleop_fluid_omega;
+
+        teleop_l_speed = max(MIN_ABSOLUTE_SPEED,
+                             min(MAX_ABSOLUTE_SPEED, l_speed));
+        teleop_r_speed = max(MIN_ABSOLUTE_SPEED,
+                             min(MAX_ABSOLUTE_SPEED, r_speed));
     }
 
     void SimFrame::teleop()
@@ -490,11 +502,8 @@ namespace scoutsim
             return;
         }
 
-        if (frame_count % 3 == 0)
-        {
-            path_image = path_bitmap.ConvertToImage();
-            Refresh();
-        }
+        path_image = path_bitmap.ConvertToImage();
+        Refresh();
 
         M_Scout::iterator it = scouts.begin();
         M_Scout::iterator end = scouts.end();
@@ -505,14 +514,13 @@ namespace scoutsim
 
         for (; it != end; ++it)
         {
-            it->second->update(0.016, path_dc,sonar_dc,sonar_visual_on,
+            it->second->update(SCOUTSIM_REFRESH_RATE,
+                               path_dc,sonar_dc,sonar_visual_on,
                                path_image, lines_image, walls_image,
                                path_dc.GetBackground().GetColour(),
-			       sonar_dc.GetBackground().GetColour(),
+                               sonar_dc.GetBackground().GetColour(),
                                state);
         }
-
-        frame_count++;
     }
 
     bool SimFrame::clearCallback(std_srvs::Empty::Request&,
diff --git a/scout/scoutsim/src/sim_frame.h b/scout/scoutsim/src/sim_frame.h
index ccea5ea..e3fb984 100644
--- a/scout/scoutsim/src/sim_frame.h
+++ b/scout/scoutsim/src/sim_frame.h
@@ -77,10 +77,10 @@
 #define ID_TELEOP_FLUID 9
 #define ID_SONAR 10
 
-// Absolute speeds (0 - 255)
-#define TELEOP_PRECISE_SPEED 160
-#define TELEOP_FLUID_MAX_SPEED 200
-#define TELEOP_FLUID_INC 10
+// Absolute speeds (-128 - 127)
+#define TELEOP_PRECISE_SPEED 62
+#define TELEOP_FLUID_MAX_SPEED 100
+#define TELEOP_FLUID_INC 8
 
 // Teleop types
 #define TELEOP_OFF 0
@@ -112,6 +112,7 @@ namespace scoutsim
             DECLARE_EVENT_TABLE()
 
         private:
+
             void onUpdate(wxTimerEvent& evt);
             void onPaint(wxPaintEvent& evt);
 
@@ -136,8 +137,8 @@ namespace scoutsim
             ros::Publisher wireless_receive;
 
             // Teleop
-            int teleop_l_speed;
-            int teleop_r_speed;
+            short teleop_l_speed;
+            short teleop_r_speed;
             ros::Publisher teleop_pub;
             int teleop_type;
 
