Revision 724
More work on odometry code. (Really want to use floats but am avoiding temptation for now.)
| trunk/code/projects/slam.bak2/odometry/odometry.c | ||
|---|---|---|
| 1 | 1 |
#include "encoders.h" |
| 2 | 2 |
|
| 3 |
//Odometry resolution, *64 microseconds. |
|
| 4 |
#define ODOMETRY_CLK=80 |
|
| 5 |
|
|
| 6 |
//Wheel = 2.613 in. |
|
| 7 |
//Circumference = 208.508133 mm |
|
| 8 |
//Distance per encoder click (circumference / 1024) = 203.621224 um. |
|
| 9 |
//Robot width = 5.3745 in. = 136.5123 mm |
|
| 10 |
|
|
| 11 |
#define ROBOT_WIDTH=137 //mm |
|
| 12 |
#define CLICK_DISTANCE=204 //um |
|
| 13 |
|
|
| 14 |
//Standard measures will be mm and us |
|
| 15 |
|
|
| 3 | 16 |
int encoder_ltm1, encoder_rtm1; |
| 4 | 17 |
|
| 5 | 18 |
int diff_x,diff_y,angle; |
| ... | ... | |
| 15 | 28 |
diff_y = 0; |
| 16 | 29 |
angle = 0; |
| 17 | 30 |
|
| 18 |
//TODO: Set up timer properly. |
|
| 19 |
//TCCR3A = _BV(COM3C1); |
|
| 20 |
//TCCR3B = _BV() |
|
| 31 |
//CTC Mode. CLK / 1024 |
|
| 32 |
TCCR3A = 0; // (Fully disconnected) |
|
| 33 |
TCCR3B = _BV(WGM32) | _BV(CS32) | _BV(CS30); //CLK/1024 , CTC Mode. |
|
| 34 |
|
|
| 35 |
OCR3A = ODOMETRY_CLK; |
|
| 21 | 36 |
} |
| 22 | 37 |
|
| 23 | 38 |
void odometry_run(){
|
| 24 | 39 |
int v_l, v_r, omega; |
| 25 | 40 |
int encoder_left, encoder_right; |
| 41 |
|
|
| 26 | 42 |
//TODO Handle the case where v_l = v_r |
| 27 | 43 |
encoder_left = encoder_read(LEFT); |
| 28 | 44 |
encoder_right = encoder_read(RIGHT); |
| 29 | 45 |
|
| 30 |
v_l = (encoder_left - encoder_ltm1); // / the timer interval |
|
| 31 |
v_r = (encoder_right - encoder_rtm1); // same |
|
| 46 |
v_l = (CLICK_DISTANCE*(encoder_left - encoder_ltm1)) / (ODOMETRY_CLK * 64); // um / us = m/s |
|
| 47 |
v_r = (CLICK_DISTANCE*(encoder_right - encoder_rtm1)) / (ODOMETRY_CLK * 64); // same |
|
| 48 |
|
|
| 49 |
if(v_l = v_r){
|
|
| 50 |
diff_x += v_l * cos(angle); |
|
| 51 |
diff_y += v_r * sin(angle); |
|
| 52 |
} |
|
| 32 | 53 |
|
| 33 | 54 |
//Relative to the center of the robot, the center of it's turn. |
| 34 |
turning_radius = ROBOT_WIDTH * ((v_l + v_r) / (v_l - v_r)); |
|
| 35 |
|
|
| 55 |
turning_radius = ROBOT_WIDTH * ((v_l + v_r) / (v_l - v_r)); // = mm |
|
| 56 |
|
|
| 57 |
//Here non floating point operations get trickier. |
|
| 36 | 58 |
if(v_r > v_l) //CCW positive. |
| 37 | 59 |
omega = -v_r / (turning_radius + ROBOT_WIDTH/2); |
| 38 | 60 |
|
| ... | ... | |
| 43 | 65 |
vmag = -omega * turning_radius; |
| 44 | 66 |
|
| 45 | 67 |
angle += omega; // * the timer interval. |
| 46 |
diff_x += vmag * cos(omega /* * timer interval.*/);
|
|
| 47 |
diff_y += vmag * sin(omega /* * timer interval.*/);
|
|
| 68 |
diff_x += vmag * cos(angle);
|
|
| 69 |
diff_y += vmag * sin(angle);
|
|
| 48 | 70 |
|
| 49 | 71 |
|
| 50 | 72 |
} |
Also available in: Unified diff