Revision 963
Minor modifications and fixes. Haven't gotten it to work yet.
| odometry.c | ||
|---|---|---|
| 1 | 1 |
#include "odometry.h" |
| 2 | 2 |
#include <encoders.h> |
| 3 |
#include <move.h> |
|
| 3 | 4 |
#include <math.h> |
| 4 | 5 |
#include <avr/interrupt.h> |
| 5 | 6 |
#include <dragonfly_lib.h> |
| 6 | 7 |
|
| 7 | 8 |
long lround(double d); |
| 8 | 9 |
|
| 10 |
char control_velocity = 0; |
|
| 11 |
int encoder_ltm1, encoder_rtm1, move_speed, move_angle; |
|
| 9 | 12 |
|
| 10 |
int encoder_ltm1, encoder_rtm1; |
|
| 11 |
|
|
| 12 | 13 |
//Measure mm displacement in x and y from starting position. (+x:theta=0 +y:theta=M_PI/2) |
| 13 |
long diff_x,diff_y, velocity; |
|
| 14 |
long diff_x,diff_y, velocity, target_velocity, target_angular_velocity;
|
|
| 14 | 15 |
|
| 15 | 16 |
//Measures radian angle displacement from starting position. (initially 0) |
| 16 | 17 |
double angle; |
| 17 | 18 |
|
| 18 | 19 |
/** |
| 19 |
* Retrieve the estimated x position. [millimeters]
|
|
| 20 |
* Retrieve the estimated x position. [mm]
|
|
| 20 | 21 |
*/ |
| 21 | 22 |
long odometry_dx(void){
|
| 22 | 23 |
return diff_x; |
| 23 | 24 |
} |
| 24 | 25 |
|
| 25 | 26 |
/** |
| 26 |
* Retrieve the estimated y position. [millimeters]
|
|
| 27 |
* Retrieve the estimated y position. [mm]
|
|
| 27 | 28 |
*/ |
| 28 | 29 |
long odometry_dy(void){
|
| 29 | 30 |
return diff_y; |
| ... | ... | |
| 35 | 36 |
double odometry_angle(void){
|
| 36 | 37 |
return angle; |
| 37 | 38 |
} |
| 38 |
|
|
| 39 |
/** |
|
| 40 |
* Retrieve the estimated velocity [mm / s] |
|
| 41 |
*/ |
|
| 39 | 42 |
long odometry_velocity(void){
|
| 40 | 43 |
return velocity; |
| 41 | 44 |
} |
| 42 | 45 |
|
| 46 |
void odometry_set_velocity(long t_velocity, double t_angular_velocity, int start_speed, int start_angle){
|
|
| 47 |
target_velocity = t_velocity; |
|
| 48 |
target_angular_velocity = t_angular_velocity; |
|
| 49 |
move_angle = start_angle; |
|
| 50 |
move_speed = start_speed; |
|
| 51 |
control_velocity = 1; |
|
| 52 |
} |
|
| 53 |
|
|
| 43 | 54 |
/** |
| 44 | 55 |
* Initializes odometry to run on timer 2. |
| 45 | 56 |
* Also resets all values so that the center of the robot is |
| ... | ... | |
| 90 | 101 |
dr = encoder_right - encoder_rtm1; |
| 91 | 102 |
|
| 92 | 103 |
//No motion. |
| 93 |
if(dl == 0 && dr == 0) return; |
|
| 104 |
if(dl == 0 && dr == 0){
|
|
| 105 |
if(control_velocity) modify_velocity(); |
|
| 106 |
return; |
|
| 107 |
} |
|
| 94 | 108 |
|
| 95 | 109 |
encoder_ltm1 = encoder_left; |
| 96 | 110 |
encoder_rtm1 = encoder_right; |
| ... | ... | |
| 111 | 125 |
diff_x += lround(dl*cos(angle)/1000.0); //mm |
| 112 | 126 |
diff_y += lround(dl*sin(angle)/1000.0); //mm |
| 113 | 127 |
velocity = lround((dl * 1000.0)/(ODOMETRY_CLK*TIME_SCALE)); //um / ms = mm/s |
| 114 |
sprintf(buf,"dc: %ld, velocity: %ld\r\n mm/s",dl,velocity); |
|
| 115 |
usb_puts(buf); |
|
| 128 |
if(control_velocity) modify_velocity(); |
|
| 116 | 129 |
return; |
| 117 | 130 |
} |
| 118 | 131 |
|
| ... | ... | |
| 123 | 136 |
//Distance the center has traveled. |
| 124 | 137 |
dc = (theta * (rl - ROBOT_WIDTH_UM)) / 2.0; //um |
| 125 | 138 |
velocity = lround( dc * 1000 /(ODOMETRY_CLK*TIME_SCALE)); |
| 126 |
sprintf(buf,"dc: %lfum, velocity: %ld\r\n mm/s",dc,velocity); |
|
| 127 |
usb_puts(buf); |
|
| 128 | 139 |
|
| 129 | 140 |
//angle is necessarily CCW+, so subtract. |
| 130 | 141 |
angle -= ANGLE_SCALE * theta; |
| ... | ... | |
| 132 | 143 |
//Change state variables. Probably lose all measurements less then a mm. |
| 133 | 144 |
diff_x += lround((DISTANCE_SCALE * dc * cos(angle))/1000.0); //mm |
| 134 | 145 |
diff_y += lround((DISTANCE_SCALE * dc * sin(angle))/1000.0); //mm |
| 146 |
|
|
| 147 |
if(control_velocity) modify_velocity(); |
|
| 135 | 148 |
} |
| 136 | 149 |
|
| 150 |
void modify_velocity(){
|
|
| 151 |
int diff = target_velocity - velocity; |
|
| 152 |
if(diff > 0){
|
|
| 153 |
move_speed++; |
|
| 154 |
move(move_speed,move_angle); |
|
| 155 |
} |
|
| 156 |
else if(diff < 0){
|
|
| 157 |
move_speed--; |
|
| 158 |
move(move_speed,move_angle); |
|
| 159 |
} |
|
| 160 |
} |
|
| 161 |
|
|
| 137 | 162 |
ISR(TIMER2_COMP_vect){
|
| 138 | 163 |
odometry_run(); |
| 139 | 164 |
} |
Also available in: Unified diff