root / trunk / code / behaviors / formation_control / circle2 / circle.c @ 1524
History | View | Annotate | Download (3.74 KB)
1 |
#include <dragonfly_lib.h> |
---|---|
2 |
#include <encoders.h> |
3 |
|
4 |
/*
|
5 |
This program is used to make robots target a center (leader) robot using the BOM,
|
6 |
then drive toward it and stop at a certain distance away.
|
7 |
|
8 |
The distance will eventually be adjustable.
|
9 |
|
10 |
With adjustment, the leader robot will be able to turn and use its standardized
|
11 |
rangefinder to reposition or space the robots evenly.
|
12 |
|
13 |
AuTHORS: Niko, Alex, Reva, Echo
|
14 |
*/
|
15 |
|
16 |
|
17 |
/*
|
18 |
TODO:
|
19 |
Check BOM more often and before final stop
|
20 |
Make BOM check more comprehensively
|
21 |
Use the center bot to check distances
|
22 |
Count them ("spam" method)
|
23 |
Use beacon to find relative positions
|
24 |
Beacon tells them how to move to be at the right distance
|
25 |
Wireless communication, initialization
|
26 |
*/
|
27 |
|
28 |
void forward(int speed){ // set the motors to this forward speed. |
29 |
motor_l_set(FORWARD,speed); |
30 |
motor_r_set(FORWARD,speed); |
31 |
} |
32 |
void left(int speed){ // turn left at this speed. |
33 |
motor_l_set(FORWARD,speed); |
34 |
motor_r_set(BACKWARD,speed); |
35 |
} |
36 |
void right(int speed){ |
37 |
motor_l_set(BACKWARD,speed); |
38 |
motor_r_set(FORWARD,speed); |
39 |
} |
40 |
void stop(void){ // could be set to motors_off(), or just use this as an alternative. |
41 |
motor_l_set(BACKWARD,0);
|
42 |
motor_r_set(FORWARD,0);
|
43 |
} |
44 |
void setforward(int spd1, int spd2){ |
45 |
motor_l_set(FORWARD,spd1); |
46 |
motor_r_set(FORWARD,spd2); |
47 |
} |
48 |
void backward(int speed){ |
49 |
motor_l_set(BACKWARD, speed); |
50 |
motor_r_set(BACKWARD, speed); |
51 |
} |
52 |
int get_distance(void){ // takes an averaged reading of the front rangefinder |
53 |
int temp,distance,kk=5; // kk sets this to 5 readings. |
54 |
distance =0;
|
55 |
for (int i=0; i<kk; i++){ |
56 |
temp = range_read_distance(IR2); |
57 |
if (temp == -1) {temp=0; kk--;} |
58 |
distance+= temp; |
59 |
delay_ms(3);
|
60 |
} |
61 |
if (kk>0) |
62 |
return (int)(distance/kk); |
63 |
else
|
64 |
return 0; |
65 |
} |
66 |
void turn_to_beacon(int max){ |
67 |
if (max>-1 && max<16){ |
68 |
int index = (max+12)%16; |
69 |
if (index==0) { |
70 |
stop(); |
71 |
} |
72 |
else if (index<8) right(170); |
73 |
else left(170); |
74 |
} |
75 |
} |
76 |
|
77 |
void turn_to_beacon2(int max){ // like the previous but no stop() call |
78 |
if (max>-1 && max<16){ |
79 |
int index = (max+12)%16; |
80 |
if (index==0) { |
81 |
|
82 |
} |
83 |
else if (index<8) right(170); |
84 |
else left(170); |
85 |
} |
86 |
} |
87 |
void orient(void){ |
88 |
int max_index = -1; |
89 |
while (max_index!=4) { |
90 |
/* Refresh and make sure the table is updated */
|
91 |
bom_refresh(BOM_ALL); |
92 |
max_index = bom_get_max(); |
93 |
turn_to_beacon(max_index); |
94 |
delay_ms(22);
|
95 |
} |
96 |
} |
97 |
void orient2(void){ |
98 |
int max_index = -1; |
99 |
while (max_index!=4) { |
100 |
/* Refresh and make sure the table is updated */
|
101 |
bom_refresh(BOM_ALL); |
102 |
max_index = bom_get_max(); |
103 |
turn_to_beacon2(max_index); |
104 |
delay_ms(22);
|
105 |
} |
106 |
} |
107 |
void go_straight(void){ // drives forward a hardcoded distance. May not be useful. |
108 |
forward(200);
|
109 |
encoder_rst_dx(LEFT); |
110 |
encoder_rst_dx(RIGHT); |
111 |
delay_ms(100);
|
112 |
int x_left = encoder_get_x(LEFT), x_right = encoder_get_x(RIGHT);
|
113 |
int count = 0; |
114 |
int d;
|
115 |
while (count<25){ //count = 25 when bot6; count <12 |
116 |
x_left = encoder_get_x(LEFT); |
117 |
x_right = encoder_get_x(RIGHT); |
118 |
d = x_right-x_left; |
119 |
if (d>13 || d<-13){ |
120 |
if(d<50 && d>-50){ |
121 |
d = round(1.0*d/4); |
122 |
setforward(200+d, 200-d); |
123 |
} |
124 |
} |
125 |
delay_ms(32);
|
126 |
count++; |
127 |
} |
128 |
} |
129 |
void go_straight_onefoot(void){ // essentially, another name for the above. Should be removed. |
130 |
go_straight(); |
131 |
} |
132 |
|
133 |
int main(void) |
134 |
{ |
135 |
|
136 |
/* Initialize dragonfly board */
|
137 |
dragonfly_init(ALL_ON); |
138 |
encoders_init(); |
139 |
|
140 |
int distance=1000; // how far away to stop. |
141 |
int onefoot=300, speed=200; // one foot is 490 for bot 1; one foot is 200 for bot6 |
142 |
orient(); |
143 |
forward(speed); |
144 |
range_init(); |
145 |
|
146 |
orb_set_color(YELLOW); |
147 |
|
148 |
distance = get_distance(); |
149 |
while (distance>=onefoot || distance==0){ |
150 |
distance = get_distance(); |
151 |
orient2(); |
152 |
forward(speed); |
153 |
delay_ms(14);
|
154 |
} |
155 |
stop(); |
156 |
orient(); |
157 |
|
158 |
//button1_wait (); // code for lab1.
|
159 |
//go_straight_onefoot();
|
160 |
stop(); |
161 |
/* ****** CODE HERE ******* */
|
162 |
|
163 |
while(1); /* END HERE */ |
164 |
|
165 |
return 0; |
166 |
} |