root / trunk / code / projects / rangefinders / IRcycle_move / testIRcycle.c @ 1455
History | View | Annotate | Download (1.92 KB)
1 |
#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 |
} |