root / trunk / code / projects / colonet / robot / colonet_dragonfly / colonet_dragonfly.c @ 681
History | View | Annotate | Download (14.4 KB)
1 | 11 | emarinel | /** @file colonet.c
|
---|---|---|---|
2 | * @brief Colonet library for DRAGONFLY colony robots
|
||
3 | *
|
||
4 | * @author Eugene Marinelli
|
||
5 | 149 | emarinel | * @date 10/10/07
|
6 | *
|
||
7 | 11 | emarinel | * @bug Handler registration not tested
|
8 | * @bug Request reponding not implemented - only accepts commands.
|
||
9 | */
|
||
10 | |||
11 | 149 | emarinel | #include <assert.h> |
12 | 523 | emarinel | #include <battery.h> |
13 | #include <colonet_defs.h> |
||
14 | #include <colonet_dragonfly.h> |
||
15 | 11 | emarinel | #include <dragonfly_lib.h> |
16 | 523 | emarinel | #include <math.h> |
17 | 149 | emarinel | #include <string.h> |
18 | #include <wireless.h> |
||
19 | |||
20 | 648 | emarinel | #define TARGET_POSITION_STOP_DISTANCE_THRESHOLD (550) |
21 | 11 | emarinel | |
22 | typedef struct { |
||
23 | unsigned char msgId; //is this necessary? |
||
24 | void (*handler)(void); |
||
25 | } UserHandler; |
||
26 | |||
27 | 661 | jknichel | typedef struct { |
28 | int up_x;
|
||
29 | int up_y;
|
||
30 | int low_x;
|
||
31 | int low_y;
|
||
32 | } VirtualWall; |
||
33 | |||
34 | 11 | emarinel | /* Globals (internal) */
|
35 | static UserHandler user_handlers[USER_DEFINED_MSG_TOTAL];
|
||
36 | |||
37 | 661 | jknichel | static volatile unsigned robot_x, robot_y; |
38 | static volatile unsigned last_x, last_y; |
||
39 | 680 | emarinel | static volatile unsigned target_x, target_y; |
40 | 523 | emarinel | static volatile int updated_robot_pos_ready; |
41 | 648 | emarinel | static volatile int robot_lost; //set to 1 if robot's position is not known by the server, else 0. |
42 | 661 | jknichel | static volatile VirtualWall virtual_wall; |
43 | 648 | emarinel | |
44 | 680 | emarinel | typedef enum {MOVING, MOVING_TO_POSITION, NO_COMMAND} ColonetRobotCommandState; |
45 | static volatile ColonetRobotCommandState robot_state; |
||
46 | 661 | jknichel | |
47 | 680 | emarinel | static volatile int virtual_wall_up_available, virtual_wall_low_available; |
48 | |||
49 | 550 | emarinel | #define ERROR_MAG_TO_MOTOR_DIFFENTIAL_CONST (1) |
50 | 433 | emarinel | |
51 | 11 | emarinel | /* Internal function prototypes */
|
52 | 550 | emarinel | static unsigned two_bytes_to_int(unsigned char high, unsigned char low); |
53 | 348 | emarinel | static void colonet_handle_receive(char type, int source, unsigned char* packet, int length); |
54 | 550 | emarinel | static void move_to_position_routine(unsigned x, unsigned y); |
55 | 644 | gtress | static int dist_squared(int x1, int y1, int x2, int y2); |
56 | 661 | jknichel | static void move_routine(void); |
57 | 11 | emarinel | |
58 | 149 | emarinel | static PacketGroupHandler colonet_pgh;
|
59 | |||
60 | 550 | emarinel | /* two_bytes_to_int(char a, char b)
|
61 | * Returns int of form [high][low]
|
||
62 | */
|
||
63 | static unsigned int two_bytes_to_int(unsigned char high, unsigned char low) { |
||
64 | return (high<<8) | low; |
||
65 | } |
||
66 | |||
67 | 680 | emarinel | static int virtual_wall_available() { |
68 | return virtual_wall_up_available && virtual_wall_low_available;
|
||
69 | } |
||
70 | |||
71 | 11 | emarinel | /* Public functions */
|
72 | 149 | emarinel | int colonet_init() {
|
73 | colonet_pgh.groupCode = COLONET_PACKET_GROUP_ID; |
||
74 | colonet_pgh.timeout_handler = NULL;
|
||
75 | colonet_pgh.handle_response = NULL;
|
||
76 | colonet_pgh.handle_receive = colonet_handle_receive; |
||
77 | colonet_pgh.unregister = NULL;
|
||
78 | |||
79 | // TODO this should return an error if wl_init has not been called yet.
|
||
80 | wl_register_packet_group(&colonet_pgh); |
||
81 | |||
82 | 644 | gtress | robot_x = last_x = 0;
|
83 | robot_y = last_y = 0;
|
||
84 | 531 | emarinel | updated_robot_pos_ready = 0;
|
85 | 648 | emarinel | robot_lost = 1;
|
86 | 433 | emarinel | |
87 | 680 | emarinel | virtual_wall_up_available = 0;
|
88 | virtual_wall_low_available = 0;
|
||
89 | |||
90 | 661 | jknichel | virtual_wall.up_x = 0;
|
91 | virtual_wall.up_y = 0;
|
||
92 | virtual_wall.low_x = 0;
|
||
93 | virtual_wall.low_y = 0;
|
||
94 | |||
95 | 680 | emarinel | robot_state = NO_COMMAND; |
96 | |||
97 | 149 | emarinel | return 0; |
98 | } |
||
99 | |||
100 | 349 | emarinel | /* colonet_add_message
|
101 | * Adds a user-defined message
|
||
102 | */
|
||
103 | int colonet_add_message(unsigned char msgId, void (*handler)(void)) { |
||
104 | if(msgId < USER_DEFINED_MSG_ID_START /* || msgId > USER_DEFINED_MSG_ID_END */){ |
||
105 | return -1; |
||
106 | } |
||
107 | |||
108 | /* Register this function in the array */
|
||
109 | user_handlers[msgId-USER_DEFINED_MSG_ID_START].msgId = msgId; |
||
110 | user_handlers[msgId-USER_DEFINED_MSG_ID_START].handler = handler; |
||
111 | |||
112 | return 0; |
||
113 | } |
||
114 | |||
115 | 519 | emarinel | void get_absolute_position(int* x, int* y) { |
116 | *x = robot_x; |
||
117 | *y = robot_y; |
||
118 | 433 | emarinel | } |
119 | |||
120 | 681 | emarinel | static void report_arrived() { |
121 | //usb_puts("requesting_abs_position\n");
|
||
122 | ColonetRobotServerPacket pkt; |
||
123 | pkt.client_id = -1; //global |
||
124 | pkt.msg_code = ROBOT_REPORT_ARRIVED_AT_POSITION; |
||
125 | wl_send_global_packet(colonet_pgh.groupCode, 0, (char*)&pkt, sizeof(ColonetRobotServerPacket), 0); |
||
126 | } |
||
127 | |||
128 | 519 | emarinel | void request_abs_position() {
|
129 | 550 | emarinel | //usb_puts("requesting_abs_position\n");
|
130 | 519 | emarinel | ColonetRobotServerPacket pkt; |
131 | 681 | emarinel | pkt.client_id = -1; //global |
132 | 519 | emarinel | pkt.msg_code = ROBOT_REQUEST_POSITION_FROM_SERVER; |
133 | 623 | emarinel | wl_send_global_packet(colonet_pgh.groupCode, 0, (char*)&pkt, sizeof(ColonetRobotServerPacket), 0); |
134 | 519 | emarinel | } |
135 | |||
136 | 644 | gtress | static int dist_squared(int x1, int y1, int x2, int y2) { |
137 | int dx = x1 - x2;
|
||
138 | int dy = y1 - y2;
|
||
139 | return dx*dx + dy*dy;
|
||
140 | 433 | emarinel | } |
141 | |||
142 | 523 | emarinel | /** Differential is the intended left motor speed - right motor speed. */
|
143 | static void set_motors_with_differential(int differential) { |
||
144 | int ml, mr;
|
||
145 | |||
146 | if (differential >= 0) { |
||
147 | /* Going left or straight. */
|
||
148 | 648 | emarinel | ml = 200;
|
149 | 523 | emarinel | mr = ml - differential; |
150 | } else {
|
||
151 | /* Turning right. */
|
||
152 | 648 | emarinel | mr = 200;
|
153 | 523 | emarinel | ml = mr + differential; |
154 | } |
||
155 | |||
156 | int motor1_dir = mr < 0 ? 0 : 1; |
||
157 | int motor2_dir = ml < 0 ? 0 : 1; |
||
158 | |||
159 | motor1_set(motor1_dir, mr); |
||
160 | motor2_set(motor2_dir, ml); |
||
161 | 521 | emarinel | } |
162 | |||
163 | 648 | emarinel | static int get_motor_differential(int e_x, int e_y, int v_x, int v_y) { |
164 | // char buf[80];
|
||
165 | // sprintf(buf,"ex:%d ey:%d vx:%d vy:%d\n", e_x, e_y, v_x, v_y);
|
||
166 | // usb_puts(buf);
|
||
167 | |||
168 | 659 | jknichel | return 250; |
169 | 661 | jknichel | |
170 | 648 | emarinel | /*
|
171 | long e_mag = sqrt_100_approx(e_x*e_x + e_y*e_y);
|
||
172 | long v_mag = sqrt_100_approx(v_x*v_x + v_y*v_y);
|
||
173 | long e_norm_x = (1000 * e_x) / e_mag;
|
||
174 | long e_norm_y = (1000 * e_y) / e_mag;
|
||
175 | long v_norm_x = (1000 * v_x) / v_mag;
|
||
176 | long v_norm_y = (1000 * v_y) / v_mag;
|
||
177 | long e_dot_v = e_norm_x*v_norm_x + e_norm_y*v_norm_y;
|
||
178 | */
|
||
179 | |||
180 | 661 | jknichel | |
181 | 648 | emarinel | } |
182 | |||
183 | 680 | emarinel | static int inside_virtual_wall(int x, int y) { |
184 | if (virtual_wall_available()) {
|
||
185 | if (robot_x < virtual_wall.up_x || robot_x > virtual_wall.low_x || robot_y < virtual_wall.up_y
|
||
186 | || robot_y > virtual_wall.low_y) { |
||
187 | return 0; |
||
188 | } else {
|
||
189 | return 1; |
||
190 | 661 | jknichel | } |
191 | 680 | emarinel | } else {
|
192 | return 1; |
||
193 | } |
||
194 | } |
||
195 | 661 | jknichel | |
196 | |||
197 | 680 | emarinel | /**
|
198 | * Main loop which responds to commands.
|
||
199 | */
|
||
200 | void colonet_run(void (*step)(void)) { |
||
201 | char buf[80]; |
||
202 | 661 | jknichel | |
203 | 680 | emarinel | while (1) { |
204 | wl_do(); |
||
205 | 661 | jknichel | request_abs_position(); |
206 | 680 | emarinel | step(); |
207 | 661 | jknichel | |
208 | 680 | emarinel | switch (robot_state) {
|
209 | case NO_COMMAND:
|
||
210 | break;
|
||
211 | 624 | emarinel | |
212 | 680 | emarinel | case MOVING:
|
213 | if (robot_lost) {
|
||
214 | motors_off(); |
||
215 | robot_state = NO_COMMAND; |
||
216 | } else {
|
||
217 | if (updated_robot_pos_ready) {
|
||
218 | if (!inside_virtual_wall(robot_x, robot_y)) {
|
||
219 | usb_puts("outside of boundary\n");
|
||
220 | motors_off(); |
||
221 | robot_state = NO_COMMAND; |
||
222 | } |
||
223 | 624 | emarinel | |
224 | 680 | emarinel | updated_robot_pos_ready = 0;
|
225 | } |
||
226 | } |
||
227 | break;
|
||
228 | 523 | emarinel | |
229 | 680 | emarinel | case MOVING_TO_POSITION:
|
230 | usb_puts("move to absolute position\n");
|
||
231 | 523 | emarinel | |
232 | 680 | emarinel | if (robot_lost) {
|
233 | motors_off(); |
||
234 | robot_state = NO_COMMAND; |
||
235 | } else {
|
||
236 | if (updated_robot_pos_ready) {
|
||
237 | updated_robot_pos_ready = 0;
|
||
238 | 648 | emarinel | |
239 | 680 | emarinel | if (!inside_virtual_wall(robot_x, robot_y)) {
|
240 | usb_puts("outside of boundary\n");
|
||
241 | motors_off(); |
||
242 | robot_state = NO_COMMAND; |
||
243 | } else {
|
||
244 | int dist = 0; |
||
245 | if ((dist = dist_squared(robot_x, robot_y, target_x, target_y)) < TARGET_POSITION_STOP_DISTANCE_THRESHOLD) {
|
||
246 | // Reached destination.
|
||
247 | motors_off(); |
||
248 | robot_state = NO_COMMAND; |
||
249 | 681 | emarinel | |
250 | // Notify server that robot arrived.
|
||
251 | report_arrived(); |
||
252 | 680 | emarinel | } else {
|
253 | // e is the error vector (where we want to go)
|
||
254 | //char buf[80];
|
||
255 | // sprintf(buf, "target_x:%d target_y:%d robot_x:%d robot_y:%d\n", target_x, target_y, robot_x, robot_y);
|
||
256 | // usb_puts(buf);
|
||
257 | 648 | emarinel | |
258 | 680 | emarinel | int e_x = (int)target_x - (int)robot_x; |
259 | int e_y = (int)target_y - (int)robot_y; |
||
260 | 531 | emarinel | |
261 | 680 | emarinel | // sprintf(buf, "e: (x:%d,y:%d)\n", e_x, e_y);
|
262 | // usb_puts(buf);
|
||
263 | 531 | emarinel | |
264 | 680 | emarinel | // v is the velocity vector (where we just went)
|
265 | int v_x = (int)robot_x - (int)last_x; |
||
266 | int v_y = (int)robot_y - (int)last_y; |
||
267 | 648 | emarinel | |
268 | 680 | emarinel | //sprintf(buf, "v: (x:%d,y:%d)\n", v_x, v_y);
|
269 | //usb_puts(buf);
|
||
270 | 648 | emarinel | |
271 | 680 | emarinel | if (abs(v_x) < 2 && abs(v_x) < 2) { |
272 | // if we didn't move since the last check, just go forward.
|
||
273 | motor1_set(1, 180); |
||
274 | motor2_set(1, 180); |
||
275 | } else {
|
||
276 | int e_dot_v = e_x * v_x + e_y * v_y;
|
||
277 | 648 | emarinel | |
278 | 680 | emarinel | // move according to algortihm
|
279 | int e_mag = e_x*e_x + e_y*e_y;
|
||
280 | // int motor_differential = 180;//e_mag >> 7;
|
||
281 | int motor_differential = get_motor_differential(e_x, e_y, v_x, v_y);
|
||
282 | 661 | jknichel | |
283 | 680 | emarinel | // p is perpendicular to v, directed to the "left" of the robot.
|
284 | int p_x = v_y;
|
||
285 | int p_y = -v_x;
|
||
286 | int dot_product_pe = p_x * e_x + p_y * e_y;
|
||
287 | 521 | emarinel | |
288 | 680 | emarinel | // if the dot product of p and e is negative, we should turn right. otherwise turn left.
|
289 | if (dot_product_pe < 0) { |
||
290 | motor_differential = -motor_differential; |
||
291 | } |
||
292 | 550 | emarinel | |
293 | 680 | emarinel | set_motors_with_differential(motor_differential); |
294 | } |
||
295 | } |
||
296 | } |
||
297 | 648 | emarinel | } |
298 | 644 | gtress | } |
299 | 523 | emarinel | |
300 | 680 | emarinel | break;
|
301 | 648 | emarinel | } |
302 | 524 | emarinel | } |
303 | 680 | emarinel | } |
304 | 550 | emarinel | |
305 | 648 | emarinel | |
306 | 149 | emarinel | /* Private functions */
|
307 | |||
308 | /** @brief Handles colonet packets. Should be called by parse_buffer
|
||
309 | * when it is determined that a colonet message has been received.
|
||
310 | *
|
||
311 | * @param robot_id The robot id
|
||
312 | * @param pkt_buf The packet buffer (e.g. wl_buf)
|
||
313 | *
|
||
314 | * @return -1 on error (invalid msgId), 0 on success
|
||
315 | */
|
||
316 | 424 | emarinel | static void colonet_handle_receive(char type, int wl_source, unsigned char* packet, int length) { |
317 | 623 | emarinel | length = length; |
318 | 624 | emarinel | wl_source = wl_source; |
319 | 623 | emarinel | |
320 | ColonetRobotServerPacket* pkt = (ColonetRobotServerPacket*)packet; |
||
321 | 11 | emarinel | unsigned char* args; //up to 7 char args |
322 | 550 | emarinel | unsigned int_args[3]; //up to 3 int (2-byte) args |
323 | 523 | emarinel | char buf[40]; |
324 | |||
325 | 623 | emarinel | |
326 | 523 | emarinel | /*
|
327 | 174 | emarinel | usb_puts("Packet received.\n");
|
328 | 523 | emarinel | |
329 | 175 | emarinel | sprintf(buf, "length=%d\n", length);
|
330 | |||
331 | int i;
|
||
332 | for (i = 0; i < length; i++) {
|
||
333 | sprintf(buf, "%d: %d ", i, packet[i]);
|
||
334 | 624 | emarinel | usb_puts(buf);
|
335 | 523 | emarinel | }
|
336 | 175 | emarinel | usb_puts("\n");
|
337 | 523 | emarinel | */
|
338 | 175 | emarinel | |
339 | 623 | emarinel | //sprintf(buf, "received message from client_id=%d of length %d\n", pkt->client_id, length);
|
340 | //usb_puts(buf);
|
||
341 | 11 | emarinel | |
342 | 174 | emarinel | ///assert(length == sizeof(ColonetRobotServerPacket));
|
343 | 11 | emarinel | |
344 | 524 | emarinel | /*
|
345 | sprintf(buf, "client_id:%d, msg_code:%d\n", pkt.client_id, pkt.msg_code);
|
||
346 | usb_puts(buf);*/
|
||
347 | 175 | emarinel | |
348 | 524 | emarinel | args = pkt->data; |
349 | 149 | emarinel | |
350 | 11 | emarinel | int_args[0] = two_bytes_to_int(args[0], args[1]); |
351 | int_args[1] = two_bytes_to_int(args[2], args[3]); |
||
352 | int_args[2] = two_bytes_to_int(args[4], args[5]); |
||
353 | |||
354 | 149 | emarinel | if (type == COLONET_REQUEST) {
|
355 | 623 | emarinel | //sprintf(buf, "received colonet request: pkt msgcode is %d\n", pkt->msg_code);
|
356 | //usb_puts(buf);
|
||
357 | 175 | emarinel | |
358 | 524 | emarinel | switch (pkt->msg_code) {
|
359 | 11 | emarinel | //Sharp
|
360 | case READ_DISTANCE:
|
||
361 | break;
|
||
362 | |||
363 | //Analog
|
||
364 | case WHEEL:
|
||
365 | break;
|
||
366 | 424 | emarinel | |
367 | 11 | emarinel | case BATTERY:
|
368 | 524 | emarinel | sprintf((char*)pkt->data, "%d", (int)battery8()); |
369 | 623 | emarinel | // NOTE: wl_send_robot_to_robot_global_packet does not work here!
|
370 | wl_send_global_packet(COLONET_PACKET_GROUP_ID, 0, (char*)pkt, sizeof(ColonetRobotServerPacket), 0); |
||
371 | 11 | emarinel | break;
|
372 | |||
373 | //BOM
|
||
374 | case GETMAXBOM:
|
||
375 | break;
|
||
376 | |||
377 | case DIGITAL_INPUT:
|
||
378 | break;
|
||
379 | } |
||
380 | 149 | emarinel | } else if (type == COLONET_COMMAND) { |
381 | 624 | emarinel | // sprintf(buf, "colonet command ... pkt->msg_code=%d\n", pkt->msg_code);
|
382 | // usb_puts(buf);
|
||
383 | 11 | emarinel | |
384 | 175 | emarinel | /* TODO uncomment this stuff */
|
385 | /* if (pkt.msg_code >= USER_DEFINED_MSG_ID_START && */
|
||
386 | /* pkt.msg_code <= USER_DEFINED_MSG_ID_END) { */
|
||
387 | /* if (user_handlers[pkt.msg_code - USER_DEFINED_MSG_ID_START].handler) { */
|
||
388 | /* /\* Call the user's handler function if it the function's address */
|
||
389 | /* * is non-zero (has been set) *\/ */
|
||
390 | /* user_handlers[pkt.msg_code - USER_DEFINED_MSG_ID_START].handler(); */
|
||
391 | /* } */
|
||
392 | /* } */
|
||
393 | 11 | emarinel | |
394 | 524 | emarinel | switch (pkt->msg_code) {
|
395 | 175 | emarinel | /* default: */
|
396 | /* printf("%s: Error - message code %d not implemented\n", __FUNCTION__, */
|
||
397 | /* pkt.msg_code); */
|
||
398 | /* break; */
|
||
399 | |||
400 | 648 | emarinel | case SERVER_REPORT_ROBOT_LOST:
|
401 | last_x = robot_x; |
||
402 | last_y = robot_y; |
||
403 | |||
404 | 680 | emarinel | robot_lost = 1;
|
405 | |||
406 | 648 | emarinel | usb_puts("lost\n");
|
407 | |||
408 | motors_off(); |
||
409 | break;
|
||
410 | |||
411 | 680 | emarinel | case SERVER_CLEAR_VIRTUAL_WALL:
|
412 | virtual_wall_up_available = 0;
|
||
413 | virtual_wall_low_available = 0;
|
||
414 | break;
|
||
415 | |||
416 | 661 | jknichel | case SERVER_REPORT_VIRTUAL_WALL_UPPER:
|
417 | virtual_wall.up_x = int_args[0];
|
||
418 | virtual_wall.up_y = int_args[1];
|
||
419 | 680 | emarinel | |
420 | virtual_wall_up_available = 1;
|
||
421 | |||
422 | sprintf(buf, "%d %d\n", virtual_wall.up_x, virtual_wall.up_y);
|
||
423 | usb_puts(buf); |
||
424 | 661 | jknichel | break;
|
425 | |||
426 | case SERVER_REPORT_VIRTUAL_WALL_LOWER:
|
||
427 | virtual_wall.low_x = int_args[0];
|
||
428 | virtual_wall.low_y = int_args[1];
|
||
429 | 680 | emarinel | |
430 | sprintf(buf, "%d %d\n", virtual_wall.low_x, virtual_wall.low_y);
|
||
431 | usb_puts(buf); |
||
432 | |||
433 | virtual_wall_low_available = 1;
|
||
434 | 661 | jknichel | break;
|
435 | |||
436 | 433 | emarinel | case SERVER_REPORT_POSITION_TO_ROBOT:
|
437 | 644 | gtress | last_x = robot_x; |
438 | last_y = robot_y; |
||
439 | 550 | emarinel | robot_x = (unsigned)int_args[0]; |
440 | robot_y = (unsigned)int_args[1]; |
||
441 | 508 | emarinel | |
442 | 531 | emarinel | updated_robot_pos_ready = 1;
|
443 | 680 | emarinel | robot_lost = 0;
|
444 | 521 | emarinel | |
445 | 680 | emarinel | // sprintf(buf, "pos is: (x:%d, y:%d)\n", robot_x, robot_y);
|
446 | 648 | emarinel | //usb_puts(buf);
|
447 | 433 | emarinel | break;
|
448 | |||
449 | case MOVE_TO_ABSOLUTE_POSITION:
|
||
450 | 648 | emarinel | sprintf(buf, "move to abs (x:%u, y:%u)\n", (unsigned)int_args[0], (unsigned)int_args[1]); |
451 | usb_puts(buf); |
||
452 | |||
453 | 680 | emarinel | target_x = (unsigned)int_args[0]; |
454 | target_y = (unsigned)int_args[1]; |
||
455 | |||
456 | robot_state = MOVING_TO_POSITION; |
||
457 | 433 | emarinel | break;
|
458 | |||
459 | 11 | emarinel | //Buzzer
|
460 | case BUZZER_INIT:
|
||
461 | buzzer_init(); |
||
462 | break;
|
||
463 | case BUZZER_SET_VAL:
|
||
464 | buzzer_set_val(args[0]);
|
||
465 | break;
|
||
466 | case BUZZER_SET_FREQ:
|
||
467 | buzzer_set_freq(args[0]);
|
||
468 | break;
|
||
469 | case BUZZER_CHIRP:
|
||
470 | buzzer_chirp(int_args[0], int_args[1]); |
||
471 | break;
|
||
472 | case BUZZER_OFF:
|
||
473 | buzzer_off(); |
||
474 | break;
|
||
475 | 624 | emarinel | |
476 | //Orb
|
||
477 | 11 | emarinel | case ORB_INIT:
|
478 | orb_init(); |
||
479 | break;
|
||
480 | case ORB_SET:
|
||
481 | orb_set(args[0], args[1], args[2]); |
||
482 | break;
|
||
483 | case ORB_SET_COLOR:
|
||
484 | orb_set_color(int_args[0]);
|
||
485 | break;
|
||
486 | case ORB_DISABLE:
|
||
487 | orb_disable(); |
||
488 | break;
|
||
489 | case ORB_ENABLE:
|
||
490 | orb_enable(); |
||
491 | break;
|
||
492 | 624 | emarinel | |
493 | 11 | emarinel | //Motors
|
494 | case MOTORS_INIT:
|
||
495 | 175 | emarinel | usb_puts("calling motors_init\n");
|
496 | 11 | emarinel | motors_init(); |
497 | break;
|
||
498 | 661 | jknichel | |
499 | 11 | emarinel | case MOTORS_OFF:
|
500 | 680 | emarinel | robot_state = NO_COMMAND; |
501 | 624 | emarinel | |
502 | 175 | emarinel | usb_puts("calling motors_off\n");
|
503 | 11 | emarinel | motors_off(); |
504 | break;
|
||
505 | 661 | jknichel | |
506 | 11 | emarinel | case MOVE:
|
507 | 680 | emarinel | if (args[1] == 0 && args[3] == 0) { |
508 | motors_off(); |
||
509 | robot_state = MOVING; |
||
510 | 661 | jknichel | |
511 | 680 | emarinel | usb_puts("stopped\n");
|
512 | } else {
|
||
513 | /* format for move: left direction, left velocity, right direction, right velocity */
|
||
514 | motor1_set(args[0], args[1]); |
||
515 | motor2_set(args[2], args[3]); |
||
516 | |||
517 | /* Loop and update position for virtual wall. */
|
||
518 | robot_state = MOVING; |
||
519 | } |
||
520 | 11 | emarinel | break;
|
521 | 661 | jknichel | |
522 | 11 | emarinel | case PRINTF:
|
523 | 624 | emarinel | usb_puts((char*)pkt->data);
|
524 | 11 | emarinel | break;
|
525 | 680 | emarinel | |
526 | 11 | emarinel | case KILL_ROBOT:
|
527 | 680 | emarinel | robot_state = NO_COMMAND; |
528 | 624 | emarinel | |
529 | 11 | emarinel | motors_off(); |
530 | buzzer_off(); |
||
531 | exit(0); //kill the robot |
||
532 | break;
|
||
533 | 524 | emarinel | //Time
|
534 | 11 | emarinel | case DELAY_MS:
|
535 | delay_ms(int_args[0]);
|
||
536 | break;
|
||
537 | |||
538 | //Analog
|
||
539 | case ANALOG_INIT:
|
||
540 | 344 | gtress | //TODO: how do we really init the analog? this doesn't work:
|
541 | //analog_init();
|
||
542 | 11 | emarinel | break;
|
543 | |||
544 | //BOM
|
||
545 | case BOM_ON:
|
||
546 | bom_on(); |
||
547 | break;
|
||
548 | case BOM_OFF:
|
||
549 | bom_off(); |
||
550 | break;
|
||
551 | |||
552 | 424 | emarinel | //Dio
|
553 | 11 | emarinel | case DIGITAL_OUTPUT:
|
554 | digital_output(int_args[0], int_args[1]); |
||
555 | break;
|
||
556 | } |
||
557 | 149 | emarinel | } else {
|
558 | 175 | emarinel | sprintf(buf, "%s: Error: Invalid colonet message type", __FUNCTION__);
|
559 | usb_puts(buf); |
||
560 | 11 | emarinel | } |
561 | } |