Statistics
| Revision:

root / trunk / code / projects / colonet / robot / colonet_dragonfly / colonet_dragonfly.c @ 681

History | View | Annotate | Download (14.4 KB)

1
/** @file colonet.c
2
 * @brief Colonet library for DRAGONFLY colony robots
3
 *
4
 * @author Eugene Marinelli
5
 * @date 10/10/07
6
 *
7
 * @bug Handler registration not tested
8
 * @bug Request reponding not implemented - only accepts commands.
9
 */
10

    
11
#include <assert.h>
12
#include <battery.h>
13
#include <colonet_defs.h>
14
#include <colonet_dragonfly.h>
15
#include <dragonfly_lib.h>
16
#include <math.h>
17
#include <string.h>
18
#include <wireless.h>
19

    
20
#define TARGET_POSITION_STOP_DISTANCE_THRESHOLD (550)
21

    
22
typedef struct {
23
  unsigned char msgId; //is this necessary?
24
  void (*handler)(void);
25
} UserHandler;
26

    
27
typedef struct {
28
  int up_x;
29
  int up_y;
30
  int low_x;
31
  int low_y;
32
} VirtualWall;
33

    
34
/* Globals (internal) */
35
static UserHandler user_handlers[USER_DEFINED_MSG_TOTAL];
36

    
37
static volatile unsigned robot_x, robot_y;
38
static volatile unsigned last_x, last_y;
39
static volatile unsigned target_x, target_y;
40
static volatile int updated_robot_pos_ready;
41
static volatile int robot_lost; //set to 1 if robot's position is not known by the server, else 0.
42
static volatile VirtualWall virtual_wall;
43

    
44
typedef enum {MOVING, MOVING_TO_POSITION, NO_COMMAND} ColonetRobotCommandState;
45
static volatile ColonetRobotCommandState robot_state;
46

    
47
static volatile int virtual_wall_up_available, virtual_wall_low_available;
48

    
49
#define ERROR_MAG_TO_MOTOR_DIFFENTIAL_CONST (1)
50

    
51
/* Internal function prototypes */
52
static unsigned two_bytes_to_int(unsigned char high, unsigned char low);
53
static void colonet_handle_receive(char type, int source, unsigned char* packet, int length);
54
static void move_to_position_routine(unsigned x, unsigned y);
55
static int dist_squared(int x1, int y1, int x2, int y2);
56
static void move_routine(void);
57

    
58
static PacketGroupHandler colonet_pgh;
59

    
60
/* 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
static int virtual_wall_available() {
68
  return virtual_wall_up_available && virtual_wall_low_available;
69
}
70

    
71
/* Public functions */
72
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
  robot_x = last_x = 0;
83
  robot_y = last_y = 0;
84
  updated_robot_pos_ready = 0;
85
  robot_lost = 1;
86

    
87
  virtual_wall_up_available = 0;
88
  virtual_wall_low_available = 0;
89

    
90
  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
  robot_state = NO_COMMAND;
96

    
97
  return 0;
98
}
99

    
100
/* 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
void get_absolute_position(int* x, int* y) {
116
  *x = robot_x;
117
  *y = robot_y;
118
}
119

    
120
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
void request_abs_position() {
129
  //usb_puts("requesting_abs_position\n");
130
  ColonetRobotServerPacket pkt;
131
  pkt.client_id = -1; //global
132
  pkt.msg_code = ROBOT_REQUEST_POSITION_FROM_SERVER;
133
  wl_send_global_packet(colonet_pgh.groupCode, 0, (char*)&pkt, sizeof(ColonetRobotServerPacket), 0);
134
}
135

    
136
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
}
141

    
142
/** 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
    ml = 200;
149
    mr = ml - differential;
150
  } else {
151
    /* Turning right. */
152
    mr = 200;
153
    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
}
162

    
163
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
  return 250;
169

    
170
  /*
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

    
181
}
182

    
183
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
    }
191
  } else {
192
    return 1;
193
  }
194
}
195

    
196

    
197
/**
198
 * Main loop which responds to commands.
199
 */
200
void colonet_run(void (*step)(void)) {
201
  char buf[80];
202

    
203
  while (1) {
204
    wl_do();
205
    request_abs_position();
206
    step();
207

    
208
    switch (robot_state) {
209
    case NO_COMMAND:
210
      break;
211

    
212
    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

    
224
          updated_robot_pos_ready = 0;
225
        }
226
      }
227
      break;
228

    
229
    case MOVING_TO_POSITION:
230
      usb_puts("move to absolute position\n");
231

    
232
      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

    
239
          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

    
250
              // Notify server that robot arrived.
251
              report_arrived();
252
            } 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

    
258
              int e_x = (int)target_x - (int)robot_x;
259
              int e_y = (int)target_y - (int)robot_y;
260

    
261
              //                sprintf(buf, "e: (x:%d,y:%d)\n", e_x, e_y);
262
              //                usb_puts(buf);
263

    
264
              // 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

    
268
              //sprintf(buf, "v: (x:%d,y:%d)\n", v_x, v_y);
269
              //usb_puts(buf);
270

    
271
              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

    
278
                // 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

    
283
                // 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

    
288
                // 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

    
293
                set_motors_with_differential(motor_differential);
294
              }
295
            }
296
          }
297
        }
298
      }
299

    
300
      break;
301
    }
302
  }
303
}
304

    
305

    
306
/* 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
static void colonet_handle_receive(char type, int wl_source, unsigned char* packet, int length) {
317
  length = length;
318
  wl_source = wl_source;
319

    
320
  ColonetRobotServerPacket* pkt = (ColonetRobotServerPacket*)packet;
321
  unsigned char* args; //up to 7 char args
322
  unsigned int_args[3]; //up to 3 int (2-byte) args
323
  char buf[40];
324

    
325

    
326
  /*
327
  usb_puts("Packet received.\n");
328

329
  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
    usb_puts(buf);
335
 }
336
  usb_puts("\n");
337
  */
338

    
339
  //sprintf(buf, "received message from client_id=%d of length %d\n", pkt->client_id, length);
340
  //usb_puts(buf);
341

    
342
  ///assert(length == sizeof(ColonetRobotServerPacket));
343

    
344
  /*
345
  sprintf(buf, "client_id:%d, msg_code:%d\n", pkt.client_id, pkt.msg_code);
346
  usb_puts(buf);*/
347

    
348
  args = pkt->data;
349

    
350
  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
  if (type == COLONET_REQUEST) {
355
    //sprintf(buf, "received colonet request: pkt msgcode is %d\n", pkt->msg_code);
356
    //usb_puts(buf);
357

    
358
    switch (pkt->msg_code) {
359
      //Sharp
360
    case READ_DISTANCE:
361
      break;
362

    
363
      //Analog
364
    case WHEEL:
365
      break;
366

    
367
    case BATTERY:
368
      sprintf((char*)pkt->data, "%d", (int)battery8());
369
      // 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
      break;
372

    
373
      //BOM
374
    case GETMAXBOM:
375
      break;
376

    
377
    case DIGITAL_INPUT:
378
      break;
379
    }
380
  } else if (type == COLONET_COMMAND) {
381
    // sprintf(buf, "colonet command ... pkt->msg_code=%d\n", pkt->msg_code);
382
    // usb_puts(buf);
383

    
384
/* 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

    
394
    switch (pkt->msg_code) {
395
/*     default: */
396
/*       printf("%s: Error - message code %d not implemented\n", __FUNCTION__, */
397
/*              pkt.msg_code); */
398
/*       break; */
399

    
400
    case SERVER_REPORT_ROBOT_LOST:
401
      last_x = robot_x;
402
      last_y = robot_y;
403

    
404
      robot_lost = 1;
405

    
406
      usb_puts("lost\n");
407

    
408
      motors_off();
409
      break;
410

    
411
    case SERVER_CLEAR_VIRTUAL_WALL:
412
      virtual_wall_up_available = 0;
413
      virtual_wall_low_available = 0;
414
      break;
415

    
416
    case SERVER_REPORT_VIRTUAL_WALL_UPPER:
417
      virtual_wall.up_x = int_args[0];
418
      virtual_wall.up_y = int_args[1];
419

    
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
      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

    
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
      break;
435

    
436
    case SERVER_REPORT_POSITION_TO_ROBOT:
437
      last_x = robot_x;
438
      last_y = robot_y;
439
      robot_x = (unsigned)int_args[0];
440
      robot_y = (unsigned)int_args[1];
441

    
442
      updated_robot_pos_ready = 1;
443
      robot_lost = 0;
444

    
445
      //      sprintf(buf, "pos is: (x:%d, y:%d)\n", robot_x, robot_y);
446
      //usb_puts(buf);
447
      break;
448

    
449
    case MOVE_TO_ABSOLUTE_POSITION:
450
                        sprintf(buf, "move to abs (x:%u, y:%u)\n", (unsigned)int_args[0], (unsigned)int_args[1]);
451
                        usb_puts(buf);
452

    
453
      target_x = (unsigned)int_args[0];
454
      target_y = (unsigned)int_args[1];
455

    
456
      robot_state = MOVING_TO_POSITION;
457
      break;
458

    
459
      //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

    
476
      //Orb
477
    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

    
493
      //Motors
494
    case MOTORS_INIT:
495
      usb_puts("calling motors_init\n");
496
      motors_init();
497
      break;
498

    
499
    case MOTORS_OFF:
500
      robot_state = NO_COMMAND;
501

    
502
      usb_puts("calling motors_off\n");
503
      motors_off();
504
      break;
505

    
506
    case MOVE:
507
      if (args[1] == 0 && args[3] == 0) {
508
        motors_off();
509
        robot_state = MOVING;
510

    
511
        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
      break;
521

    
522
    case PRINTF:
523
      usb_puts((char*)pkt->data);
524
      break;
525

    
526
    case KILL_ROBOT:
527
      robot_state = NO_COMMAND;
528

    
529
      motors_off();
530
      buzzer_off();
531
      exit(0); //kill the robot
532
      break;
533
    //Time
534
    case DELAY_MS:
535
      delay_ms(int_args[0]);
536
      break;
537

    
538
      //Analog
539
    case ANALOG_INIT:
540
      //TODO: how do we really init the analog? this doesn't work:
541
      //analog_init();
542
      break;
543

    
544
      //BOM
545
    case BOM_ON:
546
      bom_on();
547
      break;
548
    case BOM_OFF:
549
      bom_off();
550
      break;
551

    
552
    //Dio
553
    case DIGITAL_OUTPUT:
554
      digital_output(int_args[0], int_args[1]);
555
      break;
556
    }
557
  } else {
558
    sprintf(buf, "%s: Error: Invalid colonet message type", __FUNCTION__);
559
    usb_puts(buf);
560
  }
561
}