Revision 680

View differences:

trunk/code/projects/colonet/robot/colonet_dragonfly/colonet_dragonfly.c
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();
trunk/code/projects/colonet/robot/colonet_dragonfly/colonet_dragonfly.h
13 13
 */
14 14
int colonet_init(void);
15 15

  
16
/**
17
 */
18
void colonet_run(void (*step)(void));
19

  
16 20
/** @brief Registers a new colonet message handler function.  If a message
17 21
 * with msgId is received, then handler will be called.
18 22
 *
trunk/code/projects/colonet/robot/robot_slave/Makefile
15 15

  
16 16
# com1 = serial port. Use lpt1 to connect to parallel port.
17 17
#AVRDUDE_PORT = /dev/ttyUSB1
18
AVRDUDE_PORT = /dev/ttyUSB0
19
#AVRDUDE_PORT = /dev/ttyUSB2
18
#AVRDUDE_PORT = /dev/ttyUSB0
19
AVRDUDE_PORT = /dev/ttyUSB2
20 20

  
21 21
#
22 22
#
trunk/code/projects/colonet/robot/robot_slave/robot_slave.c
14 14

  
15 15
int behavior_status;
16 16

  
17
static void step() {}
18

  
17 19
int main(void) {
18 20
  dragonfly_init(ALL_ON);
19 21

  
......
28 30
  if (wl_token_ring_join() == 0) {
29 31
    orb_set_color(GREEN);
30 32

  
31
    while(1) {
32
      wl_do();
33
    }
33
    colonet_run(step);
34 34
  } else {
35 35
    orb_set_color(RED);
36 36
  }
37 37

  
38
  usb_puts("Failed to join token ring.\n");
39

  
38 40
  while(1);
39 41

  
40 42
  return 0;
trunk/code/projects/colonet/common/colonet_defs.h
156 156
#define SERVER_REPORT_VIRTUAL_WALL_UPPER 0x5A
157 157
#define SERVER_REPORT_VIRTUAL_WALL_LOWER 0x5B
158 158

  
159
#define SERVER_CLEAR_VIRTUAL_WALL 0x5C
160

  
159 161
///////////////////////////
160 162
//Colonet specific commands
161 163
//////////////////////////

Also available in: Unified diff