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):Behavior(scoutname)

{

  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;

//  msg_time_in = ros::WallTime::now();

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

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

//  left_vel = left_dist/msg_time_iin;

//  right_vel = right_dist/msg_time_in;

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

 /* TODO: Publish x and y pos */

 return;

}

void Odometry::run()

{

  while(ok())

  {

    get_position();

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

  }

}