root / trunk / code / behaviors / formation_control / circle / circle.c @ 1509
History | View | Annotate | Download (3.3 KB)
1 |
#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 |
} |