Revision 3db79f25
ID | 3db79f259e8820845822e6176be5c3b81a405021 |
Fixed Odometry behavior so that it now works and then added a get readings functionality to sonarcontrol.
scout/libscout/src/SonarControl.cpp | ||
---|---|---|
189 | 189 |
num_attempts = MAX_ATTEMPTS; |
190 | 190 |
} |
191 | 191 |
|
192 |
int* SonarControl::get_sonar_readings() |
|
193 |
{ |
|
194 |
return readings; |
|
195 |
} |
|
196 |
|
|
192 | 197 |
/** |
193 | 198 |
* @brief Converts value returne by sonar to physical distances. |
194 | 199 |
* |
scout/libscout/src/SonarControl.h | ||
---|---|---|
63 | 63 |
/** Stops sonar from panning and taking readings */ |
64 | 64 |
void set_off(); |
65 | 65 |
|
66 |
/** Get sonar readings */ |
|
67 |
int* get_sonar_readings(); |
|
68 |
|
|
66 | 69 |
private: |
67 | 70 |
/** Record the new sonar distance measurement */ |
68 | 71 |
void distance_callback(const sonar::sonar_distance::ConstPtr& msg); |
scout/libscout/src/behaviors/Odometry.cpp | ||
---|---|---|
7 | 7 |
Odometry::Odometry(string scoutname):Behavior(scoutname, "odometry") |
8 | 8 |
{ |
9 | 9 |
scout_pos = new pos; |
10 |
motor_fl_ticks = motor_fr_ticks = motor_bl_ticks = motor_br_ticks = 0; |
|
10 | 11 |
} |
11 | 12 |
|
12 | 13 |
/** Query encoders and estimate position based on encoder reading */ |
13 | 14 |
void Odometry::get_position() |
14 | 15 |
{ |
15 | 16 |
float left_dist, right_dist, total_dist, theta; |
16 |
// float left_vel, right_vel; |
|
17 | 17 |
|
18 |
/* TODO: Get current encoder ticks */ |
|
19 | 18 |
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 | 19 |
motor_fl_dist = scout_enc.fl_ticks - motor_fl_ticks; |
26 | 20 |
motor_fr_dist = scout_enc.fr_ticks - motor_fr_ticks; |
27 | 21 |
motor_bl_dist = scout_enc.bl_ticks - motor_bl_ticks; |
28 | 22 |
motor_br_dist = scout_enc.br_ticks - motor_br_ticks; |
29 |
**/ |
|
30 | 23 |
|
31 |
left_dist = DIST_PER_TICK*(motor_fl_dist + motor_bl_dist)/2; |
|
32 |
right_dist = DIST_PER_TICK*(motor_fr_dist + motor_br_dist)/2; |
|
24 |
ROS_INFO("fl %d fr %d bl %d br %d", scout_enc.fl_ticks, scout_enc.fr_ticks, scout_enc.bl_ticks, scout_enc.br_ticks); |
|
25 |
ROS_INFO("fl %d fr %d bl %d br %d", motor_fl_ticks, motor_fr_ticks, motor_bl_ticks, motor_br_ticks); |
|
26 |
ROS_INFO("fl %d fr %d bl %d br %d", motor_fl_dist, motor_fr_dist, motor_bl_dist, motor_br_dist); |
|
33 | 27 |
|
34 |
total_dist = (left_dist+right_dist)/2; |
|
35 |
theta = (right_dist - left_dist)/WHEEL_BASE; |
|
28 |
// Get Left and Right distance |
|
29 |
left_dist = DIST_PER_TICK*(((float)(motor_fl_dist))/2.0 + |
|
30 |
((float)(motor_bl_dist))/2.0); |
|
31 |
right_dist = DIST_PER_TICK*(((float)(motor_fr_dist))/2.0 + |
|
32 |
((float)(motor_br_dist))/2.0); |
|
36 | 33 |
|
37 |
scout_pos->x = total_dist*sin(theta); |
|
38 |
scout_pos->y = total_dist*cos(theta); |
|
39 |
scout_pos->theta = theta; |
|
34 |
total_dist = (left_dist)/2.0 + (right_dist)/2.0; |
|
35 |
theta = scout_pos->theta + atan2(left_dist - right_dist, WHEEL_BASE); |
|
36 |
|
|
37 |
ROS_INFO("l: %f r: %f theta: %f", left_dist, right_dist, theta); |
|
38 |
|
|
39 |
scout_pos->x += total_dist*sin(theta); |
|
40 |
scout_pos->y += total_dist*cos(theta); |
|
41 |
scout_pos->theta = fmod(theta, 2*M_PI); |
|
40 | 42 |
|
41 |
/** |
|
42 | 43 |
//Save state for next time in. |
43 | 44 |
motor_fl_ticks = scout_enc.fl_ticks; |
44 | 45 |
motor_fr_ticks = scout_enc.fr_ticks; |
45 | 46 |
motor_bl_ticks = scout_enc.bl_ticks; |
46 | 47 |
motor_br_ticks = scout_enc.br_ticks; |
47 |
**/ |
|
48 |
|
|
49 |
|
|
48 | 50 |
return; |
49 | 51 |
} |
50 | 52 |
|
scout/libscout/src/behaviors/Odometry.h | ||
---|---|---|
4 | 4 |
#include "../Behavior.h" |
5 | 5 |
|
6 | 6 |
|
7 |
#define WHEEL_RADIUS 5
|
|
7 |
#define WHEEL_RADIUS 2
|
|
8 | 8 |
#define WHEEL_CIRCUM (2*M_PI*WHEEL_RADIUS) |
9 |
#define WHEEL_BASE 2
|
|
10 |
#define ENCODER_COUNT 8800000000
|
|
9 |
#define WHEEL_BASE 5
|
|
10 |
#define ENCODER_COUNT (4*1200*M_PI/5)
|
|
11 | 11 |
#define DIST_PER_TICK (WHEEL_CIRCUM/ENCODER_COUNT) |
12 | 12 |
|
13 | 13 |
typedef struct{ |
... | ... | |
38 | 38 |
|
39 | 39 |
float msg_time_in; |
40 | 40 |
|
41 |
float motor_fl_dist;
|
|
42 |
float motor_fr_dist;
|
|
43 |
float motor_bl_dist;
|
|
44 |
float motor_br_dist;
|
|
41 |
int motor_fl_dist;
|
|
42 |
int motor_fr_dist;
|
|
43 |
int motor_bl_dist;
|
|
44 |
int motor_br_dist;
|
|
45 | 45 |
|
46 |
float motor_fl_ticks;
|
|
47 |
float motor_fr_ticks;
|
|
48 |
float motor_bl_ticks;
|
|
49 |
float motor_br_ticks;
|
|
46 |
unsigned int motor_fl_ticks;
|
|
47 |
unsigned int motor_fr_ticks;
|
|
48 |
unsigned int motor_bl_ticks;
|
|
49 |
unsigned int motor_br_ticks;
|
|
50 | 50 |
float scout_theta; |
51 | 51 |
|
52 | 52 |
pos* scout_pos; |
Also available in: Unified diff