root / trunk / code / projects / mapping / matlab / testRobot / odometry.c @ 953
History | View | Annotate | Download (3.75 KB)
1 | 953 | justin | #include "odometry.h" |
---|---|---|---|
2 | #include <encoders.h> |
||
3 | #include <move.h> |
||
4 | #include <math.h> |
||
5 | #include <avr/interrupt.h> |
||
6 | #include <dragonfly_lib.h> |
||
7 | |||
8 | long lround(double d); |
||
9 | |||
10 | char control_velocity = 0; |
||
11 | int encoder_ltm1, encoder_rtm1, move_speed, move_angle;
|
||
12 | |||
13 | //Measure mm displacement in x and y from starting position. (+x:theta=0 +y:theta=M_PI/2)
|
||
14 | long diff_x,diff_y, velocity, target_velocity, target_angular_velocity;
|
||
15 | |||
16 | //Measures radian angle displacement from starting position. (initially 0)
|
||
17 | double angle;
|
||
18 | |||
19 | /**
|
||
20 | * Retrieve the estimated x position. [mm]
|
||
21 | */
|
||
22 | long odometry_dx(void){ |
||
23 | return diff_x;
|
||
24 | } |
||
25 | |||
26 | /**
|
||
27 | * Retrieve the estimated y position. [mm]
|
||
28 | */
|
||
29 | long odometry_dy(void){ |
||
30 | return diff_y;
|
||
31 | } |
||
32 | |||
33 | /**
|
||
34 | * Retrieve the estimated angle [radians]
|
||
35 | */
|
||
36 | double odometry_angle(void){ |
||
37 | return angle;
|
||
38 | } |
||
39 | /**
|
||
40 | * Retrieve the estimated velocity [mm / s]
|
||
41 | */
|
||
42 | long odometry_velocity(void){ |
||
43 | return velocity;
|
||
44 | } |
||
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 | |||
54 | /**
|
||
55 | * Initializes odometry to run on timer 2.
|
||
56 | * Also resets all values so that the center of the robot is
|
||
57 | * considered to be at the origin facing the x direction.
|
||
58 | */
|
||
59 | void odometry_init(void){ |
||
60 | |||
61 | encoders_init(); |
||
62 | |||
63 | delay_ms(100);
|
||
64 | |||
65 | odometry_reset(); |
||
66 | |||
67 | |||
68 | //CTC Mode. CLK / 1024
|
||
69 | TCCR2 = 0; // (Fully disconnected) |
||
70 | TCCR2 = _BV(WGM21) | _BV(CS22) | _BV(CS20); //CLK/1024 , CTC Mode.
|
||
71 | |||
72 | TIMSK |= _BV(OCIE2); |
||
73 | |||
74 | OCR2 = ODOMETRY_CLK; |
||
75 | } |
||
76 | /**
|
||
77 | * Resets all values so that the center of the robot is
|
||
78 | * considered to be at the origin facing the x direction.
|
||
79 | */
|
||
80 | void odometry_reset(void){ |
||
81 | diff_x = 0;
|
||
82 | diff_y = 0;
|
||
83 | encoder_ltm1 = encoder_read(LEFT); |
||
84 | encoder_rtm1 = encoder_read(RIGHT); |
||
85 | angle = 0.0; |
||
86 | } |
||
87 | |||
88 | |||
89 | void odometry_run(void){ |
||
90 | //Angle swept through in a time step CCW-
|
||
91 | double theta, rl, dc;
|
||
92 | long dr,dl;
|
||
93 | char buf[100]; |
||
94 | |||
95 | //Get the change in wheel positions
|
||
96 | { |
||
97 | int encoder_right = encoder_read(RIGHT);
|
||
98 | int encoder_left = encoder_read(LEFT);
|
||
99 | |||
100 | dl = encoder_left - encoder_ltm1; |
||
101 | dr = encoder_right - encoder_rtm1; |
||
102 | |||
103 | //No motion.
|
||
104 | if(dl == 0 && dr == 0){ |
||
105 | if(control_velocity) modify_velocity();
|
||
106 | return;
|
||
107 | } |
||
108 | |||
109 | encoder_ltm1 = encoder_left; |
||
110 | encoder_rtm1 = encoder_right; |
||
111 | |||
112 | // Try to avoid over/underflow.
|
||
113 | dl = dl > 512 ? dl - 1024 :dl; |
||
114 | dl = dl < -512 ? dl + 1024 :dl; |
||
115 | dr = dr > 512 ? dr - 1024 :dr; |
||
116 | dr = dr < -512 ? dr + 1024 :dr; |
||
117 | |||
118 | //Convert "clicks" to um
|
||
119 | dl *= CLICK_DISTANCE_UM; //um
|
||
120 | dr *= CLICK_DISTANCE_UM; |
||
121 | } |
||
122 | |||
123 | |||
124 | if(dl == dr){
|
||
125 | diff_x += lround(dl*cos(angle)/1000.0); //mm |
||
126 | diff_y += lround(dl*sin(angle)/1000.0); //mm |
||
127 | velocity = lround((dl * 1000.0)/(ODOMETRY_CLK*TIME_SCALE)); //um / ms = mm/s |
||
128 | if(control_velocity) modify_velocity();
|
||
129 | return;
|
||
130 | } |
||
131 | |||
132 | //Call the left wheel 0, clockwise positive.
|
||
133 | rl = ((double)(ROBOT_WIDTH_UM*dl))/((double)(dl - dr)); //um |
||
134 | theta = ((double)(dl - dr))/((double)ROBOT_WIDTH_UM); //rad |
||
135 | |||
136 | //Distance the center has traveled.
|
||
137 | dc = (theta * (rl - ROBOT_WIDTH_UM)) / 2.0; //um |
||
138 | velocity = lround( dc * 1000 /(ODOMETRY_CLK*TIME_SCALE));
|
||
139 | |||
140 | //angle is necessarily CCW+, so subtract.
|
||
141 | angle -= ANGLE_SCALE * theta; |
||
142 | |||
143 | //Change state variables. Probably lose all measurements less then a mm.
|
||
144 | diff_x += lround((DISTANCE_SCALE * dc * cos(angle))/1000.0); //mm |
||
145 | diff_y += lround((DISTANCE_SCALE * dc * sin(angle))/1000.0); //mm |
||
146 | |||
147 | if(control_velocity) modify_velocity();
|
||
148 | } |
||
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 | |||
162 | ISR(TIMER2_COMP_vect){ |
||
163 | odometry_run(); |
||
164 | } |
||
165 | |||
166 | long lround(double d){ |
||
167 | double f = floor(d);
|
||
168 | return (long)(d - f > 0.5 ? f + 1 : f); |
||
169 | } |
||
170 |