36 |
36 |
|
37 |
37 |
static volatile unsigned robot_x, robot_y;
|
38 |
38 |
static volatile unsigned last_x, last_y;
|
|
39 |
static volatile unsigned target_x, target_y;
|
39 |
40 |
static volatile int updated_robot_pos_ready;
|
40 |
|
static volatile int new_movement_command_received;
|
41 |
41 |
static volatile int robot_lost; //set to 1 if robot's position is not known by the server, else 0.
|
42 |
42 |
static volatile VirtualWall virtual_wall;
|
43 |
43 |
|
44 |
|
static int virtual_wall_available;
|
|
44 |
typedef enum {MOVING, MOVING_TO_POSITION, NO_COMMAND} ColonetRobotCommandState;
|
|
45 |
static volatile ColonetRobotCommandState robot_state;
|
45 |
46 |
|
|
47 |
static volatile int virtual_wall_up_available, virtual_wall_low_available;
|
|
48 |
|
46 |
49 |
#define ERROR_MAG_TO_MOTOR_DIFFENTIAL_CONST (1)
|
47 |
50 |
|
48 |
51 |
/* Internal function prototypes */
|
... | ... | |
61 |
64 |
return (high<<8) | low;
|
62 |
65 |
}
|
63 |
66 |
|
|
67 |
static int virtual_wall_available() {
|
|
68 |
return virtual_wall_up_available && virtual_wall_low_available;
|
|
69 |
}
|
|
70 |
|
64 |
71 |
/* Public functions */
|
65 |
72 |
int colonet_init() {
|
66 |
73 |
colonet_pgh.groupCode = COLONET_PACKET_GROUP_ID;
|
... | ... | |
75 |
82 |
robot_x = last_x = 0;
|
76 |
83 |
robot_y = last_y = 0;
|
77 |
84 |
updated_robot_pos_ready = 0;
|
78 |
|
new_movement_command_received = 0;
|
79 |
85 |
robot_lost = 1;
|
80 |
86 |
|
81 |
|
virtual_wall_available = 0;
|
|
87 |
virtual_wall_up_available = 0;
|
|
88 |
virtual_wall_low_available = 0;
|
|
89 |
|
82 |
90 |
virtual_wall.up_x = 0;
|
83 |
91 |
virtual_wall.up_y = 0;
|
84 |
92 |
virtual_wall.low_x = 0;
|
85 |
93 |
virtual_wall.low_y = 0;
|
86 |
94 |
|
|
95 |
robot_state = NO_COMMAND;
|
|
96 |
|
87 |
97 |
return 0;
|
88 |
98 |
}
|
89 |
99 |
|
... | ... | |
162 |
172 |
|
163 |
173 |
}
|
164 |
174 |
|
165 |
|
static void move_routine() {
|
166 |
|
do {
|
167 |
|
int count = 0;
|
168 |
|
while (!updated_robot_pos_ready) {
|
169 |
|
wl_do();
|
170 |
|
if (count++ == 5000) { // in case the server missed it...
|
171 |
|
request_abs_position();
|
172 |
|
count = 0;
|
173 |
|
}
|
|
175 |
static int inside_virtual_wall(int x, int y) {
|
|
176 |
if (virtual_wall_available()) {
|
|
177 |
if (robot_x < virtual_wall.up_x || robot_x > virtual_wall.low_x || robot_y < virtual_wall.up_y
|
|
178 |
|| robot_y > virtual_wall.low_y) {
|
|
179 |
return 0;
|
|
180 |
} else {
|
|
181 |
return 1;
|
174 |
182 |
}
|
|
183 |
} else {
|
|
184 |
return 1;
|
|
185 |
}
|
|
186 |
}
|
175 |
187 |
|
176 |
|
wl_do();
|
177 |
188 |
|
178 |
|
if (robot_lost) {
|
179 |
|
motors_off();
|
180 |
|
}
|
|
189 |
/**
|
|
190 |
* Main loop which responds to commands.
|
|
191 |
*/
|
|
192 |
void colonet_run(void (*step)(void)) {
|
|
193 |
char buf[80];
|
181 |
194 |
|
182 |
|
updated_robot_pos_ready = 0;
|
|
195 |
while (1) {
|
|
196 |
wl_do();
|
183 |
197 |
request_abs_position();
|
184 |
|
} while (!new_movement_command_received);
|
185 |
|
}
|
|
198 |
step();
|
186 |
199 |
|
187 |
|
static void move_to_position_routine(unsigned target_x, unsigned target_y) {
|
188 |
|
new_movement_command_received = 0;
|
|
200 |
switch (robot_state) {
|
|
201 |
case NO_COMMAND:
|
|
202 |
break;
|
189 |
203 |
|
190 |
|
usb_puts("move to absolute position routine!\n");
|
|
204 |
case MOVING:
|
|
205 |
if (robot_lost) {
|
|
206 |
motors_off();
|
|
207 |
robot_state = NO_COMMAND;
|
|
208 |
} else {
|
|
209 |
if (updated_robot_pos_ready) {
|
|
210 |
if (!inside_virtual_wall(robot_x, robot_y)) {
|
|
211 |
usb_puts("outside of boundary\n");
|
|
212 |
motors_off();
|
|
213 |
robot_state = NO_COMMAND;
|
|
214 |
}
|
191 |
215 |
|
192 |
|
updated_robot_pos_ready = 0;
|
193 |
|
request_abs_position(); // While we're doing this computation, server can be reporting next position.
|
|
216 |
updated_robot_pos_ready = 0;
|
|
217 |
}
|
|
218 |
}
|
|
219 |
break;
|
194 |
220 |
|
195 |
|
int count = 0;
|
196 |
|
while (!updated_robot_pos_ready) {
|
197 |
|
wl_do();
|
198 |
|
if (count++ == 5000) { // in case the server missed it...
|
199 |
|
request_abs_position();
|
200 |
|
count = 0;
|
201 |
|
}
|
202 |
|
}
|
|
221 |
case MOVING_TO_POSITION:
|
|
222 |
usb_puts("move to absolute position\n");
|
203 |
223 |
|
204 |
|
int dist = 0;
|
|
224 |
if (robot_lost) {
|
|
225 |
motors_off();
|
|
226 |
robot_state = NO_COMMAND;
|
|
227 |
} else {
|
|
228 |
if (updated_robot_pos_ready) {
|
|
229 |
updated_robot_pos_ready = 0;
|
205 |
230 |
|
206 |
|
while (!new_movement_command_received &&
|
207 |
|
(dist = dist_squared(robot_x, robot_y, target_x, target_y)) > TARGET_POSITION_STOP_DISTANCE_THRESHOLD) {
|
208 |
|
char buf[80];
|
209 |
|
//sprintf(buf, "dist:%d\n", dist);
|
210 |
|
//usb_puts(buf);
|
|
231 |
if (!inside_virtual_wall(robot_x, robot_y)) {
|
|
232 |
usb_puts("outside of boundary\n");
|
|
233 |
motors_off();
|
|
234 |
robot_state = NO_COMMAND;
|
|
235 |
} else {
|
|
236 |
int dist = 0;
|
|
237 |
if ((dist = dist_squared(robot_x, robot_y, target_x, target_y)) < TARGET_POSITION_STOP_DISTANCE_THRESHOLD) {
|
|
238 |
// Reached destination.
|
|
239 |
motors_off();
|
|
240 |
robot_state = NO_COMMAND;
|
|
241 |
} else {
|
|
242 |
// e is the error vector (where we want to go)
|
|
243 |
//char buf[80];
|
|
244 |
// sprintf(buf, "target_x:%d target_y:%d robot_x:%d robot_y:%d\n", target_x, target_y, robot_x, robot_y);
|
|
245 |
// usb_puts(buf);
|
211 |
246 |
|
212 |
|
updated_robot_pos_ready = 0;
|
213 |
|
request_abs_position(); // While we're doing this computation, server can be reporting next position.
|
|
247 |
int e_x = (int)target_x - (int)robot_x;
|
|
248 |
int e_y = (int)target_y - (int)robot_y;
|
214 |
249 |
|
215 |
|
if (robot_lost) {
|
216 |
|
usb_puts("lost known\n");
|
217 |
|
motors_off();
|
218 |
|
} else {
|
219 |
|
// e is the error vector (where we want to go)
|
220 |
|
//char buf[80];
|
221 |
|
// sprintf(buf, "target_x:%d target_y:%d robot_x:%d robot_y:%d\n", target_x, target_y, robot_x, robot_y);
|
222 |
|
// usb_puts(buf);
|
|
250 |
// sprintf(buf, "e: (x:%d,y:%d)\n", e_x, e_y);
|
|
251 |
// usb_puts(buf);
|
223 |
252 |
|
224 |
|
int e_x = (int)target_x - (int)robot_x;
|
225 |
|
int e_y = (int)target_y - (int)robot_y;
|
|
253 |
// v is the velocity vector (where we just went)
|
|
254 |
int v_x = (int)robot_x - (int)last_x;
|
|
255 |
int v_y = (int)robot_y - (int)last_y;
|
226 |
256 |
|
227 |
|
// sprintf(buf, "e: (x:%d,y:%d)\n", e_x, e_y);
|
228 |
|
// usb_puts(buf);
|
|
257 |
//sprintf(buf, "v: (x:%d,y:%d)\n", v_x, v_y);
|
|
258 |
//usb_puts(buf);
|
229 |
259 |
|
230 |
|
// v is the velocity vector (where we just went)
|
231 |
|
int v_x = (int)robot_x - (int)last_x;
|
232 |
|
int v_y = (int)robot_y - (int)last_y;
|
|
260 |
if (abs(v_x) < 2 && abs(v_x) < 2) {
|
|
261 |
// if we didn't move since the last check, just go forward.
|
|
262 |
motor1_set(1, 180);
|
|
263 |
motor2_set(1, 180);
|
|
264 |
} else {
|
|
265 |
int e_dot_v = e_x * v_x + e_y * v_y;
|
233 |
266 |
|
234 |
|
//sprintf(buf, "v: (x:%d,y:%d)\n", v_x, v_y);
|
235 |
|
//usb_puts(buf);
|
|
267 |
// move according to algortihm
|
|
268 |
int e_mag = e_x*e_x + e_y*e_y;
|
|
269 |
// int motor_differential = 180;//e_mag >> 7;
|
|
270 |
int motor_differential = get_motor_differential(e_x, e_y, v_x, v_y);
|
236 |
271 |
|
237 |
|
if (abs(v_x) < 2 && abs(v_x) < 2) {
|
238 |
|
// if we didn't move since the last check, just go forward.
|
239 |
|
motor1_set(1, 180);
|
240 |
|
motor2_set(1, 180);
|
241 |
|
} else {
|
242 |
|
int e_dot_v = e_x * v_x + e_y * v_y;
|
|
272 |
// p is perpendicular to v, directed to the "left" of the robot.
|
|
273 |
int p_x = v_y;
|
|
274 |
int p_y = -v_x;
|
|
275 |
int dot_product_pe = p_x * e_x + p_y * e_y;
|
243 |
276 |
|
244 |
|
// move according to algortihm
|
245 |
|
int e_mag = e_x*e_x + e_y*e_y;
|
246 |
|
// int motor_differential = 180;//e_mag >> 7;
|
247 |
|
int motor_differential = get_motor_differential(e_x, e_y, v_x, v_y);
|
|
277 |
// if the dot product of p and e is negative, we should turn right. otherwise turn left.
|
|
278 |
if (dot_product_pe < 0) {
|
|
279 |
motor_differential = -motor_differential;
|
|
280 |
}
|
248 |
281 |
|
249 |
|
// p is perpendicular to v, directed to the "left" of the robot.
|
250 |
|
int p_x = v_y;
|
251 |
|
int p_y = -v_x;
|
252 |
|
int dot_product_pe = p_x * e_x + p_y * e_y;
|
253 |
|
|
254 |
|
// if the dot product of p and e is negative, we should turn right. otherwise turn left.
|
255 |
|
if (dot_product_pe < 0) {
|
256 |
|
motor_differential = -motor_differential;
|
|
282 |
set_motors_with_differential(motor_differential);
|
|
283 |
}
|
|
284 |
}
|
|
285 |
}
|
257 |
286 |
}
|
258 |
|
|
259 |
|
set_motors_with_differential(motor_differential);
|
260 |
287 |
}
|
261 |
|
}
|
262 |
288 |
|
263 |
|
if (robot_lost) {
|
264 |
|
motors_off();
|
|
289 |
break;
|
265 |
290 |
}
|
266 |
|
|
267 |
|
// once we move, ask for the position again.
|
268 |
|
count = 0;
|
269 |
|
while (!updated_robot_pos_ready) {
|
270 |
|
wl_do();
|
271 |
|
if (count++ == 5000) { // in case the server missed it...
|
272 |
|
request_abs_position();
|
273 |
|
count = 0;
|
274 |
|
}
|
275 |
|
}
|
276 |
291 |
}
|
|
292 |
}
|
277 |
293 |
|
278 |
|
motors_off();
|
279 |
294 |
|
280 |
|
// Once this function terminates, the robot be at its destination.
|
281 |
|
}
|
282 |
|
|
283 |
295 |
/* Private functions */
|
284 |
296 |
|
285 |
297 |
/** @brief Handles colonet packets. Should be called by parse_buffer
|
... | ... | |
378 |
390 |
last_x = robot_x;
|
379 |
391 |
last_y = robot_y;
|
380 |
392 |
|
|
393 |
robot_lost = 1;
|
|
394 |
|
381 |
395 |
usb_puts("lost\n");
|
382 |
396 |
|
383 |
|
robot_lost = 1;
|
384 |
|
|
385 |
397 |
motors_off();
|
386 |
398 |
break;
|
387 |
399 |
|
|
400 |
case SERVER_CLEAR_VIRTUAL_WALL:
|
|
401 |
virtual_wall_up_available = 0;
|
|
402 |
virtual_wall_low_available = 0;
|
|
403 |
break;
|
|
404 |
|
388 |
405 |
case SERVER_REPORT_VIRTUAL_WALL_UPPER:
|
389 |
406 |
virtual_wall.up_x = int_args[0];
|
390 |
407 |
virtual_wall.up_y = int_args[1];
|
|
408 |
|
|
409 |
virtual_wall_up_available = 1;
|
|
410 |
|
|
411 |
sprintf(buf, "%d %d\n", virtual_wall.up_x, virtual_wall.up_y);
|
|
412 |
usb_puts(buf);
|
391 |
413 |
break;
|
392 |
414 |
|
393 |
415 |
case SERVER_REPORT_VIRTUAL_WALL_LOWER:
|
394 |
416 |
virtual_wall.low_x = int_args[0];
|
395 |
417 |
virtual_wall.low_y = int_args[1];
|
|
418 |
|
|
419 |
sprintf(buf, "%d %d\n", virtual_wall.low_x, virtual_wall.low_y);
|
|
420 |
usb_puts(buf);
|
|
421 |
|
|
422 |
virtual_wall_low_available = 1;
|
396 |
423 |
break;
|
397 |
424 |
|
398 |
425 |
case SERVER_REPORT_POSITION_TO_ROBOT:
|
399 |
|
robot_lost = 0;
|
400 |
|
|
401 |
426 |
last_x = robot_x;
|
402 |
427 |
last_y = robot_y;
|
403 |
428 |
robot_x = (unsigned)int_args[0];
|
404 |
429 |
robot_y = (unsigned)int_args[1];
|
405 |
430 |
|
406 |
431 |
updated_robot_pos_ready = 1;
|
|
432 |
robot_lost = 0;
|
407 |
433 |
|
408 |
|
if (virtual_wall_available) {
|
409 |
|
if (robot_x < virtual_wall.up_x || robot_x > virtual_wall.low_x || robot_y < virtual_wall.up_x
|
410 |
|
|| robot_y > virtual_wall.up_y) {
|
411 |
|
robot_lost = 1;
|
412 |
|
motors_off();
|
413 |
|
}
|
414 |
|
}
|
415 |
|
|
416 |
|
// sprintf(buf, "pos is: (x:%d, y:%d)\n", robot_x, robot_y);
|
|
434 |
// sprintf(buf, "pos is: (x:%d, y:%d)\n", robot_x, robot_y);
|
417 |
435 |
//usb_puts(buf);
|
418 |
436 |
break;
|
419 |
437 |
|
420 |
438 |
case MOVE_TO_ABSOLUTE_POSITION:
|
421 |
|
new_movement_command_received = 1;
|
422 |
|
|
423 |
439 |
sprintf(buf, "move to abs (x:%u, y:%u)\n", (unsigned)int_args[0], (unsigned)int_args[1]);
|
424 |
440 |
usb_puts(buf);
|
425 |
441 |
|
426 |
|
move_to_position_routine((unsigned)int_args[0], (unsigned)int_args[1]);
|
|
442 |
target_x = (unsigned)int_args[0];
|
|
443 |
target_y = (unsigned)int_args[1];
|
|
444 |
|
|
445 |
robot_state = MOVING_TO_POSITION;
|
427 |
446 |
break;
|
428 |
447 |
|
429 |
448 |
//Buzzer
|
... | ... | |
466 |
485 |
motors_init();
|
467 |
486 |
break;
|
468 |
487 |
|
469 |
|
case MOTOR1_SET:
|
470 |
|
new_movement_command_received = 1;
|
471 |
|
|
472 |
|
sprintf(buf, "calling motor1_set [%i] [%i]\n", args[0], args[1]);
|
473 |
|
usb_puts(buf);
|
474 |
|
motor1_set(args[0], args[1]);
|
475 |
|
break;
|
476 |
|
|
477 |
|
case MOTOR2_SET:
|
478 |
|
new_movement_command_received = 1;
|
479 |
|
|
480 |
|
sprintf(buf, "calling motor2_set [%i] [%i]\n", args[0], args[1]);
|
481 |
|
usb_puts(buf);
|
482 |
|
motor2_set(args[0], args[1]);
|
483 |
|
break;
|
484 |
|
|
485 |
488 |
case MOTORS_OFF:
|
486 |
|
new_movement_command_received = 1;
|
|
489 |
robot_state = NO_COMMAND;
|
487 |
490 |
|
488 |
491 |
usb_puts("calling motors_off\n");
|
489 |
492 |
motors_off();
|
490 |
493 |
break;
|
491 |
494 |
|
492 |
495 |
case MOVE:
|
493 |
|
new_movement_command_received = 1;
|
494 |
|
/* format for move: left direction, left velocity, right direction, right velocity */
|
495 |
|
sprintf(buf, "calling move\n");
|
496 |
|
usb_puts(buf);
|
497 |
|
motor1_set(args[0], args[1]);
|
498 |
|
motor2_set(args[2], args[3]);
|
|
496 |
if (args[1] == 0 && args[3] == 0) {
|
|
497 |
motors_off();
|
|
498 |
robot_state = MOVING;
|
499 |
499 |
|
500 |
|
/* Loop and update position for virtual wall. */
|
501 |
|
move_routine();
|
|
500 |
usb_puts("stopped\n");
|
|
501 |
} else {
|
|
502 |
/* format for move: left direction, left velocity, right direction, right velocity */
|
|
503 |
motor1_set(args[0], args[1]);
|
|
504 |
motor2_set(args[2], args[3]);
|
|
505 |
|
|
506 |
/* Loop and update position for virtual wall. */
|
|
507 |
robot_state = MOVING;
|
|
508 |
}
|
502 |
509 |
break;
|
503 |
510 |
|
504 |
511 |
case PRINTF:
|
505 |
512 |
usb_puts((char*)pkt->data);
|
506 |
513 |
break;
|
|
514 |
|
507 |
515 |
case KILL_ROBOT:
|
508 |
|
new_movement_command_received = 1;
|
|
516 |
robot_state = NO_COMMAND;
|
509 |
517 |
|
510 |
518 |
motors_off();
|
511 |
519 |
buzzer_off();
|