Colony » Colony Scout » Colony Scout OS

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

}

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

void Odometry::get_position()

{

  float left_dist, right_dist, total_dist, theta;

//  float left_vel, right_vel;

  /* TODO: Get current encoder ticks */

  encoder_readings scout_enc = encoders->query();

  motor_fl_dist = scout_enc.fl_ticks;

  motor_fr_dist = scout_enc.fr_ticks;

  motor_bl_dist = scout_enc.bl_ticks;

  motor_br_dist = scout_enc.br_ticks;

/**

  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;

**/

  left_dist = DIST_PER_TICK*(motor_fl_dist + motor_bl_dist)/2;

  right_dist = DIST_PER_TICK*(motor_fr_dist + motor_br_dist)/2;

  total_dist = (left_dist+right_dist)/2;

  theta = (right_dist - left_dist)/WHEEL_BASE;

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

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

  scout_pos->theta = theta;

/**

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

  }

}