root / trunk / code / behaviors / formation_control / circle / circle.c @ 1524
History | View | Annotate | Download (4.48 KB)
1 |
#include <dragonfly_lib.h> |
---|---|
2 |
#include <encoders.h> |
3 |
|
4 |
#define RADIUS 300 |
5 |
|
6 |
/*
|
7 |
This is a PREVIOUS VERSION of circle2. Circle2 uses Echo's more exact code, while this uses a different method which is more buggy.
|
8 |
This code has been kept for the time being, in case some of the code is useful for recylcling (the control loop, for instance) - without
|
9 |
reverting the entire repositiory.
|
10 |
|
11 |
This program is used to make robots target a center (leader) robot using the BOM,
|
12 |
then drive toward it and stop at a certain distance away.
|
13 |
|
14 |
The distance will eventually be adjustable.
|
15 |
|
16 |
With adjustment, the leader robot will be able to turn and use its standardized
|
17 |
rangefinder to reposition or space the robots evenly.
|
18 |
|
19 |
AuTHORS: Niko, Alex
|
20 |
*/
|
21 |
|
22 |
extern void blink(int); |
23 |
extern void search(void); |
24 |
extern void approach(void); |
25 |
extern void lastDrive(void); |
26 |
|
27 |
int main(void) |
28 |
{ |
29 |
|
30 |
/* Initialize dragonfly board */
|
31 |
dragonfly_init(ALL_ON); |
32 |
encoders_init(); |
33 |
|
34 |
orb_set_color(GREEN); |
35 |
delay_ms(300);
|
36 |
|
37 |
search(); |
38 |
|
39 |
approach(); |
40 |
|
41 |
while(1); /* END HERE */ |
42 |
|
43 |
return 0; |
44 |
} |
45 |
|
46 |
|
47 |
|
48 |
void blink(int num) // blinks the number of times specifed, with the orbs. |
49 |
{ // useful for error checking.
|
50 |
for(int i = 0; i<num; i++) |
51 |
{ |
52 |
orb_set_color(ORB_OFF); |
53 |
delay_ms(200);
|
54 |
orb_set_color(RED); |
55 |
delay_ms(200);
|
56 |
} |
57 |
orb_set_color(ORB_OFF); |
58 |
} |
59 |
void search() //makes the robot face the beacon |
60 |
{ |
61 |
orb_set_color(YELLOW); //show that the robot is searching
|
62 |
bom_refresh(BOM_ALL); |
63 |
int top=bom_get_max();
|
64 |
|
65 |
if ((top<4) || (top>11)) //determine which way to turn |
66 |
{ |
67 |
motor_r_set(BACKWARD,200);
|
68 |
motor_l_set(FORWARD,200);
|
69 |
} |
70 |
else if((top>4) && (top<12)) |
71 |
{ |
72 |
motor_r_set(FORWARD,200);
|
73 |
motor_l_set(BACKWARD,200);
|
74 |
} |
75 |
while (top != 4) |
76 |
{ |
77 |
bom_refresh(BOM_ALL); |
78 |
top=bom_get_max(); |
79 |
} |
80 |
motors_off(); |
81 |
} |
82 |
void approach() // drives to the right distance. |
83 |
{ |
84 |
int dist = 0; |
85 |
int temp = 0; |
86 |
int counter = 0; |
87 |
// int correct = 0;
|
88 |
|
89 |
while(dist != RADIUS) //puts the robots in the given distance RADIUS |
90 |
{ |
91 |
temp=0;
|
92 |
counter = 0;
|
93 |
|
94 |
while(counter<8) // averages the first 8 legitimate (non -1) readings |
95 |
{ |
96 |
if(range_read_distance(IR2) == -1){} |
97 |
|
98 |
else
|
99 |
{ |
100 |
temp += range_read_distance(IR2); |
101 |
counter++; |
102 |
} |
103 |
delay_ms(20);
|
104 |
} |
105 |
dist = temp/8;
|
106 |
|
107 |
/*if (correct >2){
|
108 |
orb_set_color(RED);
|
109 |
break;
|
110 |
|
111 |
} // stop within the dead zone: it knows it is in the dead zone if it hits +- 15 from RADIUS 3 times*/
|
112 |
bom_refresh(BOM_ALL); |
113 |
int top = bom_get_max();
|
114 |
while(top!=4) |
115 |
{ |
116 |
search(); |
117 |
bom_refresh(BOM_ALL); |
118 |
top = bom_get_max(); |
119 |
} |
120 |
|
121 |
if ( dist<(RADIUS+30) && dist>(RADIUS-30)) { // defines the dead zone +- 30 from the given radius |
122 |
orb_set_color(RED); |
123 |
break;
|
124 |
} |
125 |
|
126 |
else if ((dist>RADIUS) || (dist==-1)) |
127 |
{ |
128 |
///delay_ms(200);
|
129 |
search(); |
130 |
|
131 |
motor_r_set(FORWARD,200);
|
132 |
motor_l_set(FORWARD,200);
|
133 |
} |
134 |
else if ((dist<RADIUS)) |
135 |
{ |
136 |
//delay_ms(200);
|
137 |
search(); |
138 |
|
139 |
motor_r_set(BACKWARD,200);
|
140 |
motor_l_set(BACKWARD,200);
|
141 |
} |
142 |
|
143 |
} |
144 |
|
145 |
orb_set_color(BLUE); |
146 |
|
147 |
motors_off(); |
148 |
|
149 |
/*usb_puti(range_read_distance(IR2));
|
150 |
usb_puts("\n\r");
|
151 |
motor_r_set(FORWARD,200);
|
152 |
motor_l_set(FORWARD,200);
|
153 |
delay_ms(300);
|
154 |
usb_puti(range_read_distance(IR2));
|
155 |
usb_puts("\n\r");
|
156 |
delay_ms(300);
|
157 |
usb_puti(range_read_distance(IR2));
|
158 |
usb_puts("\n\r");
|
159 |
delay_ms(300);
|
160 |
usb_puti(range_read_distance(IR2));
|
161 |
usb_puts("\n\r");*/
|
162 |
} |
163 |
void lastDrive() // used for the final drive in lab1 and not useful here; control loop code may be recycled. |
164 |
{ |
165 |
int lspeed = 190; |
166 |
int rspeed = 190; |
167 |
int firstldist = encoder_get_x(LEFT);
|
168 |
int firstrdist = encoder_get_x(RIGHT);
|
169 |
int ldist = firstldist;
|
170 |
int rdist = firstrdist;
|
171 |
while ( (ldist-firstldist<800) && (rdist-firstrdist<800) ) |
172 |
{ |
173 |
motor_l_set(FORWARD,lspeed); |
174 |
motor_r_set(FORWARD,rspeed); |
175 |
|
176 |
int lvel = encoder_get_v(LEFT);
|
177 |
int rvel = encoder_get_v(RIGHT);
|
178 |
|
179 |
usb_puts("\n\rLeft distance: ");
|
180 |
usb_puti(ldist-firstldist); |
181 |
usb_puts("\n\rRight distance: ");
|
182 |
usb_puti(rdist-firstrdist); |
183 |
usb_puts("\n\rLeft velocity: ");
|
184 |
usb_puti(lvel); |
185 |
usb_puts("\n\rRight velocity: ");
|
186 |
usb_puti(rvel); |
187 |
usb_puts("\n\r\n\r");
|
188 |
|
189 |
if (rvel<lvel) // adjust the speeds if the encoders have a mismatch. |
190 |
{ |
191 |
rspeed += 1;
|
192 |
lspeed -= 1;
|
193 |
} |
194 |
else if (rvel>lvel) |
195 |
{ |
196 |
rspeed -= 1;
|
197 |
lspeed += 1;
|
198 |
} |
199 |
ldist = encoder_get_x(LEFT); |
200 |
rdist = encoder_get_x(RIGHT); |
201 |
|
202 |
delay_ms(25);
|
203 |
} |
204 |
motors_off(); |
205 |
orb_set_color(GREEN); |
206 |
} |