Revision 31be19a6
ID | 31be19a6f1dbf2140fc23d0e97e1a5a706cb78b8 |
Behaviours now have names!
scout/libscout/src/behaviors/Odometry.cpp | ||
---|---|---|
4 | 4 |
using namespace std; |
5 | 5 |
|
6 | 6 |
/** Set up the odometry node and prepare communcations over ROS */ |
7 |
Odometry::Odometry(string scoutname):Behavior(scoutname) |
|
7 |
Odometry::Odometry(string scoutname):Behavior(scoutname, "odometry")
|
|
8 | 8 |
{ |
9 | 9 |
scout_pos = new pos; |
10 | 10 |
} |
... | ... | |
17 | 17 |
|
18 | 18 |
/* TODO: Get current encoder ticks */ |
19 | 19 |
encoder_readings scout_enc = encoders->query(); |
20 |
motor_fl_dist = scout_enc.fl_ticks; |
|
21 |
motor_fr_dist = scout_enc.fr_ticks; |
|
22 |
motor_bl_dist = scout_enc.bl_ticks; |
|
23 |
motor_br_dist = scout_enc.br_ticks; |
|
24 |
|
|
25 |
// msg_time_in = ros::WallTime::now(); |
|
20 |
motor_fl_dist = scout_enc.fl_ticks; |
|
21 |
motor_fr_dist = scout_enc.fr_ticks; |
|
22 |
motor_bl_dist = scout_enc.bl_ticks; |
|
23 |
motor_br_dist = scout_enc.br_ticks; |
|
24 |
/** |
|
25 |
motor_fl_dist = scout_enc.fl_ticks - motor_fl_ticks; |
|
26 |
motor_fr_dist = scout_enc.fr_ticks - motor_fr_ticks; |
|
27 |
motor_bl_dist = scout_enc.bl_ticks - motor_bl_ticks; |
|
28 |
motor_br_dist = scout_enc.br_ticks - motor_br_ticks; |
|
29 |
**/ |
|
26 | 30 |
|
27 | 31 |
left_dist = DIST_PER_TICK*(motor_fl_dist + motor_bl_dist)/2; |
28 | 32 |
right_dist = DIST_PER_TICK*(motor_fr_dist + motor_br_dist)/2; |
29 | 33 |
|
30 |
// left_vel = left_dist/msg_time_iin; |
|
31 |
// right_vel = right_dist/msg_time_in; |
|
32 |
|
|
33 | 34 |
total_dist = (left_dist+right_dist)/2; |
34 | 35 |
theta = (right_dist - left_dist)/WHEEL_BASE; |
35 | 36 |
|
36 | 37 |
scout_pos->x = total_dist*sin(theta); |
37 | 38 |
scout_pos->y = total_dist*cos(theta); |
38 |
|
|
39 |
/* TODO: Publish x and y pos */ |
|
39 |
scout_pos->theta = theta; |
|
40 |
|
|
41 |
/** |
|
42 |
//Save state for next time in. |
|
43 |
motor_fl_ticks = scout_enc.fl_ticks; |
|
44 |
motor_fr_ticks = scout_enc.fr_ticks; |
|
45 |
motor_bl_ticks = scout_enc.bl_ticks; |
|
46 |
motor_br_ticks = scout_enc.br_ticks; |
|
47 |
**/ |
|
40 | 48 |
return; |
41 | 49 |
} |
42 | 50 |
|
... | ... | |
45 | 53 |
while(ok()) |
46 | 54 |
{ |
47 | 55 |
get_position(); |
48 |
ROS_INFO("Scout is at x: %f y: %f \n", scout_pos->x, scout_pos->y); |
|
56 |
//TODO publish position |
|
57 |
ROS_INFO("Scout is at x: %f y: %f theta: %f\n", scout_pos->x, scout_pos->y, scout_pos->theta); |
|
49 | 58 |
} |
50 | 59 |
} |
Also available in: Unified diff