|
| |
<<
functions\cross_bridge.c

Click to enlarge
void cross_bridge (float side, int bridge)
{
int full_speed=90, side_align, sensor_to_check, tilt=0, bump_side, distance, check=1;
/* turn(side,50); */ /* to get away from hole in wall? */
/*straight(-full_speed/3,-full_speed/3,1.,0);*/
motors_off();
sleep(.5);
if (side<0.)
{
encoder3_counts=0;
left_motor(-50);
right_motor(-40);
while (encoder3_counts<20);
}
if (side>0.)
{
encoder1_counts=0;
right_motor(-50);
left_motor(-40);
while(encoder1_counts<19);
}
straight(-full_speed/3,-full_speed/3,3.,0);
while (check==1)
{
distance=straight(-full_speed,-full_speed,37.,1);
if (distance>30)
{
turn(side,50);
}
check=0;
}
servo_a5_init(0);
distance=straight(-full_speed,-full_speed,100.,1); /* run into wall just past ore collection stage */
distance=straight(full_speed,full_speed,3.,0); /* forward, turn toward outside wall */
/* right_motor(-90);
left_motor(-90);
while(mercury()==0);
while(mercury()==1);
motors_off();
sleep(3.); */
turn(98.*side,50);
straight(full_speed,full_speed,100.,1); /* run into outside wall */
bump_side=sensor_check();
side_align=2;
wall_align(bump_side,full_speed,side_align);
/*
if(side>0.) side_align=8;
if(side<0.) side_align=4;
wall_align(bump_side,full_speed, side_align); */
if(bridge==125)
{
straight(-full_speed,-full_speed,1.,0);
turn(-50.*side, 50);
straight(-full_speed,-full_speed,1.,0);
turn(-50.*side, 50);
straight(full_speed,full_speed,4.,0);
if (side>0.)
{
left_motor(-60);
right_motor(-35);
}
if (side<0.)
{
left_motor(-35);
right_motor(-60);
}
sleep(3.);
while (mercury()==0);
sleep(2.);
while(mercury()==1);
}
if(bridge==100)
{
straight(-full_speed,-full_speed,9.,0);
turn(-104.*side,50); /*turn so rear is toward bridge*/
right_motor(-90);
left_motor(-90);
sleep(2.);
while (mercury()==0);
sleep(3.);
while(mercury()==1);
/*run along wall till ramp found*/
if (side<0.)
{
left_motor(-60);
right_motor(-35);
}
if (side>0.)
{
left_motor(-35);
right_motor(-60);
}
}
sleep(3.);
while(mercury()==0);
sleep(1.);
beep();
motors_off();
straight(-full_speed,-full_speed,25.,1);
}
|