root / trunk / code / projects / rangefinders / IRcycle_move / testIRcycle.c @ 1455
History | View | Annotate | Download (1.92 KB)
1 | 1455 | dshope | #include <dragonfly_lib.h> |
---|---|---|---|
2 | #include <encoders.h> |
||
3 | |||
4 | int main(void) |
||
5 | { |
||
6 | /* initialize components, set wireless channel */
|
||
7 | dragonfly_init(ALL_ON); |
||
8 | encoders_init(); |
||
9 | |||
10 | //Replace the next two lines with your own code
|
||
11 | //You can add as many lines as you want
|
||
12 | |||
13 | uint8_t rangeNumber[] = {IR1,IR2,IR3,IR4,IR5}; //0 indexed
|
||
14 | uint8_t cIndex; |
||
15 | |||
16 | //initial setup (IR1)
|
||
17 | cIndex=1;
|
||
18 | orb1_set_color(BLUE); |
||
19 | orb2_set_color(GREEN); |
||
20 | |||
21 | int lastDir = FORWARD;
|
||
22 | int travel=0; |
||
23 | encoder_rst_dx(LEFT); |
||
24 | encoder_rst_dx(RIGHT); |
||
25 | |||
26 | while (1) { |
||
27 | |||
28 | if (lastDir == FORWARD) {
|
||
29 | lastDir=BACKWARD; |
||
30 | motor_l_set(lastDir,200);
|
||
31 | motor_r_set(lastDir,200);
|
||
32 | } else {
|
||
33 | lastDir=FORWARD; |
||
34 | motor_l_set(lastDir,200);
|
||
35 | motor_r_set(lastDir,200);
|
||
36 | } |
||
37 | |||
38 | while(travel<660) { |
||
39 | |||
40 | usb_puti(range_read_distance(rangeNumber[cIndex])); |
||
41 | usb_putc('\r');
|
||
42 | |||
43 | /**
|
||
44 | *if (button1_click()) {
|
||
45 | * switch (cIndex){ //button1 chooses a color channel for "editing"
|
||
46 | * case 0: //IR1
|
||
47 | * orb1_set_color(RED);
|
||
48 | * orb2_set_color(RED);
|
||
49 | * cIndex=1;
|
||
50 | * break;
|
||
51 | * case 1: //IR2
|
||
52 | *
|
||
53 | * orb1_set_color(GREEN);
|
||
54 | * orb2_set_color(GREEN);
|
||
55 | * cIndex=2;
|
||
56 | * break;
|
||
57 | * case 2: //IR3
|
||
58 | *
|
||
59 | * orb1_set_color(BLUE);
|
||
60 | * orb2_set_color(BLUE);
|
||
61 | * cIndex=3;
|
||
62 | * break;
|
||
63 | * case 3: //IR4
|
||
64 | * orb1_set_color(YELLOW);
|
||
65 | * orb2_set_color(YELLOW);
|
||
66 | * cIndex=4;
|
||
67 | * break;
|
||
68 | * case 4: //IR5
|
||
69 | * orb1_set_color(BLUE);
|
||
70 | * orb2_set_color(GREEN);
|
||
71 | * cIndex=0;
|
||
72 | * break;
|
||
73 | * default: //undefined
|
||
74 | * return 0;
|
||
75 | * break;
|
||
76 | * }
|
||
77 | * delay_ms(50); //allows button press to release
|
||
78 | *}
|
||
79 | */
|
||
80 | |||
81 | delay_ms(50); //refresh frequency: 1000/(50ms) refresh rate |
||
82 | |||
83 | travel=abs((encoder_get_dx(LEFT)+encoder_get_dx(RIGHT))/2);
|
||
84 | } |
||
85 | |||
86 | //reset travel variables
|
||
87 | travel=0;
|
||
88 | encoder_rst_dx(LEFT); |
||
89 | encoder_rst_dx(RIGHT); |
||
90 | |||
91 | } |
||
92 | |||
93 | //this tell the robot to just chill out forever. don't put anything after this
|
||
94 | while(1); |
||
95 | return 0; |
||
96 | } |