Colony » Colony Scout

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

  encoder_readings scout_enc = encoders->query();

  motor_fl_ticks_last = motor_bl_ticks_last = scout_enc.fl_ticks;

  motor_fr_ticks_last = motor_br_ticks_last = scout_enc.fr_ticks;

}

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

  //Rate r(LOOP_RATE); 

  Duration r = Duration(LOOP_TIME);

  motors->set_sides(50,50, MOTOR_ABSOLUTE);

  while(ok())

  {

    current_time = Time::now().toSec();

    loop_time = current_time - last_time;

    last_time = current_time;

    get_position(loop_time);

    ROS_INFO("loop time %f", 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();

  }

}