Revision 1622
New version of cicle_spacing. (It works now...). Before merging with circle.
circle_spacing.c | ||
---|---|---|
7 | 7 |
int sending = 0; |
8 | 8 |
int stop = 0; |
9 | 9 |
struct vector slave_position; |
10 |
int desired_max_bom; |
|
11 |
int bom_max_counter; |
|
10 | 12 |
|
13 |
|
|
14 |
void set_desired_max_bom(int desired_angle) |
|
15 |
{ |
|
16 |
if (desired_angle >= 348 || desired_angle < 11) desired_max_bom = 0; |
|
17 |
if (desired_angle >= 11 && desired_angle < 33) desired_max_bom = 1; |
|
18 |
if (desired_angle >= 33 && desired_angle < 56) desired_max_bom = 2; |
|
19 |
if (desired_angle >= 56 && desired_angle < 78) desired_max_bom = 3; |
|
20 |
if (desired_angle >= 78 && desired_angle < 101) desired_max_bom = 4; |
|
21 |
if (desired_angle >= 101 && desired_angle < 123) desired_max_bom = 5; |
|
22 |
if (desired_angle >= 123 && desired_angle < 145) desired_max_bom = 6; |
|
23 |
if (desired_angle >= 145 && desired_angle < 167) desired_max_bom = 7; |
|
24 |
if (desired_angle >= 167 && desired_angle < 190) desired_max_bom = 8; |
|
25 |
if (desired_angle >= 190 && desired_angle < 212) desired_max_bom = 9; |
|
26 |
if (desired_angle >= 212 && desired_angle < 235) desired_max_bom = 10; |
|
27 |
if (desired_angle >= 235 && desired_angle < 257) desired_max_bom = 11; |
|
28 |
if (desired_angle >= 257 && desired_angle < 280) desired_max_bom = 12; |
|
29 |
if (desired_angle >= 280 && desired_angle < 302) desired_max_bom = 13; |
|
30 |
if (desired_angle >= 302 && desired_angle < 325) desired_max_bom = 14; |
|
31 |
if (desired_angle >= 325 && desired_angle < 348) desired_max_bom = 15; |
|
32 |
} |
|
33 |
|
|
34 |
|
|
35 |
|
|
36 |
|
|
37 |
|
|
38 |
|
|
11 | 39 |
void switch_sending () |
12 | 40 |
{ |
13 | 41 |
if(sending) |
... | ... | |
30 | 58 |
|
31 | 59 |
|
32 | 60 |
int sending_counter = 0; |
33 |
|
|
61 |
|
|
34 | 62 |
/* Initialize dragonfly board */ |
35 | 63 |
|
36 | 64 |
dragonfly_init(ALL_ON); |
... | ... | |
42 | 70 |
char send_buffer[2]; |
43 | 71 |
|
44 | 72 |
|
45 |
if(master) orb1_set_color(RED);
|
|
73 |
if(master) {orb1_set_color(RED); set_desired_max_bom(30);}
|
|
46 | 74 |
else orb1_set_color(BLUE); |
47 | 75 |
|
76 |
|
|
48 | 77 |
while(1) |
49 | 78 |
{ |
50 | 79 |
|
... | ... | |
58 | 87 |
if(master) |
59 | 88 |
{ |
60 | 89 |
|
61 |
if(sending_counter++>2)
|
|
90 |
if(sending_counter++>4)
|
|
62 | 91 |
{ |
63 | 92 |
switch_sending(); |
64 | 93 |
sending_counter = 0; |
... | ... | |
78 | 107 |
|
79 | 108 |
else // recieving |
80 | 109 |
{ |
110 |
int max_bom = bom_get_max(); |
|
111 |
usb_puts("bom_get_max : "); |
|
112 |
usb_puti(max_bom); |
|
113 |
usb_puts("\n\r"); |
|
81 | 114 |
|
82 |
bom_get_vector(&slave_position, NULL); |
|
83 |
if(slave_position.y/slave_position.x > 10){ |
|
84 |
|
|
85 |
//if(bom_get_max()+1==5){ |
|
115 |
if(max_bom == desired_max_bom) |
|
116 |
{ |
|
117 |
if(bom_max_counter) // only stops the slave if two consecutive boms reading give the desired bom as the max one. Filters the noise. |
|
118 |
{ |
|
119 |
send_buffer[0] = 's'; |
|
120 |
wl_basic_send_global_packet(42, send_buffer, 2); |
|
121 |
} |
|
122 |
bom_max_counter =1; |
|
123 |
} |
|
124 |
else bom_max_counter = 0; |
|
86 | 125 |
|
87 |
send_buffer[0] = 's'; |
|
88 |
wl_basic_send_global_packet(42, send_buffer, 2); |
|
89 |
} |
|
90 | 126 |
} |
91 | 127 |
} |
92 | 128 |
|
... | ... | |
99 | 135 |
|
100 | 136 |
if(sending) |
101 | 137 |
{ |
102 |
/* |
|
103 |
motor_l_set(FORWARD,0); |
|
104 |
motor_r_set(FORWARD,0); |
|
105 |
*/ |
|
138 |
|
|
106 | 139 |
} |
107 | 140 |
|
108 | 141 |
else // recieving |
... | ... | |
117 | 150 |
|
118 | 151 |
else |
119 | 152 |
{ |
153 |
int max_bom = bom_get_max(); |
|
154 |
usb_puts("bom_get_max : "); |
|
155 |
usb_puti(max_bom); |
|
156 |
usb_puts("/n/r"); |
|
120 | 157 |
|
121 |
|
|
122 |
|
|
123 |
|
|
124 |
//bom_get_vector(&slave_position, NULL); |
|
125 |
//if(abs(slave_position.x)>2 && (double)slave_position.y/(double)slave_position.x < 0.1){ |
|
126 |
|
|
127 |
if(bom_get_max()+1==8){ |
|
158 |
|
|
159 |
if(max_bom == 8) |
|
160 |
{ |
|
128 | 161 |
orb2_set_color(BLUE); |
129 | 162 |
motor_r_set(FORWARD,180); |
130 | 163 |
motor_l_set(FORWARD,180); |
131 | 164 |
|
132 | 165 |
} |
133 |
else { |
|
134 |
orb2_set_color(GREEN); |
|
166 |
else if(max_bom == 7 || max_bom == 6 || max_bom == 5) |
|
167 |
{ |
|
168 |
motor_l_set(FORWARD,180); |
|
169 |
motor_r_set(FORWARD,0); |
|
170 |
} |
|
171 |
else if(max_bom == -1); |
|
172 |
else |
|
173 |
{ |
|
174 |
orb2_set_color(GREEN); |
|
135 | 175 |
motor_l_set(FORWARD,0); |
136 | 176 |
motor_r_set(FORWARD,180); |
137 | 177 |
} |
... | ... | |
156 | 196 |
} |
157 | 197 |
|
158 | 198 |
|
199 |
|
|
200 |
|
|
201 |
|
|
202 |
|
Also available in: Unified diff