[CRUSH-code] Re: ToDo for fix-it period
NumBeast
numbermaniac at gmail.com
Thu Mar 1 22:32:22 MST 2007
Skipped content of type multipart/alternative-------------- next part --------------
148 Getdata(&rxdata); /* DO NOT DELETE, or you will be stuck here forever! */
149 #define counter_limit 20
150 #define max_angle 80
151 Run_Camera();
152
153 angle_window = 10;
154 if(T_Packet_Data.pixels < 100)
155 {
156 angle_window = 25;
157 }
158
159 if( Get_Tracking_State() == CAMERA_ON_TARGET)
160 {
161 counter = 0;
162
163 // turn according to the pan angle to head for the light
164 temp_pan_error = (int)PAN_SERVO - 124;
165 if(temp_pan_error > angle_window && temp_pan_error > -max_angle) {
166 // steer right
167 DRIVE_LEFT = 127;
168 DRIVE_RIGHT = 254;
169 } else if(temp_pan_error < -angle_window && temp_pan_error < max_angle) {
170 // steer left
171 DRIVE_LEFT = 254;
172 DRIVE_RIGHT = 127;
173 } else if(temp_pan_error < -max_angle){
174 DRIVE_LEFT = 0;
175 DRIVE_RIGHT = 256;
176 } else if(temp_pan_error > max_angle){
177 DRIVE_LEFT = 256;
178 DRIVE_RIGHT = 0;
179 } else {
180 // drive straight
181 DRIVE_LEFT = DRIVE_RIGHT = 254;
182 }
183
184 } else {
185 // not locked onto light, drive in last known direction for counter_limit
186 if(counter < counter_limit)
187 {
188 counter = counter + 1;
189 }
190 // we have driven past our counter limit, so stop driving
191 else
192 {
193 DRIVE_LEFT = DRIVE_RIGHT = 127;
194 }
195
196 }
197
198 //PAN_SERVO
199 #define motor_max 180
200 #define motor_rev 70
201 /* Add your own autonomous code here. */
202 if(DRIVE_LEFT > motor_max)
203 DRIVE_LEFT = motor_max;
204 if(DRIVE_LEFT < motor_rev)
205 DRIVE_LEFT = motor_rev;
206 if(DRIVE_RIGHT > motor_max)
207 DRIVE_RIGHT = motor_max;
208 if(DRIVE_RIGHT < motor_rev)
209 DRIVE_RIGHT = motor_rev;
210
211 if(T_Packet_Data.pixels > 125 || TILT_SERVO > 200){
212 DRIVE_LEFT = DRIVE_RIGHT = 127; //This value is very tricky, too small and it will oscilate. Too big and it
213 //will crash into the light.
214 }
215 if(T_Packet_Data.pixels > 145 || TILT_SERVO > 210){
216 DRIVE_LEFT = DRIVE_RIGHT = 108;
217 }
218 if(print_counter++ >= 20) {
219 printf("Wheels: l=%u r=%u \n\r", DRIVE_LEFT, DRIVE_RIGHT);
220 print_counter = 0;
221 }
222
223 DRIVE_LEFT = 254 - DRIVE_LEFT;
224 DRIVE_RIGHT = 254 - DRIVE_RIGHT;
225 //Generate_Pwms(pwm13,pwm14,pwm15,pwm16);
226 PWM(pwm13,pwm14,pwm15,pwm16);
227 Putdata(&txdata); /* DO NOT DELETE, or you will get no PWM outputs! */
228 }
229 }
230 }
if(packet_buffer_index < sizeof(T_Packet_Data_Type)) // still building the packet?
209 {
210 // move packet character to our buffer
211 packet_buffer[packet_buffer_index] = byte;
212 packet_buffer_index++;
213 }
214 if(packet_buffer_index == sizeof(T_Packet_Data_Type)) // complete packet?
215 {
216 T_Packet_Data.mx = packet_buffer[0];
217 T_Packet_Data.my = packet_buffer[1];
218 T_Packet_Data.x1 = packet_buffer[2];
219 T_Packet_Data.y1 = packet_buffer[3];
220 T_Packet_Data.x2 = packet_buffer[4];
221 T_Packet_Data.y2 = packet_buffer[5];
222 T_Packet_Data.pixels = packet_buffer[6];
223 T_Packet_Data.confidence = packet_buffer[7];
224
225 camera_t_packets++;
226 switch(viewstate)
227 {
228 case 0:
229 // If we have a centroid to split on...
230 if(T_Packet_Data.mx && T_Packet_Data.my) {
231 // This printf is the data that is interpreted by programport.exe visualization
232 printf("X:%d:%d:%d:%d:%d\r\n", (int) viewstate, (int) T_Packet_Data.x1, (int) T_Packet_Data.y1, (int) T_Packet_Data.x2, (int) T_Packet_Data.y2);
233
234 // This splits down the exact center of the box instead of the centroid
235 slicex = (T_Packet_Data.x2-T_Packet_Data.x1)/2 + T_Packet_Data.x1;
236 printf("Slicex is %d", (int) slicex);
237
238 // If slicex is too high and will cause an NCK we set it back to the max
239 if(slicex > 158)
240 slicex = 158;
241
242 // Initial versions split along the y axis too
243 slicey = T_Packet_Data.my;
244
245 // Reset the virtual window to view the left portion of the window
246 Virtual_Window(1, 1, slicex, 239);
247
248 // Go to the next state in the switch
249 viewstate = 1;
250 }
251 break;
252 case 1:
253 // This printf is the data that is interpreted by programport.exe visualization
254 printf("X:%d:%d:%d:%d:%d\r\n", (int) viewstate, (int) T_Packet_Data.x1, (int) T_Packet_Data.y1, (int) T_Packet_Data.x2, (int) T_Packet_Data.y2);
255
256 // Split the window into right side and go to the next state in the switch
257 Virtual_Window(slicex, 1, 159, 239);
258 viewstate = 2;
259 break;
260 case 2:
261 // This printf is the data that is interpreted by programport.exe visualization
262 printf("X:%d:%d:%d:%d:%d\r\n", (int) viewstate, (int) T_Packet_Data.x1, (int) T_Packet_Data.y1, (int) T_Packet_Data.x2, (int) T_Packet_Data.y2);
263
264 // Resets the view to the whole window and goes back to the initial state
265 Virtual_Window(1, 1, 159, 239);
266 viewstate = 0;
267 break;
268 }
269 Track_Color(R_MIN_DEFAULT, R_MAX_DEFAULT,
270 G_MIN_DEFAULT, G_MAX_DEFAULT,
271 B_MIN_DEFAULT, B_MAX_DEFAULT);
272 state = UNSYNCHRONIZED;
273 }
274 break;
275
More information about the CRUSH-code
mailing list