root / trunk / code / behaviors / formation_control / circle / circle.c @ 1509
History | View | Annotate | Download (3.3 KB)
1 | 1507 | nparis | #include <dragonfly_lib.h> |
---|---|---|---|
2 | #include <encoders.h> |
||
3 | |||
4 | #define RADIUS 300 |
||
5 | |||
6 | extern void blink(int); |
||
7 | extern void search(void); |
||
8 | extern void approach(void); |
||
9 | extern void lastDrive(void); |
||
10 | |||
11 | int main(void) |
||
12 | { |
||
13 | |||
14 | /* Initialize dragonfly board */
|
||
15 | dragonfly_init(ALL_ON); |
||
16 | encoders_init(); |
||
17 | |||
18 | orb_set_color(GREEN); |
||
19 | delay_ms(300);
|
||
20 | |||
21 | search(); |
||
22 | |||
23 | approach(); |
||
24 | |||
25 | while(1); /* END HERE */ |
||
26 | |||
27 | return 0; |
||
28 | } |
||
29 | |||
30 | |||
31 | |||
32 | void blink(int num) |
||
33 | { |
||
34 | for(int i = 0; i<num; i++) |
||
35 | { |
||
36 | orb_set_color(ORB_OFF); |
||
37 | delay_ms(200);
|
||
38 | orb_set_color(RED); |
||
39 | delay_ms(200);
|
||
40 | } |
||
41 | orb_set_color(ORB_OFF); |
||
42 | } |
||
43 | void search() //makes the robot face the beacon |
||
44 | { |
||
45 | orb_set_color(YELLOW); //show that the robot is searching
|
||
46 | bom_refresh(BOM_ALL); |
||
47 | int top=bom_get_max();
|
||
48 | |||
49 | if ((top<4) || (top>11)) //determine which way to turn |
||
50 | { |
||
51 | motor_r_set(BACKWARD,200);
|
||
52 | motor_l_set(FORWARD,200);
|
||
53 | } |
||
54 | else if((top>4) && (top<12)) |
||
55 | { |
||
56 | motor_r_set(FORWARD,200);
|
||
57 | motor_l_set(BACKWARD,200);
|
||
58 | } |
||
59 | while (top != 4) |
||
60 | { |
||
61 | bom_refresh(BOM_ALL); |
||
62 | top=bom_get_max(); |
||
63 | } |
||
64 | motors_off(); |
||
65 | } |
||
66 | void approach()
|
||
67 | { |
||
68 | int dist = 0; |
||
69 | int temp = 0; |
||
70 | int counter = 0; |
||
71 | int correct = 0; |
||
72 | |||
73 | while(dist != RADIUS) //puts the robots in the given distance RADIUS |
||
74 | { |
||
75 | temp=0;
|
||
76 | counter = 0;
|
||
77 | |||
78 | while(counter<8) |
||
79 | { |
||
80 | if(range_read_distance(IR2) == -1){} |
||
81 | |||
82 | else
|
||
83 | { |
||
84 | temp += range_read_distance(IR2); |
||
85 | counter++; |
||
86 | } |
||
87 | delay_ms(20);
|
||
88 | } |
||
89 | dist = temp/8;
|
||
90 | |||
91 | /*if (correct >2){
|
||
92 | orb_set_color(RED);
|
||
93 | break;
|
||
94 | |||
95 | } // stop within the dead zone: it knows it is in the dead zone if it hits +- 15 from RADIUS 3 times*/
|
||
96 | |||
97 | if ( dist<(RADIUS+30) && dist>(RADIUS-30)) { // the dead zone |
||
98 | orb_set_color(RED); |
||
99 | break;
|
||
100 | } |
||
101 | |||
102 | else if ((dist>RADIUS) || (dist==-1)) |
||
103 | { |
||
104 | ///delay_ms(200);
|
||
105 | search(); |
||
106 | |||
107 | motor_r_set(FORWARD,200);
|
||
108 | motor_l_set(FORWARD,200);
|
||
109 | } |
||
110 | else if ((dist<RADIUS)) |
||
111 | { |
||
112 | //delay_ms(200);
|
||
113 | search(); |
||
114 | |||
115 | motor_r_set(BACKWARD,200);
|
||
116 | motor_l_set(BACKWARD,200);
|
||
117 | } |
||
118 | |||
119 | } |
||
120 | |||
121 | orb_set_color(BLUE); |
||
122 | |||
123 | motors_off(); |
||
124 | |||
125 | /*usb_puti(range_read_distance(IR2));
|
||
126 | usb_puts("\n\r");
|
||
127 | motor_r_set(FORWARD,200);
|
||
128 | motor_l_set(FORWARD,200);
|
||
129 | delay_ms(300);
|
||
130 | usb_puti(range_read_distance(IR2));
|
||
131 | usb_puts("\n\r");
|
||
132 | delay_ms(300);
|
||
133 | usb_puti(range_read_distance(IR2));
|
||
134 | usb_puts("\n\r");
|
||
135 | delay_ms(300);
|
||
136 | usb_puti(range_read_distance(IR2));
|
||
137 | usb_puts("\n\r");*/
|
||
138 | } |
||
139 | void lastDrive()
|
||
140 | { |
||
141 | int lspeed = 190; |
||
142 | int rspeed = 190; |
||
143 | int firstldist = encoder_get_x(LEFT);
|
||
144 | int firstrdist = encoder_get_x(RIGHT);
|
||
145 | int ldist = firstldist;
|
||
146 | int rdist = firstrdist;
|
||
147 | while ( (ldist-firstldist<800) && (rdist-firstrdist<800) ) |
||
148 | { |
||
149 | motor_l_set(FORWARD,lspeed); |
||
150 | motor_r_set(FORWARD,rspeed); |
||
151 | |||
152 | int lvel = encoder_get_v(LEFT);
|
||
153 | int rvel = encoder_get_v(RIGHT);
|
||
154 | |||
155 | usb_puts("\n\rLeft distance: ");
|
||
156 | usb_puti(ldist-firstldist); |
||
157 | usb_puts("\n\rRight distance: ");
|
||
158 | usb_puti(rdist-firstrdist); |
||
159 | usb_puts("\n\rLeft velocity: ");
|
||
160 | usb_puti(lvel); |
||
161 | usb_puts("\n\rRight velocity: ");
|
||
162 | usb_puti(rvel); |
||
163 | usb_puts("\n\r\n\r");
|
||
164 | |||
165 | if (rvel<lvel)
|
||
166 | { |
||
167 | rspeed += 1;
|
||
168 | lspeed -= 1;
|
||
169 | } |
||
170 | else if (rvel>lvel) |
||
171 | { |
||
172 | rspeed -= 1;
|
||
173 | lspeed += 1;
|
||
174 | } |
||
175 | ldist = encoder_get_x(LEFT); |
||
176 | rdist = encoder_get_x(RIGHT); |
||
177 | |||
178 | delay_ms(25);
|
||
179 | } |
||
180 | motors_off(); |
||
181 | orb_set_color(GREEN); |
||
182 | } |