Revision 1943

View differences:

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