commit c63c97520e4ba4810364a2b18151db6f250300b8
Author: Alex <azirbel@andrew.cmu.edu>
Date:   Mon Nov 12 21:40:22 2012 -0500

    Scoutsim now has consistent units of m and m/s.
    
    This will cause some problems.
    
     * Behaviors which relied on the old units will have to change. We need to change the Control classes to scale things right.
     * Encoder ticks are almost fixed, but not quite right (I think because of a rounding problem).
     * Speeds are now all in m/s, which is much different than before - we'll have to change the Control class somehow.
    
    I'm afraid I'm out of time to make the rest of these changes now, but I'll do them ASAP.

diff --git a/scout/scoutsim/src/scout.cpp b/scout/scoutsim/src/scout.cpp
index 018ecec..8d6e01a 100644
--- a/scout/scoutsim/src/scout.cpp
+++ b/scout/scoutsim/src/scout.cpp
@@ -105,8 +105,6 @@ namespace scoutsim
             linesensor_readings.push_back(0);
         }
 
-        meter = scout.GetHeight();
-
         // Initialize sonar
         sonar_position = 0;
         sonar_stop_l = 0;
@@ -140,7 +138,6 @@ namespace scoutsim
         {
             motor_br_speed = msg->br_speed;
         }
-
     }
 
     bool Scout::setPenCallback(scoutsim::SetPen::Request& req,
@@ -194,14 +191,16 @@ namespace scoutsim
     }
 
     unsigned int Scout::trace_sonar(const wxImage& walls_image, int x, int y,
-                                    double robot_theta, int sonar_pos, wxMemoryDC& sonar_dc, bool sonar_on)
+                                    double robot_theta, int sonar_pos,
+                                    wxMemoryDC& sonar_dc, bool sonar_on)
     {
         double angle = robot_theta + (PI * ((float) sonar_pos) / 24.0) - PI / 2;
         unsigned int d = 0;
 
         unsigned int reading = 0;
-	int d_x = 0;
-	int d_y = 0;
+        int d_x = 0;
+        int d_y = 0;
+
         do
         {
             d_x = x + (int) floor(d * cos(angle));
@@ -233,39 +232,39 @@ namespace scoutsim
         /// @todo Consider using different cutoffs for different features
         while (reading < 128); /// @todo Get rid of hardcoded stuff like this
         
-	if(sonar_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_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;
     }
 
@@ -333,13 +332,13 @@ namespace scoutsim
     /// Sends back the position of this scout so scoutsim can save
     /// the world state
     geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc,
-					wxMemoryDC& sonar_dc,
-					bool sonar_on,
+                                        wxMemoryDC& sonar_dc,
+                                        bool sonar_on,
                                         const wxImage& path_image,
                                         const wxImage& lines_image,
                                         const wxImage& walls_image,
                                         wxColour background_color,
-					wxColour sonar_color,
+                                        wxColour sonar_color,
                                         world_state state)
     {
         // Assume that the two motors on the same side will be set to
@@ -348,35 +347,40 @@ namespace scoutsim
         float l_speed = (float (motor_fl_speed + motor_bl_speed)) / 2;
         float r_speed = (float (motor_fr_speed + motor_br_speed)) / 2;
 
-        // Set the linear and angular speeds
-        float lin_vel = SPEED_SCALE_FACTOR * (l_speed + r_speed) / 2;
-        float ang_vel = SPEED_SCALE_FACTOR * (r_speed - l_speed);
+        // Find linear and angular movement in m
+        float lin_dist = SCOUTSIM_REFRESH_RATE * (l_speed + r_speed) / 2;
+        float ang_dist = SCOUTSIM_REFRESH_RATE * (r_speed - l_speed);
 
         Vector2 old_pos = pos;
 
         // Update encoders
-        /// @todo replace
-        fl_ticks += (unsigned int) motor_fl_speed;
-        fr_ticks += (unsigned int) motor_fr_speed;
-        bl_ticks += (unsigned int) motor_bl_speed;
-        br_ticks += (unsigned int) motor_br_speed;
+        fl_ticks += (unsigned int) (motor_fl_speed * SCOUTSIM_REFRESH_RATE *
+                                    ENCODER_TICKS_PER_METER);
+        fr_ticks += (unsigned int) (motor_fr_speed * SCOUTSIM_REFRESH_RATE *
+                                    ENCODER_TICKS_PER_METER);
+        bl_ticks += (unsigned int) (motor_bl_speed * SCOUTSIM_REFRESH_RATE *
+                                    ENCODER_TICKS_PER_METER);
+        br_ticks += (unsigned int) (motor_br_speed * SCOUTSIM_REFRESH_RATE *
+                                    ENCODER_TICKS_PER_METER);
+
+        orient = fmod((float) (orient + ang_dist), (float) (2.0 * PI));
 
-        orient = fmod(orient + ang_vel * dt, 2*PI);
-        pos.x += sin(orient + PI/2.0) * lin_vel * dt;
-        pos.y += cos(orient + PI/2.0) * lin_vel * dt;
+        // 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;
 
         // Clamp to screen size
         if (pos.x < 0 || pos.x >= state.canvas_width
                 || pos.y < 0 || pos.y >= state.canvas_height)
         {
-            ROS_WARN("Oh no! I hit the wall! (Clamping from [x=%f, y=%f])", pos.x, pos.y);
+            ROS_WARN("I hit the wall! (Clamping from [x=%f, y=%f])", pos.x, pos.y);
         }
 
         pos.x = min(max(pos.x, 0.0f), state.canvas_width);
         pos.y = min(max(pos.y, 0.0f), state.canvas_height);
 
-        int canvas_x = pos.x * meter;
-        int canvas_y = pos.y * meter;
+        int canvas_x = pos.x * PIX_PER_METER;
+        int canvas_y = pos.y * PIX_PER_METER;
 
         {
             wxImage rotated_image =
@@ -405,8 +409,8 @@ namespace scoutsim
         p.x = pos.x;
         p.y = state.canvas_height - pos.y;
         p.theta = orient;
-        p.linear_velocity = lin_vel;
-        p.angular_velocity = ang_vel;
+        p.linear_velocity = l_speed;
+        p.angular_velocity = r_speed;
         pose_pub.publish(p);
 
         update_linesensor(lines_image, canvas_x, canvas_y, p.theta);
@@ -434,8 +438,8 @@ namespace scoutsim
             {
                 path_dc.SelectObject(*path_bitmap);
                 path_dc.SetPen(pen);
-                path_dc.DrawLine(pos.x * meter, pos.y * meter,
-                                 old_pos.x * meter, old_pos.y * meter);
+                path_dc.DrawLine(pos.x * PIX_PER_METER, pos.y * PIX_PER_METER,
+                                 old_pos.x * PIX_PER_METER, old_pos.y * PIX_PER_METER);
             }
         }
 
@@ -450,8 +454,7 @@ namespace scoutsim
     void Scout::paint(wxDC& dc)
     {
         wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
-        dc.DrawBitmap(scout, pos.x * meter - (scout_size.GetWidth() / 2),
-                      pos.y * meter - (scout_size.GetHeight() / 2), true);
+        dc.DrawBitmap(scout, pos.x * PIX_PER_METER - (scout_size.GetWidth() / 2),
+                      pos.y * PIX_PER_METER - (scout_size.GetHeight() / 2), true);
     }
-
 }
diff --git a/scout/scoutsim/src/scout.h b/scout/scoutsim/src/scout.h
index 7e462ce..d643024 100644
--- a/scout/scoutsim/src/scout.h
+++ b/scout/scoutsim/src/scout.h
@@ -70,9 +70,6 @@
 
 #define PI 3.14159265
 
-/// The scale factor so the speed of scout robots is reasonable for the sim
-#define SPEED_SCALE_FACTOR 0.05
-
 #define NUM_LINESENSORS 8
 
 // Distance, pixels, from center of robot to the linesensors.
@@ -135,19 +132,20 @@ namespace scoutsim
                                      unsigned char g,
                                      unsigned char b);
             unsigned int trace_sonar(const wxImage& walls_image, int x, int y,
-                                     double robot_theta, int sonar_pos,wxMemoryDC& sonar_dc, bool sonar_on);
+                                     double robot_theta, int sonar_pos,
+                                     wxMemoryDC& sonar_dc, bool sonar_on);
             void update_sonar(const wxImage& walls_image, int x, int y,
                               double robot_theta, wxMemoryDC& sonar_dc,bool sonar_on);
             void update_linesensor(const wxImage& lines_image, int x, int y,
                                    double theta);
-	    int old_front_dx;
-	    int old_front_dy;
-	    int old_back_dx;
-	    int old_back_dy;
-	    bool isFront;
+            int old_front_dx;
+            int old_front_dy;
+            int old_back_dx;
+            int old_back_dy;
+            bool isFront;
 
-	    wxBitmap *path_bitmap;
-	    bool sonar_on;
+            wxBitmap *path_bitmap;
+            bool sonar_on;
 
             ros::NodeHandle node;
 
@@ -175,6 +173,7 @@ namespace scoutsim
             int sonar_stop_l;
             int sonar_stop_r;
             int sonar_direction;
+
             // The last time the sonar changed its position.
             ros::Time last_sonar_time;
             ros::Duration sonar_tick_time;
@@ -197,12 +196,8 @@ namespace scoutsim
             ros::ServiceServer query_linesensor_srv;
 
             ros::WallTime last_command_time;
-
-            float meter;
-
     };
     typedef boost::shared_ptr<Scout> ScoutPtr;
-
 }
 
 #endif
diff --git a/scout/scoutsim/src/scout_constants.h b/scout/scoutsim/src/scout_constants.h
index 72d7606..e5504f6 100644
--- a/scout/scoutsim/src/scout_constants.h
+++ b/scout/scoutsim/src/scout_constants.h
@@ -49,6 +49,10 @@
 
 namespace scoutsim
 {
+    // Pixels in scoutsim per real-world meter.
+    // Note that scout image size has been set based on this.
+    const float PIX_PER_METER = 200.0;
+
     // Scout dimensions, in meters
     const float SCOUT_WIDTH = 0.05;
     const float SCOUT_LENGTH = 0.2;
@@ -57,7 +61,13 @@ namespace scoutsim
     const float SCOUT_SONAR_X = -0.01;
     const float SCOUT_SONAR_Y = 0.0;
 
+    // @todo Inaccurate. Update
+    const float ENCODER_TICKS_PER_METER = 363.78;
+
     const unsigned int SONAR_MAX_RANGE = 255;
+
     // 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
 }
diff --git a/scout/scoutsim/src/sim_frame.cpp b/scout/scoutsim/src/sim_frame.cpp
index 88d6103..6028537 100644
--- a/scout/scoutsim/src/sim_frame.cpp
+++ b/scout/scoutsim/src/sim_frame.cpp
@@ -68,7 +68,7 @@ namespace scoutsim
         srand(time(NULL));
 
         update_timer = new wxTimer(this);
-        update_timer->Start(16);
+        update_timer->Start(SCOUTSIM_REFRESH_RATE * 1000);
 
         Connect(update_timer->GetId(), wxEVT_TIMER,
                 wxTimerEventHandler(SimFrame::onUpdate), NULL, this);
@@ -77,9 +77,6 @@ namespace scoutsim
 
         images_path = ros::package::getPath("scoutsim") + "/images/";
 
-        // 200 pixels per meter. @todo make this a constant elsewhere.
-        meter = 200;
-
         map_base_name =  ros::package::getPath("scoutsim") + "/maps/" +
                            map_name + ".bmp";
         map_lines_name = ros::package::getPath("scoutsim") + "/maps/" +
@@ -160,8 +157,8 @@ namespace scoutsim
 
         SetMenuBar(menuBar);
 
-        width_in_meters = GetSize().GetWidth() / meter;
-        height_in_meters = GetSize().GetHeight() / meter;
+        width_in_meters = GetSize().GetWidth() / PIX_PER_METER;
+        height_in_meters = GetSize().GetHeight() / PIX_PER_METER;
     }
 
     SimFrame::~SimFrame()
@@ -314,6 +311,7 @@ namespace scoutsim
         SetSize(wxSize(path_bitmap.GetWidth(), path_bitmap.GetHeight()));
     }
 
+    // Runs every SCOUTSIM_REFRESH_RATE.
     void SimFrame::onUpdate(wxTimerEvent& evt)
     {
         ros::spinOnce();
@@ -393,13 +391,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;
         }
     }
 
@@ -407,28 +405,28 @@ namespace scoutsim
     {
         if (wxGetKeyState(WXK_UP))
         {
-            teleop_fluid_speed += 2;
+            teleop_fluid_speed += TELEOP_FLUID_INC * 2;
         }
         else if (wxGetKeyState(WXK_DOWN))
         {
-            teleop_fluid_speed -= 2;
+            teleop_fluid_speed -= TELEOP_FLUID_INC * 2;
         }
         else if (teleop_fluid_speed > 0)
         {
-            teleop_fluid_speed -= 1;
+            teleop_fluid_speed -= TELEOP_FLUID_INC;
         }
         else if (teleop_fluid_speed < 0)
         {
-            teleop_fluid_speed += 1;
+            teleop_fluid_speed += TELEOP_FLUID_INC;
         }
 
         if (wxGetKeyState(WXK_LEFT))
         {
-            teleop_fluid_omega -= 1;
+            teleop_fluid_omega -= TELEOP_FLUID_INC;
         }
         else if (wxGetKeyState(WXK_RIGHT))
         {
-            teleop_fluid_omega += 1;
+            teleop_fluid_omega += TELEOP_FLUID_INC;
         }
         else
         {
@@ -439,10 +437,18 @@ namespace scoutsim
         {
             teleop_fluid_speed = TELEOP_FLUID_MAX_SPEED;
         }
+        else if (teleop_fluid_speed < -TELEOP_FLUID_MAX_SPEED)
+        {
+            teleop_fluid_speed = -TELEOP_FLUID_MAX_SPEED;
+        }
         if (teleop_fluid_omega > TELEOP_FLUID_MAX_SPEED / 2)
         {
             teleop_fluid_omega = TELEOP_FLUID_MAX_SPEED / 2;
         }
+        else if (teleop_fluid_omega < -TELEOP_FLUID_MAX_SPEED / 2)
+        {
+            teleop_fluid_omega = -TELEOP_FLUID_MAX_SPEED / 2;
+        }
 
         teleop_l_speed = teleop_fluid_speed + teleop_fluid_omega;
         teleop_r_speed = teleop_fluid_speed - teleop_fluid_omega;
diff --git a/scout/scoutsim/src/sim_frame.h b/scout/scoutsim/src/sim_frame.h
index 8ddb02b..f1789a6 100644
--- a/scout/scoutsim/src/sim_frame.h
+++ b/scout/scoutsim/src/sim_frame.h
@@ -77,8 +77,9 @@
 #define ID_TELEOP_FLUID 9
 #define ID_SONAR 10
 
-#define TELEOP_PRECISE_SPEED 30
-#define TELEOP_FLUID_MAX_SPEED 80
+#define TELEOP_PRECISE_SPEED 1.0f
+#define TELEOP_FLUID_MAX_SPEED 1.5f
+#define TELEOP_FLUID_INC 0.1f
 
 // Teleop types
 #define TELEOP_OFF 0
@@ -134,13 +135,13 @@ namespace scoutsim
             ros::Publisher wireless_receive;
 
             // Teleop
-            short teleop_l_speed;
-            short teleop_r_speed;
+            float teleop_l_speed;
+            float teleop_r_speed;
             ros::Publisher teleop_pub;
             int teleop_type;
 
-            short teleop_fluid_speed;
-            short teleop_fluid_omega;
+            float teleop_fluid_speed;
+            float teleop_fluid_omega;
 
             ros::NodeHandle nh;
             wxTimer* update_timer;
@@ -166,7 +167,6 @@ namespace scoutsim
 
             std::string images_path;
 
-            float meter;
             float width_in_meters;
             float height_in_meters;
 
