Project

General

Profile

Revision beaf9201

IDbeaf92012a2c675cf35a708fcb2da88749db9b89
Parent 97c5c430
Child 492d2fde

Added by Colony Scout about 11 years ago

Fixes to the Odometery behavior.

View differences:

scout/libscout/src/test_behaviors/Odometry_new.cpp
9 9
{
10 10
  name = scoutname;
11 11
  scout_pos = new posi;
12
  motor_fl_ticks_last = motor_fr_ticks_last = 0;
13
  motor_bl_ticks_last = motor_br_ticks_last = 0;
12

  
13
  encoder_readings scout_enc = encoders->query();
14
  motor_fl_ticks_last = motor_bl_ticks_last = scout_enc.fl_ticks;
15
  motor_fr_ticks_last = motor_br_ticks_last = scout_enc.fr_ticks;
14 16
}
15 17

  
16 18

  
......
55 57
  k01 = robot_v * sin(scout_pos->theta);
56 58
  k02 = robot_w;
57 59
  
58
  k10 = robot_v * cos(scout_pos->theta + LOOP_TIME/2*k02);
59
  k11 = robot_v * sin(scout_pos->theta + LOOP_TIME/2*k02);
60
  k10 = robot_v * cos(scout_pos->theta + loop_time/2*k02);
61
  k11 = robot_v * sin(scout_pos->theta + loop_time/2*k02);
60 62
  k12 = robot_w;
61 63

  
62
  k20 = robot_v * cos(scout_pos->theta + LOOP_TIME/2*k12);
63
  k21 = robot_v * sin(scout_pos->theta + LOOP_TIME/2*k12);
64
  k20 = robot_v * cos(scout_pos->theta + loop_time/2*k12);
65
  k21 = robot_v * sin(scout_pos->theta + loop_time/2*k12);
64 66
  k22 = robot_w;
65 67

  
66
  k30 = robot_v * cos(scout_pos->theta + LOOP_TIME * k22);
67
  k31 = robot_v * sin(scout_pos->theta + LOOP_TIME * k22);
68
  k30 = robot_v * cos(scout_pos->theta + loop_time * k22);
69
  k31 = robot_v * sin(scout_pos->theta + loop_time * k22);
68 70
  k32 = robot_w;
69 71

  
70
  scout_pos->x += LOOP_TIME/6 * (k00+2*(k10+k20)+k30);
71
  scout_pos->y += LOOP_TIME/6 * (k01+2*(k11+k21)+k31);
72
  scout_pos->theta += LOOP_TIME/6 * (k02+2*(k12+k22)+k32);
72
  scout_pos->x += loop_time/6 * (k00+2*(k10+k20)+k30);
73
  scout_pos->y += loop_time/6 * (k01+2*(k11+k21)+k31);
74
  scout_pos->theta += loop_time/6 * (k02+2*(k12+k22)+k32);
73 75

  
74 76
  // update the 
75 77
  motor_fl_ticks_last = scout_enc.fl_ticks;
......
84 86
  scout_position = node.advertise< ::messages::ScoutPosition>(name+"/posn", 1000);
85 87
  double last_time = Time::now().toSec();
86 88
  double loop_time, current_time;
89

  
90
  //Rate r(LOOP_RATE); 
91
  Duration r = Duration(LOOP_TIME);
92

  
87 93
  motors->set_sides(50,50, MOTOR_ABSOLUTE);
94

  
88 95
  while(ok())
89 96
  {
90
    //Rate r(LOOP_RATE); 
91
    Duration r = Duration(LOOP_TIME);
92 97
    current_time = Time::now().toSec();
93 98
    loop_time = current_time - last_time;
94 99
    last_time = current_time;
100

  
95 101
    get_position(loop_time);
102
    ROS_INFO("loop time %f", loop_time);
103

  
96 104
    position.name = name;
97 105
    position.x = scout_pos->x;
98 106
    position.y = scout_pos->y;
99 107
    position.theta = scout_pos->theta;
100 108
    scout_position.publish(position);
109

  
101 110
    ROS_INFO("scout is at %f %f theta: %f", position.x, position.y, position.theta);
111

  
102 112
    r.sleep();
103 113
  }
104

  
105 114
}
scout/libscout/src/test_behaviors/Odometry_new.h
7 7
#include "../Sensors.h"
8 8
#include "messages/ScoutPosition.h"
9 9

  
10
#define WHEEL_R 1 //this is a virtual unit now... use real values
11
#define WHEEL_B 1  // this is a virtual unit now... use real values
10
#define WHEEL_R 0.025 //this is a virtual unit now... use real values
11
#define WHEEL_B 0.105  // this is a virtual unit now... use real values
12 12
#define TICKS_PER_REV 48
13
#define PI 3.1415926
14
#define RAD_PER_TICK ((TICKS_PER_REV)/360.0 * PI / 180.0)
13
#define RAD_PER_TICK (2*M_PI/TICKS_PER_REV)
15 14
#define LOOP_RATE 10 // Hz, integrate 10 times per second
16 15
#define LOOP_TIME (1.0/LOOP_RATE) // secs
17 16

  

Also available in: Unified diff