Revision 1627
More modifications to circle.c
| circle.c | ||
|---|---|---|
| 33 | 33 |
if (desired_angle >= 325 && desired_angle < 348) desired_max_bom = 15; |
| 34 | 34 |
} |
| 35 | 35 |
|
| 36 |
void switch_sending ()
|
|
| 36 |
void switch_sending(void)
|
|
| 37 | 37 |
{
|
| 38 | 38 |
if(sending) |
| 39 | 39 |
{
|
| ... | ... | |
| 126 | 126 |
|
| 127 | 127 |
/*~~~~~~~~~~~~~ NEED document |
| 128 | 128 |
*/ |
| 129 |
int correctTurn(void)
|
|
| 129 |
void correctTurn(void)
|
|
| 130 | 130 |
{
|
| 131 |
int bomNum = 0; |
|
| 132 |
bom_refresh(BOM_ALL); |
|
| 133 |
bomNum = bom_get_max(); |
|
| 134 |
usb_puti(bomNum); |
|
| 135 |
if(bomNum == 4) |
|
| 136 |
{ timeout = 0;
|
|
| 137 |
//motor_l_set(1, 200); |
|
| 138 |
//motor_r_set(1, 200); |
|
| 139 |
return 1; //<--------------------------------------------1 and 0 are switched |
|
| 140 |
} |
|
| 141 |
else |
|
| 131 |
orb1_set_color(BLUE); // BLUE and PURPLE |
|
| 132 |
left(220); |
|
| 133 |
while(1) |
|
| 142 | 134 |
{
|
| 143 |
if(bomNum == -1){
|
|
| 144 |
timeout++; |
|
| 145 |
if(timeout > 500000) |
|
| 146 |
{
|
|
| 147 |
motor_r_set(FORWARD, 210); |
|
| 148 |
motor_l_set(BACKWARD, 190); |
|
| 149 |
} |
|
| 150 |
} |
|
| 151 |
else if((bomNum >= 12) || (bomNum < 4)) |
|
| 135 |
int bomNum = 0; // bomNum is the current maximum reading |
|
| 136 |
bom_refresh(BOM_ALL); |
|
| 137 |
bomNum = bom_get_max(); |
|
| 138 |
usb_puti(bomNum); |
|
| 139 |
if(bomNum == 4) // when it's turned the right way, stop |
|
| 152 | 140 |
{
|
| 153 |
motor_l_set(FORWARD, 200);timeout = 0; |
|
| 154 |
motor_r_set(BACKWARD, 200); |
|
| 141 |
timeout = 0; |
|
| 142 |
//motor_l_set(1, 200); |
|
| 143 |
//motor_r_set(1, 200); |
|
| 144 |
break; // exits the while() loop to stop the method |
|
| 155 | 145 |
} |
| 156 |
else |
|
| 146 |
else // facing the wrong way
|
|
| 157 | 147 |
{
|
| 158 |
motor_l_set(BACKWARD, 200);timeout = 0; |
|
| 159 |
motor_r_set(FORWARD, 200); |
|
| 148 |
if(bomNum == -1){
|
|
| 149 |
|
|
| 150 |
timeout++; |
|
| 151 |
|
|
| 152 |
if(timeout > 500000) // if it's been looking too long, move a little bit as it turns |
|
| 153 |
{
|
|
| 154 |
motor_r_set(FORWARD, 210); |
|
| 155 |
motor_l_set(BACKWARD, 190); |
|
| 156 |
} |
|
| 157 |
} |
|
| 158 |
else if((bomNum >= 12) || (bomNum < 4)) |
|
| 159 |
{
|
|
| 160 |
motor_l_set(FORWARD, 200); |
|
| 161 |
motor_r_set(BACKWARD, 200); |
|
| 162 |
timeout = 0; |
|
| 163 |
} |
|
| 164 |
else |
|
| 165 |
{
|
|
| 166 |
motor_l_set(BACKWARD, 200); |
|
| 167 |
motor_r_set(FORWARD, 200); |
|
| 168 |
timeout = 0; |
|
| 169 |
} |
|
| 160 | 170 |
} |
| 161 | 171 |
} |
| 162 |
return 0;
|
|
| 172 |
return; |
|
| 163 | 173 |
} |
| 164 | 174 |
|
| 165 | 175 |
|
| 166 | 176 |
/*~~~~~~~~~~~~~~ NEED document |
| 177 |
|
|
| 178 |
Actually, we should just get rid of this. |
|
| 167 | 179 |
*/ |
| 168 | 180 |
void correctApproach(void) |
| 169 | 181 |
{
|
| ... | ... | |
| 194 | 206 |
} |
| 195 | 207 |
|
| 196 | 208 |
|
| 197 |
/* |
|
| 198 |
Start turning until the front receive the MAx bom reading |
|
| 199 |
*/ |
|
| 200 |
void turn_to_beacon(int max){
|
|
| 201 |
if (max>-1 && max<16){
|
|
| 202 |
int index = (max+12)%16; |
|
| 203 |
if (index==0) {
|
|
| 204 |
stop(); |
|
| 205 |
} |
|
| 206 |
else if (index<8) right(170); |
|
| 207 |
else left(170); |
|
| 208 |
} |
|
| 209 |
} |
|
| 210 |
|
|
| 211 |
|
|
| 212 |
/* |
|
| 213 |
Start turning its fron to the MAx bom reading |
|
| 214 |
But it won't stop by itself. |
|
| 215 |
*/ |
|
| 216 |
void turn_to_beacon2(int max){ // like the previous but no stop() call'
|
|
| 217 |
|
|
| 218 |
|
|
| 219 |
|
|
| 220 |
|
|
| 221 |
if (max>-1 && max<16){
|
|
| 222 |
int index = (max+12)%16; |
|
| 223 |
if (index==0) {
|
|
| 224 |
|
|
| 225 |
} |
|
| 226 |
else if (index<8) right(170); |
|
| 227 |
else left(170); |
|
| 228 |
} |
|
| 229 |
} |
|
| 230 |
|
|
| 231 |
|
|
| 232 |
|
|
| 233 |
|
|
| 234 |
/* |
|
| 235 |
Turn towards the MAX bom direction |
|
| 236 |
until the front receive the MAx bom reading |
|
| 237 |
*/ |
|
| 238 |
void orient(void){
|
|
| 239 |
int max_index = -1; |
|
| 240 |
while (max_index!=4) {
|
|
| 241 |
/* Refresh and make sure the table is updated */ |
|
| 242 |
bom_refresh(BOM_ALL); |
|
| 243 |
max_index = bom_get_max(); |
|
| 244 |
turn_to_beacon(max_index); |
|
| 245 |
delay_ms(22); |
|
| 246 |
} |
|
| 247 |
} |
|
| 248 |
|
|
| 249 |
|
|
| 250 |
/* |
|
| 251 |
Turn towards the MAX bom direction |
|
| 252 |
until the front receive the MAx bom reading |
|
| 253 |
|
|
| 254 |
This function has less stop() so the robots run more smooth |
|
| 255 |
*/ |
|
| 256 |
void orient2(void){
|
|
| 257 |
int max_index = -1; |
|
| 258 |
while (max_index!=4) {
|
|
| 259 |
/* Refresh and make sure the table is updated */ |
|
| 260 |
bom_refresh(BOM_ALL); |
|
| 261 |
max_index = bom_get_max(); |
|
| 262 |
turn_to_beacon2(max_index); |
|
| 263 |
delay_ms(22); |
|
| 264 |
} |
|
| 265 |
} |
|
| 266 | 209 |
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ |
| 267 | 210 |
void go_straight(void){ // drives forward a hardcoded distance. May not be useful.
|
| 268 | 211 |
forward(200); |
| ... | ... | |
| 328 | 271 |
int beacon_State=0; |
| 329 | 272 |
int waitingCounter=0; |
| 330 | 273 |
int robotsReceived=0; |
| 274 |
|
|
| 331 | 275 |
if(wheel()<100) |
| 332 | 276 |
{
|
| 333 | 277 |
state=EDGE; |
| ... | ... | |
| 410 | 354 |
// COLOR afer DONE ---> MAGENTA |
| 411 | 355 |
orb_set_color(MAGENTA); |
| 412 | 356 |
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ |
| 413 |
while(correctTurn()){
|
|
| 414 |
} |
|
| 357 |
correctTurn(); // turn to face the beacon |
|
| 415 | 358 |
forward(220); |
| 416 | 359 |
//range_init(); |
| 417 | 360 |
|
| ... | ... | |
| 534 | 477 |
packet_data=wl_basic_do_default(&data_length); |
| 535 | 478 |
if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK) |
| 536 | 479 |
{
|
| 537 |
orb_set_color(RED);orb2_set_color(BLUE); |
|
| 480 |
orb_set_color(RED); |
|
| 481 |
orb2_set_color(BLUE); |
|
| 538 | 482 |
//only add to robots seen if you haven't gotten an ACK from this robot |
| 539 | 483 |
if(used[packet_data[1]]==0){
|
| 540 | 484 |
robotsReceived++; |
| ... | ... | |
| 550 | 494 |
Sending DONE package. |
| 551 | 495 |
*/ |
| 552 | 496 |
case 3: |
| 497 |
blink(robotsReceived); |
|
| 553 | 498 |
orb_set_color(GREEN); |
| 554 | 499 |
send_buffer[0]=CIRCLE_ACTION_DONE; |
| 555 | 500 |
wl_basic_send_global_packet(42,send_buffer,2); |
Also available in: Unified diff