Colony » Colony Scout

#include "Odometry.h"

using namespace std;

/** Set up the odometry node and prepare communcations over ROS */

Odometry::Odometry(string scoutname, Sensors* sensors):Behavior(scoutname, 

                                                "odometry", sensors)

{

  scout_pos = new pos;

  motor_fl_ticks = motor_fr_ticks = motor_bl_ticks = motor_br_ticks = 0;

}

/** Query encoders and estimate position based on encoder reading */

void Odometry::get_position()

{

  float left_dist, right_dist, total_dist, theta;

  encoder_readings scout_enc = encoders->query();

  motor_fl_dist = scout_enc.fl_ticks - motor_fl_ticks;

  motor_fr_dist = scout_enc.fr_ticks - motor_fr_ticks;

  motor_bl_dist = scout_enc.bl_ticks - motor_bl_ticks;

  motor_br_dist = scout_enc.br_ticks - motor_br_ticks;

  ROS_INFO("fl %d  fr  %d  bl  %d  br  %d", scout_enc.fl_ticks, scout_enc.fr_ticks, scout_enc.bl_ticks, scout_enc.br_ticks);

  ROS_INFO("fl %d  fr  %d  bl  %d  br  %d", motor_fl_ticks, motor_fr_ticks, motor_bl_ticks, motor_br_ticks);

  ROS_INFO("fl %d  fr  %d  bl  %d  br  %d", motor_fl_dist, motor_fr_dist, motor_bl_dist, motor_br_dist);

  // Get Left and Right distance

  left_dist = DIST_PER_TICK*(((float)(motor_fl_dist))/2.0 +

                             ((float)(motor_bl_dist))/2.0);

  right_dist = DIST_PER_TICK*(((float)(motor_fr_dist))/2.0 + 

                              ((float)(motor_br_dist))/2.0);

  total_dist = (left_dist)/2.0 + (right_dist)/2.0;

  theta = scout_pos->theta + atan2(left_dist - right_dist, WHEEL_BASE);

  ROS_INFO("l: %f   r: %f   theta: %f", left_dist, right_dist, theta);

  scout_pos->x += total_dist*sin(theta);

  scout_pos->y += total_dist*cos(theta);

  scout_pos->theta = fmod(theta, 2*M_PI);

  //Save state for next time in.

  motor_fl_ticks = scout_enc.fl_ticks;

  motor_fr_ticks = scout_enc.fr_ticks;

  motor_bl_ticks = scout_enc.bl_ticks;

  motor_br_ticks = scout_enc.br_ticks;

 return;

}

void Odometry::run()

{

  while(ok())

  {

    get_position();

    //TODO publish position

    ROS_INFO("Scout is at x: %f  y: %f theta: %f\n", scout_pos->x, scout_pos->y, scout_pos->theta);

  }

}