Statistics
| Revision:

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

History | View | Annotate | Download (14 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
void request_abs_position() {
121
  //usb_puts("requesting_abs_position\n");
122
  ColonetRobotServerPacket pkt;
123
  pkt.client_id = -1;
124
  pkt.msg_code = ROBOT_REQUEST_POSITION_FROM_SERVER;
125
  wl_send_global_packet(colonet_pgh.groupCode, 0, (char*)&pkt, sizeof(ColonetRobotServerPacket), 0);
126
}
127

    
128
static int dist_squared(int x1, int y1, int x2, int y2) {
129
  int dx = x1 - x2;
130
  int dy = y1 - y2;
131
  return dx*dx + dy*dy;
132
}
133

    
134
/** Differential is the intended left motor speed - right motor speed. */
135
static void set_motors_with_differential(int differential) {
136
  int ml, mr;
137

    
138
  if (differential >= 0) {
139
    /* Going left or straight. */
140
    ml = 200;
141
    mr = ml - differential;
142
  } else {
143
    /* Turning right. */
144
    mr = 200;
145
    ml = mr + differential;
146
  }
147

    
148
  int motor1_dir = mr < 0 ? 0 : 1;
149
  int motor2_dir = ml < 0 ? 0 : 1;
150

    
151
  motor1_set(motor1_dir, mr);
152
  motor2_set(motor2_dir, ml);
153
}
154

    
155
static int get_motor_differential(int e_x, int e_y, int v_x, int v_y) {
156
  //  char buf[80];
157
  //  sprintf(buf,"ex:%d ey:%d vx:%d vy:%d\n", e_x, e_y, v_x, v_y);
158
  //  usb_puts(buf);
159

    
160
  return 250;
161

    
162
  /*
163
  long e_mag = sqrt_100_approx(e_x*e_x + e_y*e_y);
164
  long v_mag = sqrt_100_approx(v_x*v_x + v_y*v_y);
165
  long e_norm_x = (1000 * e_x) / e_mag;
166
  long e_norm_y = (1000 * e_y) / e_mag;
167
  long v_norm_x = (1000 * v_x) / v_mag;
168
  long v_norm_y = (1000 * v_y) / v_mag;
169
  long e_dot_v = e_norm_x*v_norm_x + e_norm_y*v_norm_y;
170
  */
171

    
172

    
173
}
174

    
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;
182
    }
183
  } else {
184
    return 1;
185
  }
186
}
187

    
188

    
189
/**
190
 * Main loop which responds to commands.
191
 */
192
void colonet_run(void (*step)(void)) {
193
  char buf[80];
194

    
195
  while (1) {
196
    wl_do();
197
    request_abs_position();
198
    step();
199

    
200
    switch (robot_state) {
201
    case NO_COMMAND:
202
      break;
203

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

    
216
          updated_robot_pos_ready = 0;
217
        }
218
      }
219
      break;
220

    
221
    case MOVING_TO_POSITION:
222
      usb_puts("move to absolute position\n");
223

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

    
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);
246

    
247
              int e_x = (int)target_x - (int)robot_x;
248
              int e_y = (int)target_y - (int)robot_y;
249

    
250
              //                sprintf(buf, "e: (x:%d,y:%d)\n", e_x, e_y);
251
              //                usb_puts(buf);
252

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

    
257
              //sprintf(buf, "v: (x:%d,y:%d)\n", v_x, v_y);
258
              //usb_puts(buf);
259

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

    
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);
271

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

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

    
282
                set_motors_with_differential(motor_differential);
283
              }
284
            }
285
          }
286
        }
287
      }
288

    
289
      break;
290
    }
291
  }
292
}
293

    
294

    
295
/* Private functions */
296

    
297
/** @brief Handles colonet packets.  Should be called by parse_buffer
298
 * when it is determined that a colonet message has been received.
299
 *
300
 * @param robot_id The robot id
301
 * @param pkt_buf The packet buffer (e.g. wl_buf)
302
 *
303
 * @return -1 on error (invalid msgId), 0 on success
304
 */
305
static void colonet_handle_receive(char type, int wl_source, unsigned char* packet, int length) {
306
  length = length;
307
  wl_source = wl_source;
308

    
309
  ColonetRobotServerPacket* pkt = (ColonetRobotServerPacket*)packet;
310
  unsigned char* args; //up to 7 char args
311
  unsigned int_args[3]; //up to 3 int (2-byte) args
312
  char buf[40];
313

    
314

    
315
  /*
316
  usb_puts("Packet received.\n");
317

318
  sprintf(buf, "length=%d\n", length);
319

320
  int i;
321
  for (i = 0; i < length; i++) {
322
    sprintf(buf, "%d: %d ", i, packet[i]);
323
    usb_puts(buf);
324
 }
325
  usb_puts("\n");
326
  */
327

    
328
  //sprintf(buf, "received message from client_id=%d of length %d\n", pkt->client_id, length);
329
  //usb_puts(buf);
330

    
331
  ///assert(length == sizeof(ColonetRobotServerPacket));
332

    
333
  /*
334
  sprintf(buf, "client_id:%d, msg_code:%d\n", pkt.client_id, pkt.msg_code);
335
  usb_puts(buf);*/
336

    
337
  args = pkt->data;
338

    
339
  int_args[0] = two_bytes_to_int(args[0], args[1]);
340
  int_args[1] = two_bytes_to_int(args[2], args[3]);
341
  int_args[2] = two_bytes_to_int(args[4], args[5]);
342

    
343
  if (type == COLONET_REQUEST) {
344
    //sprintf(buf, "received colonet request: pkt msgcode is %d\n", pkt->msg_code);
345
    //usb_puts(buf);
346

    
347
    switch (pkt->msg_code) {
348
      //Sharp
349
    case READ_DISTANCE:
350
      break;
351

    
352
      //Analog
353
    case WHEEL:
354
      break;
355

    
356
    case BATTERY:
357
      sprintf((char*)pkt->data, "%d", (int)battery8());
358
      // NOTE: wl_send_robot_to_robot_global_packet does not work here!
359
      wl_send_global_packet(COLONET_PACKET_GROUP_ID, 0, (char*)pkt, sizeof(ColonetRobotServerPacket), 0);
360
      break;
361

    
362
      //BOM
363
    case GETMAXBOM:
364
      break;
365

    
366
    case DIGITAL_INPUT:
367
      break;
368
    }
369
  } else if (type == COLONET_COMMAND) {
370
    // sprintf(buf, "colonet command ... pkt->msg_code=%d\n", pkt->msg_code);
371
    // usb_puts(buf);
372

    
373
/* TODO uncomment this stuff */
374
/*     if (pkt.msg_code >= USER_DEFINED_MSG_ID_START && */
375
/*         pkt.msg_code <= USER_DEFINED_MSG_ID_END) { */
376
/*       if (user_handlers[pkt.msg_code - USER_DEFINED_MSG_ID_START].handler) { */
377
/*         /\* Call the user's handler function if it the function's address */
378
/*          * is non-zero (has been set) *\/ */
379
/*         user_handlers[pkt.msg_code - USER_DEFINED_MSG_ID_START].handler(); */
380
/*       } */
381
/*     } */
382

    
383
    switch (pkt->msg_code) {
384
/*     default: */
385
/*       printf("%s: Error - message code %d not implemented\n", __FUNCTION__, */
386
/*              pkt.msg_code); */
387
/*       break; */
388

    
389
    case SERVER_REPORT_ROBOT_LOST:
390
      last_x = robot_x;
391
      last_y = robot_y;
392

    
393
      robot_lost = 1;
394

    
395
      usb_puts("lost\n");
396

    
397
      motors_off();
398
      break;
399

    
400
    case SERVER_CLEAR_VIRTUAL_WALL:
401
      virtual_wall_up_available = 0;
402
      virtual_wall_low_available = 0;
403
      break;
404

    
405
    case SERVER_REPORT_VIRTUAL_WALL_UPPER:
406
      virtual_wall.up_x = int_args[0];
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);
413
      break;
414

    
415
    case SERVER_REPORT_VIRTUAL_WALL_LOWER:
416
      virtual_wall.low_x = int_args[0];
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;
423
      break;
424

    
425
    case SERVER_REPORT_POSITION_TO_ROBOT:
426
      last_x = robot_x;
427
      last_y = robot_y;
428
      robot_x = (unsigned)int_args[0];
429
      robot_y = (unsigned)int_args[1];
430

    
431
      updated_robot_pos_ready = 1;
432
      robot_lost = 0;
433

    
434
      //      sprintf(buf, "pos is: (x:%d, y:%d)\n", robot_x, robot_y);
435
      //usb_puts(buf);
436
      break;
437

    
438
    case MOVE_TO_ABSOLUTE_POSITION:
439
                        sprintf(buf, "move to abs (x:%u, y:%u)\n", (unsigned)int_args[0], (unsigned)int_args[1]);
440
                        usb_puts(buf);
441

    
442
      target_x = (unsigned)int_args[0];
443
      target_y = (unsigned)int_args[1];
444

    
445
      robot_state = MOVING_TO_POSITION;
446
      break;
447

    
448
      //Buzzer
449
    case BUZZER_INIT:
450
      buzzer_init();
451
      break;
452
    case BUZZER_SET_VAL:
453
      buzzer_set_val(args[0]);
454
      break;
455
    case BUZZER_SET_FREQ:
456
      buzzer_set_freq(args[0]);
457
      break;
458
    case BUZZER_CHIRP:
459
      buzzer_chirp(int_args[0], int_args[1]);
460
      break;
461
    case BUZZER_OFF:
462
      buzzer_off();
463
      break;
464

    
465
      //Orb
466
    case ORB_INIT:
467
      orb_init();
468
      break;
469
    case ORB_SET:
470
      orb_set(args[0], args[1], args[2]);
471
      break;
472
    case ORB_SET_COLOR:
473
      orb_set_color(int_args[0]);
474
      break;
475
    case ORB_DISABLE:
476
      orb_disable();
477
      break;
478
    case ORB_ENABLE:
479
      orb_enable();
480
      break;
481

    
482
      //Motors
483
    case MOTORS_INIT:
484
      usb_puts("calling motors_init\n");
485
      motors_init();
486
      break;
487

    
488
    case MOTORS_OFF:
489
      robot_state = NO_COMMAND;
490

    
491
      usb_puts("calling motors_off\n");
492
      motors_off();
493
      break;
494

    
495
    case MOVE:
496
      if (args[1] == 0 && args[3] == 0) {
497
        motors_off();
498
        robot_state = MOVING;
499

    
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
      }
509
      break;
510

    
511
    case PRINTF:
512
      usb_puts((char*)pkt->data);
513
      break;
514

    
515
    case KILL_ROBOT:
516
      robot_state = NO_COMMAND;
517

    
518
      motors_off();
519
      buzzer_off();
520
      exit(0); //kill the robot
521
      break;
522
    //Time
523
    case DELAY_MS:
524
      delay_ms(int_args[0]);
525
      break;
526

    
527
      //Analog
528
    case ANALOG_INIT:
529
      //TODO: how do we really init the analog? this doesn't work:
530
      //analog_init();
531
      break;
532

    
533
      //BOM
534
    case BOM_ON:
535
      bom_on();
536
      break;
537
    case BOM_OFF:
538
      bom_off();
539
      break;
540

    
541
    //Dio
542
    case DIGITAL_OUTPUT:
543
      digital_output(int_args[0], int_args[1]);
544
      break;
545
    }
546
  } else {
547
    sprintf(buf, "%s: Error: Invalid colonet message type", __FUNCTION__);
548
    usb_puts(buf);
549
  }
550
}