commit 36701fcdf70230a47c0a3b1c60c190811451d20e
Author: Yuyang Guo <yuyang@yuyang-VirtualBox.(none)>
Date:   Mon Mar 4 21:12:49 2013 -0500

    added new Odometry in test behaviors, done some simple testing in scoutsim

diff --git a/scout/libscout/src/test_behaviors/Odometry_new.cpp b/scout/libscout/src/test_behaviors/Odometry_new.cpp
new file mode 100644
index 0000000..9c632cd
--- /dev/null
+++ b/scout/libscout/src/test_behaviors/Odometry_new.cpp
@@ -0,0 +1,105 @@
+
+#include "Odometry_new.h"
+
+using namespace std;
+
+/** Set up the odometry node and prepare communications over ROS */
+Odometry_new::Odometry_new(string scoutname, Sensors* sensors):
+                Behavior(scoutname, "odometry", sensors)
+{
+  name = scoutname;
+  scout_pos = new posi;
+  motor_fl_ticks_last = motor_fr_ticks_last = 0;
+  motor_bl_ticks_last = motor_br_ticks_last = 0;
+}
+
+
+/** Query encoders and integrate using RK4 to estimate position */
+void Odometry_new::get_position(double loop_time)
+{
+  float avg_left_ticks, avg_right_ticks;
+  float ang_vl, ang_vr, lin_vl, lin_vr;
+  float robot_v, robot_w;
+  float k00, k01, k02, k10, k11, k12, k20, k21, k22, k30, k31, k32;
+  // figure out the encoder ticks during the time
+  encoder_readings scout_enc = encoders->query();
+  motor_fl_ticks_iter = scout_enc.fl_ticks - motor_fl_ticks_last;
+  motor_fr_ticks_iter = scout_enc.fr_ticks - motor_fr_ticks_last;
+  motor_bl_ticks_iter = scout_enc.bl_ticks - motor_bl_ticks_last;
+  motor_br_ticks_iter = scout_enc.br_ticks - motor_br_ticks_last;
+  
+
+  avg_left_ticks = (float) motor_fl_ticks_iter;
+  avg_right_ticks = (float) motor_fr_ticks_iter;
+  // right now the back encoders does not work so uncomment the next
+  // two lines when the work!!!
+  /*
+  avg_left_ticks = motor_fl_ticks_iter/(2.0) + motor_bl_ticks_iter/(2.0);
+  avg_right_ticks = motor_fr_ticks_iter/(2.0) + motor_br_ticks_iter/(2.0);
+  */
+  
+  // figure out the angular velocity in radians per second
+  ang_vl = (avg_left_ticks/loop_time) * RAD_PER_TICK;
+  ang_vr = (avg_right_ticks/loop_time) * RAD_PER_TICK;
+
+  // figure out the linear velosity of robot
+  lin_vl = ang_vl * WHEEL_R;
+  lin_vr = ang_vr * WHEEL_R;
+  
+  // figure out how the robot speed and turning speed
+  robot_v = (lin_vl/2.0 + lin_vr/2.0);
+  robot_w = (lin_vr - lin_vl)/WHEEL_B;
+
+  // now use RK4 to integrade the velocities to get the the move in pos
+  k00 = robot_v * cos(scout_pos->theta);
+  k01 = robot_v * sin(scout_pos->theta);
+  k02 = robot_w;
+  
+  k10 = robot_v * cos(scout_pos->theta + LOOP_TIME/2*k02);
+  k11 = robot_v * sin(scout_pos->theta + LOOP_TIME/2*k02);
+  k12 = robot_w;
+
+  k20 = robot_v * cos(scout_pos->theta + LOOP_TIME/2*k12);
+  k21 = robot_v * sin(scout_pos->theta + LOOP_TIME/2*k12);
+  k22 = robot_w;
+
+  k30 = robot_v * cos(scout_pos->theta + LOOP_TIME * k22);
+  k31 = robot_v * sin(scout_pos->theta + LOOP_TIME * k22);
+  k32 = robot_w;
+
+  scout_pos->x += LOOP_TIME/6 * (k00+2*(k10+k20)+k30);
+  scout_pos->y += LOOP_TIME/6 * (k01+2*(k11+k21)+k31);
+  scout_pos->theta += LOOP_TIME/6 * (k02+2*(k12+k22)+k32);
+
+  // update the 
+  motor_fl_ticks_last = scout_enc.fl_ticks;
+  motor_fr_ticks_last = scout_enc.fr_ticks;
+  motor_bl_ticks_last = scout_enc.bl_ticks;
+  motor_br_ticks_last = scout_enc.br_ticks;
+
+}
+
+void Odometry_new::run()
+{
+  scout_position = node.advertise< ::messages::ScoutPosition>(name+"/posn", 1000);
+  double last_time = Time::now().toSec();
+  double loop_time, current_time;
+  motors->set_sides(50,50, MOTOR_ABSOLUTE);
+  while(ok())
+  {
+    //Rate r(LOOP_RATE); 
+    Duration r = Duration(LOOP_TIME);
+    current_time = Time::now().toSec();
+    loop_time = current_time - last_time;
+    last_time = current_time;
+    get_position(loop_time);
+    position.name = name;
+    position.x = scout_pos->x;
+    position.y = scout_pos->y;
+    position.theta = scout_pos->theta;
+    scout_position.publish(position);
+    ROS_INFO("scout is at %f %f theta: %f", position.x, position.y, position.theta);
+    r.sleep();
+  }
+
+}
diff --git a/scout/libscout/src/test_behaviors/Odometry_new.h b/scout/libscout/src/test_behaviors/Odometry_new.h
new file mode 100644
index 0000000..45d1f77
--- /dev/null
+++ b/scout/libscout/src/test_behaviors/Odometry_new.h
@@ -0,0 +1,63 @@
+// a smarter Odometry the use RK4 integration
+
+#ifndef _ODOMETRY_NEW_H_
+#define _ODOMETRY_NEW_H_
+
+#include "../Behavior.h"
+#include "../Sensors.h"
+#include "messages/ScoutPosition.h"
+
+#define WHEEL_R 1 //this is a virtual unit now... use real values
+#define WHEEL_B 1  // this is a virtual unit now... use real values
+#define TICKS_PER_REV 48
+#define PI 3.1415926
+#define RAD_PER_TICK ((TICKS_PER_REV)/360.0 * PI / 180.0)
+#define LOOP_RATE 10 // Hz, integrate 10 times per second
+#define LOOP_TIME (1.0/LOOP_RATE) // secs
+
+typedef struct{
+  float x;
+  float y;
+  float theta;
+} posi;
+
+class Odometry_new : Behavior{
+
+  public:
+
+    /** Set up the odometry node and prepare communications over ROS */
+    Odometry_new(std::string scoutname, Sensors* sensors);
+
+    /** Query encoders and estimate position based on encoder reading */
+    void get_position(double loop_time);
+
+    /** Gets scout position and prints to screen */
+    void run();
+    
+  private:
+
+    /** ROS publisher and client declaration */
+    ros::NodeHandle node;
+    ros::Publisher scout_position;
+    ros::ServiceClient query_encoders_client;
+    messages::ScoutPosition position;
+
+    std::string name;
+
+    // FIXME: not sure whether these would overflow....
+    int motor_fl_ticks_iter;
+    int motor_fr_ticks_iter;
+    int motor_bl_ticks_iter;
+    int motor_br_ticks_iter;
+    
+    int motor_fl_ticks_last;
+    int motor_fr_ticks_last;
+    int motor_bl_ticks_last;
+    int motor_br_ticks_last;
+    int scout_theta;
+
+    posi* scout_pos;
+
+};
+
+#endif
