Statistics
| Revision:

root / trunk / code / projects / calibration_platform / robot / cal_sta_robot.c @ 1943

History | View | Annotate | Download (1.43 KB)

1
#include <dragonfly_lib.h>
2
#include <wl_basic.h>
3

    
4
#define TIME_DELAY 2000
5

    
6
int main (void) {
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

    
13
        /* Initialize the dragonfly boards, the xbee, and the encoders */
14
        dragonfly_init(ALL_ON);
15
        xbee_init();
16
        encoders_init();
17
        
18
        while (1) {
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");
22

    
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

    
50
                delay_ms(TIME_DELAY);
51
        }
52

    
53
}