Project

General

Profile

Statistics
| Revision:

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

History | View | Annotate | Download (12.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 550 emarinel
#define TARGET_POSITION_STOP_DISTANCE_THRESHOLD (40.0)
21 11 emarinel
22
typedef struct {
23
  unsigned char msgId; //is this necessary?
24
  void (*handler)(void);
25
} UserHandler;
26
27
/* Globals (internal) */
28
static UserHandler user_handlers[USER_DEFINED_MSG_TOTAL];
29
30 550 emarinel
static unsigned robot_x, robot_y;
31 523 emarinel
static volatile int updated_robot_pos_ready;
32 550 emarinel
#define ERROR_MAG_TO_MOTOR_DIFFENTIAL_CONST (1)
33 433 emarinel
34 11 emarinel
/* Internal function prototypes */
35 550 emarinel
static unsigned two_bytes_to_int(unsigned char high, unsigned char low);
36 348 emarinel
static void colonet_handle_receive(char type, int source, unsigned char* packet, int length);
37 550 emarinel
static void move_to_position_routine(unsigned x, unsigned y);
38 11 emarinel
39 149 emarinel
static PacketGroupHandler colonet_pgh;
40
41 550 emarinel
/* two_bytes_to_int(char a, char b)
42
 * Returns int of form [high][low]
43
 */
44
static unsigned int two_bytes_to_int(unsigned char high, unsigned char low) {
45
  return (high<<8) | low;
46
}
47
48 11 emarinel
/* Public functions */
49 149 emarinel
int colonet_init() {
50
  colonet_pgh.groupCode = COLONET_PACKET_GROUP_ID;
51
  colonet_pgh.timeout_handler = NULL;
52
  colonet_pgh.handle_response = NULL;
53
  colonet_pgh.handle_receive = colonet_handle_receive;
54
  colonet_pgh.unregister = NULL;
55
56
  // TODO this should return an error if wl_init has not been called yet.
57
  wl_register_packet_group(&colonet_pgh);
58
59 433 emarinel
  robot_x = 0;
60
  robot_y = 0;
61 531 emarinel
  updated_robot_pos_ready = 0;
62 433 emarinel
63 149 emarinel
  return 0;
64
}
65
66 349 emarinel
/* colonet_add_message
67
 * Adds a user-defined message
68
 */
69
int colonet_add_message(unsigned char msgId, void (*handler)(void)) {
70
  if(msgId < USER_DEFINED_MSG_ID_START  /* || msgId > USER_DEFINED_MSG_ID_END */){
71
    return -1;
72
  }
73
74
  /* Register this function in the array */
75
  user_handlers[msgId-USER_DEFINED_MSG_ID_START].msgId = msgId;
76
  user_handlers[msgId-USER_DEFINED_MSG_ID_START].handler = handler;
77
78
  return 0;
79
}
80
81 519 emarinel
void get_absolute_position(int* x, int* y) {
82
  *x = robot_x;
83
  *y = robot_y;
84 433 emarinel
}
85
86 519 emarinel
void request_abs_position() {
87 550 emarinel
  //usb_puts("requesting_abs_position\n");
88 519 emarinel
89
  ColonetRobotServerPacket pkt;
90
  pkt.client_id = -1;
91
  pkt.msg_code = ROBOT_REQUEST_POSITION_FROM_SERVER;
92
  wl_send_global_packet(colonet_pgh.groupCode, 0, (char*)&pkt, sizeof(pkt), 0);
93
}
94
95 550 emarinel
static float sqrt_approx(float x) {
96
  float x2 = x*x;
97
  float x3 = x*x2;
98
99
  return 0.00014*x3 - 0.0078*x2 + 0.29*x + 0.84;
100 433 emarinel
}
101
102 550 emarinel
static float dist(float x1, float y1, float x2, float y2) {
103
  return sqrt_approx((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1));
104
}
105
106 521 emarinel
/* cubic approximation of arctan. */
107 523 emarinel
/*static void arctan(float t) {
108 521 emarinel
  float t3 = t*t*t;
109
  return -0.039 * t3 + 0.74 * t + 0.00011;
110 523 emarinel
  }*/
111
112
/** Differential is the intended left motor speed - right motor speed. */
113
static void set_motors_with_differential(int differential) {
114
  int ml, mr;
115
116
  if (differential >= 0) {
117
    /* Going left or straight. */
118
    ml = 250;
119
    mr = ml - differential;
120
  } else {
121
    /* Turning right. */
122
    mr = 250;
123
    ml = mr + differential;
124
  }
125
126
  int motor1_dir = mr < 0 ? 0 : 1;
127
  int motor2_dir = ml < 0 ? 0 : 1;
128
129
  motor1_set(motor1_dir, mr);
130
  motor2_set(motor2_dir, ml);
131 521 emarinel
}
132
133 550 emarinel
static void move_to_position_routine(unsigned target_x, unsigned target_y) {
134
  usb_puts("move to absolute position routine!\n");
135
136
  updated_robot_pos_ready = 0;
137
  request_abs_position(); // While we're doing this computation, server can be reporting next position.
138 523 emarinel
139 550 emarinel
  int count = 0;
140
  while (!updated_robot_pos_ready) {
141
    wl_do();
142
    if (count++ == 5000) { // in case the server missed it...
143
      request_abs_position();
144
      count = 0;
145
    }
146
  }
147 523 emarinel
148 550 emarinel
//  usb_puts("got past first loop.\n");
149 521 emarinel
150 550 emarinel
  unsigned last_x = robot_x, last_y = robot_y;
151
152 531 emarinel
  char buf[40];
153 550 emarinel
  //sprintf(buf, "cur dist is %f\n", dist((float)robot_x, (float)robot_y, (float)target_x, (float)target_y));
154
  //usb_puts(buf);
155
  //sprintf(buf, "radius squared is %f\n", TARGET_POSITION_STOP_DISTANCE_THRESHOLD);
156
  //usb_puts(buf);
157 531 emarinel
158 550 emarinel
//  usb_puts("entering while loop.\n");
159
160
  while (dist(robot_x, robot_y, target_x, target_y) > TARGET_POSITION_STOP_DISTANCE_THRESHOLD) {
161
162
//    usb_puts("inside while loop.\n");
163
164
    updated_robot_pos_ready = 0;
165 531 emarinel
    request_abs_position(); // While we're doing this computation, server can be reporting next position.
166
167 550 emarinel
//    usb_puts("after request_abs_position.\n");
168 531 emarinel
169 523 emarinel
    int cur_robot_x = robot_x;
170
    int cur_robot_y = robot_y;
171 521 emarinel
172 550 emarinel
    //usb_puts("after cur_robot_x/y = robot_x/y.\n");
173
174
175 581 jknichel
    int e_x = target_x - cur_robot_x;
176
    int e_y = target_y - cur_robot_y;
177 550 emarinel
178
    int v_x = cur_robot_x - last_x;
179
    int v_y = cur_robot_y - last_y;
180
181
182 581 jknichel
183
    sprintf(buf, "vx:%d vy:%d rx:%d ry:%d ex:%d ey:%d\n", v_x, v_y, e_x, e_y, e_x, e_y);
184 550 emarinel
    usb_puts(buf);
185
186
    int e_mag = e_x*e_x + e_y*e_y;
187
188 581 jknichel
    int motor_differential = e_mag >> 7;// / 128;
189 550 emarinel
190
/*
191 531 emarinel
    sprintf(buf, "Current position: %d %d\n", cur_robot_x, cur_robot_y);
192 526 emarinel
    usb_puts(buf);
193

194 531 emarinel
    sprintf(buf, "Target position: %d %d\n", target_x, target_y);
195
    usb_puts(buf);
196

197 523 emarinel
    float r_x = (float)target_x - cur_robot_x;
198
    float r_y = (float)target_y - cur_robot_y;
199
    float r_mag = sqrt_approx(r_x * r_x + r_y * r_y);
200
    r_x /= r_mag;
201
    r_y /= r_mag;
202 521 emarinel

203 523 emarinel
    float v_x = (float)cur_robot_x - last_x;
204
    float v_y = robot_y - last_y;
205
    float v_mag = sqrt_approx(v_x*v_x + v_y*v_y);
206
    v_x /= v_mag;
207
    v_y /= v_mag;
208 521 emarinel

209 523 emarinel
    float e_x = r_x - v_x;
210
    float e_y = r_y - v_y;
211
    float e_mag = sqrt_approx(e_x * e_x + e_y * e_y);
212 521 emarinel

213 531 emarinel
    sprintf(buf, "Error: <%f,%f>; mag: %f\n", e_x, e_y, e_mag);
214
    usb_puts(buf);
215

216 550 emarinel
    // Motor differential proportional to magnitude of directional error.
217
  //  int motor_differential = (int)(e_mag * ERROR_MAG_TO_MOTOR_DIFFENTIAL_CONST);
218
*/
219
220 521 emarinel
221 550 emarinel
    int p_x = v_y;
222
    int p_y = -v_x;
223
224 581 jknichel
    int e_trans_x = p_x * e_x + p_y * e_y;
225 550 emarinel
226
/*
227
    // Determine left or right by transforming error vector to robot axes
228 523 emarinel
    // Perpendicular
229
    float p_x = v_y;
230
    float p_y = -v_x;
231 521 emarinel

232 550 emarinel
    // Inverse matrix
233 523 emarinel
    float coeff = 1.0 / (p_x * v_y - v_x * p_y);
234
    float mat[2][2];
235
    mat[0][0] = v_y * coeff;
236
    mat[0][1] = -v_x * coeff;
237
    //mat[1][0] = -p_y * coeff;     //Not needed for our purposes.
238
    //mat[1][1] = p_x * coeff;
239

240
    float e_trans_x = mat[0][0] * e_x + mat[0][1] * e_y;
241
    //float e_trans_y = mat[1][0] * e_x + mat[1][1] * e_y;  //Not needed for our purposes.
242 550 emarinel
*/
243 523 emarinel
244 550 emarinel
245 523 emarinel
    if (e_trans_x < 0) {
246
      motor_differential = -motor_differential;
247
    }
248
249 531 emarinel
    sprintf(buf, "motor_diff: %d\n", motor_differential);
250
    usb_puts(buf);
251
252 523 emarinel
    last_x = cur_robot_x;
253
    last_y = cur_robot_y;
254
255
    set_motors_with_differential(motor_differential);
256 550 emarinel
257
    count = 0;
258
    while (!updated_robot_pos_ready) { // ghetto condition variable
259
      wl_do();
260
      if (count++ == 5000) { // in case the server missed it...
261
        request_abs_position();
262
        count = 0;
263
      }
264
    }
265 524 emarinel
  }
266 550 emarinel
267 531 emarinel
  usb_puts("reached destination!\n");
268 521 emarinel
}
269
270 149 emarinel
/* Private functions */
271
272
/** @brief Handles colonet packets.  Should be called by parse_buffer
273
 * when it is determined that a colonet message has been received.
274
 *
275
 * @param robot_id The robot id
276
 * @param pkt_buf The packet buffer (e.g. wl_buf)
277
 *
278
 * @return -1 on error (invalid msgId), 0 on success
279
 */
280 424 emarinel
static void colonet_handle_receive(char type, int wl_source, unsigned char* packet, int length) {
281 524 emarinel
  ColonetRobotServerPacket* pkt= (ColonetRobotServerPacket*)packet;
282 11 emarinel
  unsigned char* args; //up to 7 char args
283 550 emarinel
  unsigned int_args[3]; //up to 3 int (2-byte) args
284 11 emarinel
285 523 emarinel
  char buf[40];
286
287
  length = length;
288
  /*
289 174 emarinel
  usb_puts("Packet received.\n");
290 523 emarinel

291 175 emarinel
  sprintf(buf, "length=%d\n", length);
292

293
  int i;
294
  for (i = 0; i < length; i++) {
295
    sprintf(buf, "%d: %d ", i, packet[i]);
296 523 emarinel
    usb_puts(buf);
297
 }
298 175 emarinel
  usb_puts("\n");
299 523 emarinel
  */
300 175 emarinel
301 174 emarinel
  //printf("received message from %d of length %d\n", source, length);
302 11 emarinel
303 174 emarinel
  ///assert(length == sizeof(ColonetRobotServerPacket));
304 11 emarinel
305 524 emarinel
  /*
306
  sprintf(buf, "client_id:%d, msg_code:%d\n", pkt.client_id, pkt.msg_code);
307
  usb_puts(buf);*/
308 175 emarinel
309 524 emarinel
  args = pkt->data;
310 149 emarinel
311 11 emarinel
  int_args[0] = two_bytes_to_int(args[0], args[1]);
312
  int_args[1] = two_bytes_to_int(args[2], args[3]);
313
  int_args[2] = two_bytes_to_int(args[4], args[5]);
314
315 149 emarinel
  if (type == COLONET_REQUEST) {
316 531 emarinel
    sprintf(buf, "received colonet request: pkt msgcode is %d\n", pkt->msg_code);
317
    usb_puts(buf);
318 175 emarinel
319 524 emarinel
    switch (pkt->msg_code) {
320 11 emarinel
      //Sharp
321
    case READ_DISTANCE:
322
      break;
323
    case LINEARIZE_DISTANCE:
324
      break;
325
    case LOG_DISTANCE:
326
      break;
327
328
      //Analog
329
    case CALL_ANALOG8:
330
      break;
331
    case CALL_ANALOG10:
332
      break;
333
    case WHEEL:
334
      break;
335 424 emarinel
336 11 emarinel
    case BATTERY:
337 574 gtress
      sprintf(buf, "it's a battery request\n");
338
      usb_puts(buf);
339 524 emarinel
      sprintf((char*)pkt->data, "%d", (int)battery8());
340 424 emarinel
      wl_send_robot_to_robot_global_packet(colonet_pgh.groupCode, 0, (char*)&pkt, sizeof(pkt), wl_source, 0);
341 574 gtress
      sprintf(buf, "sent battery value: %s\n", pkt->data);
342
      usb_puts(buf);
343 11 emarinel
      break;
344
345
      //BOM
346
    case GETMAXBOM:
347
      break;
348
349
      //Dio
350
    case DIGITAL_INPUT:
351
      break;
352
    case BUTTON1_READ:
353
      break;
354
    case BUTTON2_READ:
355
      break;
356
357
      //Bumper
358
    case DETECT_BUMP:
359
      break;
360
    }
361 149 emarinel
  } else if (type == COLONET_COMMAND) {
362 531 emarinel
    sprintf(buf, "colonet command ... pkt->msg_code=%d\n", pkt->msg_code);
363
    usb_puts(buf);
364 11 emarinel
365 175 emarinel
/* TODO uncomment this stuff */
366
/*     if (pkt.msg_code >= USER_DEFINED_MSG_ID_START && */
367
/*         pkt.msg_code <= USER_DEFINED_MSG_ID_END) { */
368
/*       if (user_handlers[pkt.msg_code - USER_DEFINED_MSG_ID_START].handler) { */
369
/*         /\* Call the user's handler function if it the function's address */
370
/*          * is non-zero (has been set) *\/ */
371
/*         user_handlers[pkt.msg_code - USER_DEFINED_MSG_ID_START].handler(); */
372
/*       } */
373
/*     } */
374 11 emarinel
375 524 emarinel
    switch (pkt->msg_code) {
376 175 emarinel
/*     default: */
377
/*       printf("%s: Error - message code %d not implemented\n", __FUNCTION__, */
378
/*              pkt.msg_code); */
379
/*       break; */
380
381 433 emarinel
    case SERVER_REPORT_POSITION_TO_ROBOT:
382 550 emarinel
      robot_x = (unsigned)int_args[0];
383
      robot_y = (unsigned)int_args[1];
384 508 emarinel
385 531 emarinel
      updated_robot_pos_ready = 1;
386 521 emarinel
387 550 emarinel
      sprintf(buf, "pos is: %u %u\n", robot_x, robot_y);
388 508 emarinel
      usb_puts(buf);
389 433 emarinel
      break;
390
391
    case MOVE_TO_ABSOLUTE_POSITION:
392 531 emarinel
      usb_puts("move to abs position2!\n");
393 550 emarinel
      move_to_position_routine((unsigned)int_args[0], (unsigned)int_args[1]);
394 433 emarinel
      break;
395
396 11 emarinel
      //Buzzer
397
    case BUZZER_INIT:
398
      buzzer_init();
399
      break;
400
    case BUZZER_SET_VAL:
401
      buzzer_set_val(args[0]);
402
      break;
403
    case BUZZER_SET_FREQ:
404
      buzzer_set_freq(args[0]);
405
      break;
406
    case BUZZER_CHIRP:
407
      buzzer_chirp(int_args[0], int_args[1]);
408
      break;
409
    case BUZZER_OFF:
410
      buzzer_off();
411
      break;
412
    case ORB_INIT:
413
      orb_init();
414
      break;
415 424 emarinel
    //Orb
416 11 emarinel
    case ORB_SET:
417
      orb_set(args[0], args[1], args[2]);
418
      break;
419
    case ORB_SET_COLOR:
420
      orb_set_color(int_args[0]);
421
      break;
422
    case ORB_DISABLE:
423
      orb_disable();
424
      break;
425
    case ORB_ENABLE:
426
      orb_enable();
427
      break;
428
    case LED_INIT:
429
      break;
430
    case LED_USER:
431
      break;
432
      //Motors
433
    case MOTORS_INIT:
434 175 emarinel
      usb_puts("calling motors_init\n");
435 11 emarinel
      motors_init();
436
      break;
437
    case MOTOR1_SET:
438 424 emarinel
      sprintf(buf, "calling motor1_set [%i] [%i]\n", args[0], args[1]);
439 180 gtress
      usb_puts(buf);
440
      motor1_set(args[0], args[1]);
441 11 emarinel
      break;
442
    case MOTOR2_SET:
443 180 gtress
      sprintf(buf, "calling motor2_set [%i] [%i]\n", args[0], args[1]);
444
      usb_puts(buf);
445
      motor2_set(args[0], args[1]);
446 11 emarinel
      break;
447
    case MOTORS_OFF:
448 175 emarinel
      usb_puts("calling motors_off\n");
449 11 emarinel
      motors_off();
450
      break;
451
    case MOVE:
452 180 gtress
      sprintf(buf, "calling move with: %d, %d\n", args[0], args[1]);
453 175 emarinel
      usb_puts(buf);
454
      move(args[0], args[1]);
455 11 emarinel
      break;
456
457
    case USB_INIT:
458
      usb_init();
459
      break;
460
461
    case USB_PUTC:
462
      usb_putc((char)args[0]);
463
      break;
464
465
    case PRINTF:
466 524 emarinel
      printf("%s", pkt->data);
467 11 emarinel
      break;
468
    case KILL_ROBOT:
469
      motors_off();
470
      buzzer_off();
471
      exit(0); //kill the robot
472
      break;
473 524 emarinel
    //Time
474 11 emarinel
    case DELAY_MS:
475
      delay_ms(int_args[0]);
476
      break;
477
478
      //Analog
479
    case ANALOG_INIT:
480 344 gtress
      //TODO: how do we really init the analog? this doesn't work:
481
      //analog_init();
482 11 emarinel
      break;
483
484
      //BOM
485
    case BOM_ON:
486
      bom_on();
487
      break;
488
    case BOM_OFF:
489
      bom_off();
490
      break;
491
492 424 emarinel
    //Dio
493 11 emarinel
    case DIGITAL_OUTPUT:
494
      digital_output(int_args[0], int_args[1]);
495
      break;
496
    }
497 149 emarinel
  } else {
498 175 emarinel
    sprintf(buf, "%s: Error: Invalid colonet message type", __FUNCTION__);
499
    usb_puts(buf);
500 11 emarinel
  }
501
}