Revision 1943
Updated calibration station robot code to print values for more sensors.
trunk/code/projects/calibration_platform/robot/cal_sta_robot.c | ||
---|---|---|
1 |
/* |
|
2 |
* template.c - A starting point for developing behaviors using the Colony |
|
3 |
* robots. To create a new behavior, you should copy this "template" |
|
4 |
* folder to another folder and rename the "template.c" file |
|
5 |
* appropriately. |
|
6 |
* |
|
7 |
* This template will have the robot drive in circles and flash the orbs. |
|
8 |
* |
|
9 |
* Author: John Sexton, Colony Project, CMU Robotics Club |
|
10 |
*/ |
|
11 |
|
|
12 | 1 |
#include <dragonfly_lib.h> |
13 | 2 |
#include <wl_basic.h> |
14 | 3 |
|
15 |
/* Time delay which determines how long the robot circles before it |
|
16 |
* changes direction. */ |
|
17 |
#define TIME_DELAY 1000 |
|
4 |
#define TIME_DELAY 2000 |
|
18 | 5 |
|
19 | 6 |
int main (void) { |
20 | 7 |
|
8 |
int rangefinders[5] = {0, 0, 0, 0, 0}; |
|
9 |
int BOMs[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; |
|
10 |
int encoders[2] = {0, 0}; |
|
11 |
int i; |
|
12 |
|
|
21 | 13 |
/* Initialize the dragonfly boards, the xbee, and the encoders */ |
22 | 14 |
dragonfly_init(ALL_ON); |
23 | 15 |
xbee_init(); |
24 | 16 |
encoders_init(); |
25 | 17 |
|
26 | 18 |
while (1) { |
27 |
/* Drive left, set orbs, and wait */ |
|
28 |
orbs_set_color(RED, GREEN); |
|
29 |
usb_puts("RO:Robot\n"); |
|
30 |
delay_ms(TIME_DELAY); |
|
19 |
// Collect rangefinder data and send it |
|
20 |
rangefinders[0] = range_read_distance(IR1); |
|
21 |
usb_puts("RO:IR1:"); usb_puti(rangefinders[0]); usb_puts("\n"); |
|
31 | 22 |
|
32 |
/* Drive right, change orb colors, and wait */ |
|
33 |
orbs_set_color(PURPLE, BLUE); |
|
34 |
usb_puts("RO:Marvin\n"); |
|
23 |
rangefinders[1] = range_read_distance(IR2); |
|
24 |
usb_puts("RO:IR2:"); usb_puti(rangefinders[1]); usb_puts("\n"); |
|
25 |
|
|
26 |
rangefinders[2] = range_read_distance(IR3); |
|
27 |
usb_puts("RO:IR3:"); usb_puti(rangefinders[2]); usb_puts("\n"); |
|
28 |
|
|
29 |
rangefinders[3] = range_read_distance(IR4); |
|
30 |
usb_puts("RO:IR4:"); usb_puti(rangefinders[3]); usb_puts("\n"); |
|
31 |
|
|
32 |
rangefinders[4] = range_read_distance(IR5); |
|
33 |
usb_puts("RO:IR5:"); usb_puti(rangefinders[4]); usb_puts("\n"); |
|
34 |
|
|
35 |
// Collect BOM data and send it |
|
36 |
bom_refresh(BOM_ALL); |
|
37 |
for (i = 0; i < 16; i++) { |
|
38 |
BOMs[i] = bom_get(i); |
|
39 |
usb_puts("RO:BOM"); usb_puti(i); usb_puts(":"); |
|
40 |
usb_puti(BOMs[i]); usb_puts("\n"); |
|
41 |
} |
|
42 |
|
|
43 |
// Collect encoder data and send it |
|
44 |
encoders[0] = encoder_get_dx(LEFT); |
|
45 |
usb_puts("RO:ENCL:"); usb_puti(encoders[0]); usb_puts("\n"); |
|
46 |
|
|
47 |
encoders[1] = encoder_get_dx(RIGHT); |
|
48 |
usb_puts("RO:ENCR:"); usb_puti(encoders[1]); usb_puts("\n"); |
|
49 |
|
|
35 | 50 |
delay_ms(TIME_DELAY); |
36 | 51 |
} |
37 | 52 |
|
Also available in: Unified diff