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