Revision 929
Nothing was fixed nothing changed.
| odometry.c | ||
|---|---|---|
| 113 | 113 |
void odometry_run(void){
|
| 114 | 114 |
//Angle swept through in a time step CCW- |
| 115 | 115 |
double theta, rl, dc; |
| 116 |
long dr,dl; |
|
| 117 |
char buf[40]; |
|
| 116 | 118 |
|
| 117 |
double dr,dl; |
|
| 118 |
|
|
| 119 |
char buf[40]; |
|
| 120 |
//Relative to the inner wheel, turn radius. |
|
| 121 |
|
|
| 122 | 119 |
//Get the change in wheel positions |
| 120 |
{
|
|
| 121 |
int encoder_right = encoder_read(RIGHT); |
|
| 122 |
int encoder_left = encoder_read(LEFT); |
|
| 123 | 123 |
|
| 124 |
int encoder_right = encoder_read(RIGHT);
|
|
| 125 |
int encoder_left = encoder_read(LEFT);
|
|
| 124 |
dl = encoder_left - encoder_ltm1;
|
|
| 125 |
dr = encoder_right - encoder_rtm1;
|
|
| 126 | 126 |
|
| 127 |
dl = encoder_left - encoder_ltm1;
|
|
| 128 |
dr = encoder_right - encoder_rtm1;
|
|
| 127 |
encoder_ltm1 = encoder_left;
|
|
| 128 |
encoder_rtm1 = encoder_right;
|
|
| 129 | 129 |
|
| 130 |
//if(dl >=-1 && dl <= 1 && dr >= -1 && dr <= 1) return; |
|
| 130 |
// Try to avoid over/underflow. |
|
| 131 |
dl = dl > 512 ? dl - 1024 :dl; |
|
| 132 |
dl = dl < -512 ? dl + 1024 :dl; |
|
| 133 |
dr = dr > 512 ? dr - 1024 :dr; |
|
| 134 |
dr = dr < -512 ? dr + 1024 :dr; |
|
| 131 | 135 |
|
| 132 |
encoder_ltm1 = encoder_left; |
|
| 133 |
encoder_rtm1 = encoder_right; |
|
| 134 |
|
|
| 135 |
// Try to avoid over/underflow. |
|
| 136 |
dl = dl > 512 ? dl - 1024 :dl; |
|
| 137 |
dl = dl < -512 ? dl + 1024 :dl; |
|
| 138 |
dr = dr > 512 ? dr - 1024 :dr; |
|
| 139 |
dr = dr < -512 ? dr + 1024 :dr; |
|
| 140 |
|
|
| 141 |
//Convert "clicks" to um |
|
| 142 |
dl *= CLICK_DISTANCE_UM; //um |
|
| 143 |
dr *= CLICK_DISTANCE_UM; |
|
| 136 |
//Convert "clicks" to um |
|
| 137 |
dl *= CLICK_DISTANCE_UM; //um |
|
| 138 |
dr *= CLICK_DISTANCE_UM; |
|
| 139 |
} |
|
| 144 | 140 |
|
| 145 | 141 |
if(dl == dr){
|
| 146 |
//usb_putc('#');
|
|
| 147 | 142 |
diff_x += dl*cos(angle)/1000; |
| 148 | 143 |
diff_y += dl*sin(angle)/1000; |
| 149 | 144 |
return; |
| ... | ... | |
| 151 | 146 |
|
| 152 | 147 |
//Call the left wheel 0, clockwise positive. |
| 153 | 148 |
rl = ((double)(ROBOT_WIDTH_UM*dl))/((double)(dl - dr)); //um |
| 154 |
|
|
| 155 | 149 |
theta = ((double)(dl - dr))/((double)ROBOT_WIDTH_UM); //rad |
| 156 | 150 |
|
| 157 | 151 |
dc = (rl - ROBOT_WIDTH_UM/2)*theta; //um |
| 158 |
|
|
| 152 |
|
|
| 153 |
//Change state variables. |
|
| 159 | 154 |
diff_x += ((dc * cos(angle))/1000); //mm |
| 160 | 155 |
diff_y += ((dc * sin(angle))/1000); //mm |
| 161 |
|
|
| 162 |
//Change state variables. |
|
| 163 |
|
|
| 156 |
|
|
| 164 | 157 |
//angle is necessarily CCW+, so subtract. |
| 165 | 158 |
angle -= theta; |
| 166 | 159 |
|
Also available in: Unified diff