void follow()
{
while (!check_command(RETURNTOMAIN))
{
while(!Object(front2, DISTANCE_WALL))
Motors_Forward(PWM, PWM);
while (Object(front2, DISTANCE_WALL))
Motors_Stop();
delay_ms(250);
if (Object(front1, DISTANCE_WALL))
while (!Object(front2, DISTANCE_WALL))
Motors_Left(PWM, PWM);
else if (Object(front3, DISTANCE_WALL))
while (!Object(front2, DISTANCE_WALL))
Motors_Right(PWM, PWM);
}
Motors_Stop();
}
void manual()
{
time_elapsed = 0;
distance_traveled = 0;
while (!check_command(RETURNTOMAIN))
{
if (check_command(GOFORWARD))
Motors_Forward(PWM,PWM);
else if (check_command(GOBACK))
Motors_Back(PWM,PWM);
else if (check_command(ROTATELEFT))
Motors_Left(PWM,PWM);
else if (check_command(ROTATERIGHT))
Motors_Right(PWM,PWM);
else if (check_command(STOPMOTORS))
Motors_Stop();
}
Motors_Stop();
}
void obstacle_dodge()
{
while (!check_command(RETURNTOMAIN))
{
if(!Object(front2, DISTANCE_WALL))
Motors_Forward(PWM, PWM);
else
while (Object(front1, DISTANCE_WALL) || Object(front2, DISTANCE_WALL) || Object(front3, DISTANCE_WALL))
{
if (milliseconds % 2 == 0)
Rotate_Left(20);
else
Rotate_Right(20);
}
}
Motors_Stop();
}
void lateral_parking()
{
time_elapsed = 0;
distance_traveled = 0;
do //Follow right wall until spot is detected
{
Wall_Follow(right, 6);
} while (Object(right, DISTANCE_WALL));
Go_Forward(27); //Go forward until at the middle of the spot
Rotate_Right(22); //Rotate towards parking spot
Go_Forward(20); //Advance inside parking spot
Rotate_Left(24); //Rotate to be parallel to road
while (front2[2] > back[2]) //The car should be closer to the car on the back
{
Ultrasonic_Scan();
Wall_Follow(right, 3); //Follow right wall so it can adjust if not centered or rotated properly
}
Motors_Stop();
}
void USART1_IRQHandler() {
if (USART1->SR & USART_SR_RXNE) {
USART1->SR &= ~USART_SR_RXNE;
received = USART1->DR;
switch (curWaiting) {
case WAITING_FOR_COMMAND:
switch (received) {
case TURN_EVERYTHING_OFF:
telemetryOn = 0;
research = NO_RESEARCH;
break;
case STOP_MOTORS:
Motors_Stop();
break;
case CALIBRATION:
research = NO_RESEARCH;
Motors_Stop();
ADXL345_Calibr();
ADXRS290_Calibr();
//Gyro_Calibr();
break;
case LOWPASS:
lowpassOn ^= 1;
break;
case TELEMETRY:
telemetryOn ^= 1;
break;
case PWM1:
initWaitingForInt();
curWaitingForInt = WAITING_FOR_PWM1;
break;
case PWM2:
initWaitingForInt();
curWaitingForInt = WAITING_FOR_PWM2;
break;
case MIN_PWM:
initWaitingForInt();
curWaitingForInt = WAITING_FOR_MIN_PWM;
break;
case MAX_PWM:
initWaitingForInt();
curWaitingForInt = WAITING_FOR_MAX_PWM;
break;
case KP:
initWaitingForFloat();
curWaitingForFloat = WAITING_FOR_KP;
break;
case KI:
initWaitingForFloat();
curWaitingForFloat = WAITING_FOR_KI;
break;
case KD:
initWaitingForFloat();
curWaitingForFloat = WAITING_FOR_KD;
break;
case TURN_USELESS:
turnUselessOn ^= 1;
break;
case GYRO_RECALIBRATION:
gyroRecalibrationOn ^= 1;
break;
case TRANQUILITY_TIME:
initWaitingForInt();
curWaitingForInt = WAITING_FOR_TRANQUILITY_TIME;
break;
case PWM_STEP:
initWaitingForInt();
curWaitingForInt = WAITING_FOR_PWM_STEP;
break;
case EVERY_N:
initWaitingForInt();
curWaitingForInt = WAITING_FOR_EVERY_N;
break;
case ACCEL_FREQ_HZ25:
freshFreq = 1;
curFreq = HZ25;
curDT = 0.04;
break;
case ACCEL_FREQ_HZ50:
freshFreq = 1;
curFreq = HZ50;
curDT = 0.02;
break;
case ACCEL_FREQ_HZ100:
freshFreq = 1;
curFreq = HZ100;
curDT = 0.01;
break;
case ACCEL_FREQ_HZ800:
freshFreq = 1;
curFreq = HZ800;
curDT = 0.00125;
break;
case ACCEL_FREQ_HZ1600:
freshFreq = 1;
curFreq = HZ1600;
curDT = 0.000625;
break;
case ACCEL_FREQ_HZ3200:
freshFreq = 1;
curFreq = HZ3200;
curDT = 0.0003125;
break;
/* case GYRO_FREQ_HZ100:
gyroFreshFreq = 1;
gyroCurFreq = GYRO_HZ100;
gyroCurDT = 0.01;
break;
case GYRO_FREQ_HZ250:
gyroFreshFreq = 1;
gyroCurFreq = GYRO_HZ250;
gyroCurDT = 0.004;
break;
case GYRO_FREQ_HZ500:
gyroFreshFreq = 1;
gyroCurFreq = GYRO_HZ500;
gyroCurDT = 0.002;
break;
case GYRO_FREQ_HZ1000:
gyroFreshFreq = 1;
gyroCurFreq = GYRO_HZ1000;
gyroCurDT = 0.001;
break;*/
case MAX_ANGLE:
curWaitingForFloat = WAITING_FOR_MAX_ANGLE;
initWaitingForFloat();
break;
case ACCEL_DEVIATION:
curWaitingForFloat = WAITING_FOR_ACCEL_DEVIATION;
initWaitingForFloat();
break;
case BOUNDARY_ANGLE:
curWaitingForFloat = WAITING_FOR_BOUNDARY_ANGLE;
initWaitingForFloat();
break;
case MAX_ANGVEL:
curWaitingForFloat = WAITING_FOR_MAX_ANGVEL;
initWaitingForFloat();
break;
case IMPULSE:
research = IMPULSE_RESPONSE;
researchIndex = 0;
break;
case STEP:
research = STEP_RESPONSE;
researchIndex = 0;
break;
case SINE:
curWaiting = WAITING_FOR_FLOAT;
curWaitingForFloat = WAITING_FOR_RESEARCH_AMPL;
initWaitingForFloat();
research = SINE_RESPONSE;
break;
case EXP:
curWaiting = WAITING_FOR_FLOAT;
curWaitingForFloat = WAITING_FOR_RESEARCH_AMPL;
initWaitingForFloat();
research = EXP_RESPONSE;
break;
case NO_RESEARCH_SYMBOL:
research = NO_RESEARCH;
Motors_Stop();
researchIndex = 0;
break;
case SIMPLE:
research = SIMPLE_CONTROL;
Kp = (maxPwm - minPwm) / maxAngle;
break;
case PID:
research = PID_CONTROL;
break;
case OPERATOR:
research = OPERATOR_CONTROL;
break;
case ADJUST:
research = ADJUST_CONTROL;
pwm1 = minPwm;
pwm2 = minPwm;
break;
case PROGRAMMING_MODE:
GPIOB->MODER |= 1 << 8*2;
GPIOB->BSRRL |= 1 << 8;
for (rst = 0; rst < 100000; rst++) {
__nop();
}
SCB->AIRCR = (0x5fa << 16) | (1 << 2);
break;
}
break;
case WAITING_FOR_INT:
switch (received) {
case NUMBER_END:
curWaiting = WAITING_FOR_COMMAND;
while (i >= 0) {
intValue += (str[i--] - '0') * st;
st *= 10;
}
switch (curWaitingForInt) {
case WAITING_FOR_PWM1:
if (research == OPERATOR_CONTROL) {
pwm1 = intValue;
TIM4->CCR1 = pwm1;
}
break;
case WAITING_FOR_PWM2:
if (research == OPERATOR_CONTROL) {
pwm2 = intValue;
TIM4->CCR3 = pwm2;
}
break;
case WAITING_FOR_MIN_PWM:
minPwm = intValue;
break;
case WAITING_FOR_MAX_PWM:
maxPwm = intValue;
break;
// case WAITING_FOR_TRANQUILITY_TIME:
// tranquilityTime = intValue / 10;
// break;
// case WAITING_FOR_PWM_STEP:
// pwmStep = intValue;
// break;
// case WAITING_FOR_EVERY_N:
// everyN = intValue;
// break;
default:
break;
}
break;
default:
str[++i] = received;
break;
}
break;
case WAITING_FOR_FLOAT:
switch (received) {
case 'b':
curWaiting = WAITING_FOR_COMMAND;
k = -1;
while( k < i) {
floatValue += (str[++k] - '0') * d_st;
d_st *= my_pow;
}
switch (curWaitingForFloat) {
case WAITING_FOR_KP:
Kp = floatValue;
break;
case WAITING_FOR_KD:
Kd = floatValue;
break;
case WAITING_FOR_KI:
Ki = floatValue;
break;
case WAITING_FOR_RESEARCH_AMPL:
researchAmplitude = floatValue;
curWaiting = WAITING_FOR_FLOAT;
curWaitingForFloat = WAITING_FOR_RESEARCH_FREQ;
initWaitingForFloat();
break;
case WAITING_FOR_RESEARCH_FREQ:
researchFrequency = floatValue;
researchIndex = 0;
break;
case WAITING_FOR_MAX_ANGLE:
maxAngle = floatValue;
break;
case WAITING_FOR_ACCEL_DEVIATION:
accelDeviation = floatValue;
break;
// case WAITING_FOR_BOUNDARY_ANGLE:
// boundaryAngle = floatValue;
// break;
case WAITING_FOR_MAX_ANGVEL:
maxAngVel = floatValue;
Kd = (maxPwm - minPwm) / maxAngVel;
break;
default:
break;
}
break;
case '.':
while( i >= 0) {
floatValue += (str[i--] - '0') * d_st;
d_st *= my_pow;
}
d_st = 1.0/10;
my_pow = 1.0/10;
break;
default:
str[++i] = received;
break;
}
break;
}
}
}
void random_parking()
{
time_elapsed = 0;
distance_traveled = 0;
int spot = 0;
uint16_t distance_temp;
while (!Object(front2,13))
Motors_Forward(PWM,PWM);
Rotate_Left(23);
while (!Object(left,20))
Motors_Forward(PWM, PWM);
while (!Object(front2,14))
Wall_Follow(left,10);
Rotate_Right(23);
//Spot 6
distance_temp = distance_traveled;
while (Object(left,30))
{
if ((distance_traveled - distance_temp >= 33) && (distance_traveled - distance_temp <= 36) && !Object(right, 40))
{
spot = 6;
break;
}
Wall_Follow(left,14);
}
//Spot 5
if (!spot)
{
distance_temp = distance_traveled;
while(!Object(left,30))
{
if ((distance_traveled - distance_temp >= 13) && (distance_traveled - distance_temp <= 16) && !Object(right, 40))
{
spot = 5;
break;
}
Motors_Forward(PWM, PWM);
}
}
//Spot 4
if (!spot)
{
while(!Object(left,30))
Motors_Forward(PWM, PWM);
distance_temp = distance_traveled;
while(!Object(front2,15))
{
if ((distance_traveled - distance_temp >= 13) && (distance_traveled - distance_temp <= 16) && !Object(right, 40))
{
spot = 4;
break;
}
Wall_Follow(left,14);
}
}
if (!spot)
{
Rotate_Right(23);
while (!Object(right,20))
Motors_Forward(PWM,PWM);
while (Object(right,20))
Wall_Follow(right,9);
while (!Object(front2,15))
Motors_Forward(PWM, PWM);
Rotate_Right(23);
}
//Spot 3
if (!spot)
{
distance_temp = distance_traveled;
while (Object(left,30))
{
if ((distance_traveled - distance_temp >= 30) && (distance_traveled - distance_temp <= 33) && !Object(right, 40))
{
spot = 3;
break;
}
Wall_Follow(left,14);
}
}
//Spot 2
if (!spot)
{
distance_temp = distance_traveled;
while(!Object(left,30))
{
if ((distance_traveled - distance_temp >= 10) && (distance_traveled - distance_temp <= 13) && !Object(right, 40))
{
spot = 2;
break;
}
Motors_Forward(PWM, PWM);
}
}
//Spot 1
if (!spot)
{
while(!Object(left,30))
Motors_Forward(PWM, PWM);
distance_temp = distance_traveled;
while(!Object(front2,15))
{
if ((distance_traveled - distance_temp >= 10) && (distance_traveled - distance_temp <= 13) && !Object(right, 40))
{
spot = 1;
break;
}
Wall_Follow(left,14);
}
}
if (spot)
{
Rotate_Right(23);
while (!Object(left,30))
Motors_Forward(PWM,PWM);
while (!Object(front2,16))
Wall_Follow(right,3);
Motors_Stop();
delay_ms(200);
LEDs_Blink(12000);
switch (spot)
{
case 1:
while (!Object(back,10))
Motors_Back(PWM,PWM);
Rotate_Right(23);
while (Object(right,30))
Wall_Follow(right,14);
Go_Forward(12);
Rotate_Right(23);
break;
case 2:
Go_Back(40);
Rotate_Right(46);
break;
case 3:
while (!Object(back,10))
Motors_Back(PWM,PWM);
Rotate_Left(23);
while (Object(left,30))
Wall_Follow(left,14);
Go_Forward(14);
Rotate_Left(23);
break;
case 4:
while (!Object(back,10))
Motors_Back(PWM,PWM);
Rotate_Right(23);
while (Object(right,30))
Wall_Follow(right,14);
Go_Forward(14);
Rotate_Right(23);
break;
case 5:
Go_Back(40);
Rotate_Right(46);
break;
case 6:
while (!Object(back,10))
Motors_Back(PWM,PWM);
Rotate_Left(23);
while (Object(left,30))
Wall_Follow(left,14);
Go_Forward(14);
Rotate_Left(23);
break;
}
Go_Forward(50);
Motors_Stop();
}
}
void fixed_parking()
{
time_elapsed = 0;
distance_traveled = 0;
int spot = 0, exit = 0;
uint16_t distance_temp;
while (check_command(NOMESSAGE));
spot = UART_Read();
clear_last_received();
while (check_command(NOMESSAGE));
exit = UART_Read();
clear_last_received();
switch (spot)
{
case 1:
{
while (!Object(front2,13))
Motors_Forward(PWM,PWM);
Rotate_Right(23);
while (!Object(right,30))
Motors_Forward(PWM, PWM);
while (!Object(front2,15))
Wall_Follow(right,12);
Rotate_Left(23);
distance_temp = distance_traveled;
while (distance_traveled - distance_temp < 35)
Wall_Follow(right,14);
Rotate_Left(23);
while (!Object(left,30))
Motors_Forward(PWM,PWM);
while (!Object(front2,14))
Wall_Follow(left,3);
Motors_Stop();
LEDs_Blink(12000);
while (!Object(back,10))
Motors_Back(PWM,PWM);
switch (exit)
{
case 1:
Rotate_Right(23);
while (Object(right,30))
Wall_Follow(right,14);
Go_Forward(10);
Rotate_Right(23);
Go_Forward(50);
break;
case 2:
Rotate_Right(23);
while (Object(right,30))
Wall_Follow(right,14);
while(!Object(right,30))
Motors_Forward(PWM, PWM);
while(!Object(front2,15))
Wall_Follow(right,14);
Rotate_Left(23);
while (!Object(left,30))
Motors_Forward(PWM,PWM);
while (Object(right,30))
Wall_Follow(left,10);
Go_Forward(10);
Rotate_Right(23);
Go_Forward(50);
break;
case 3:
Rotate_Left(23);
while (!Object(front2,15))
Wall_Follow(left,14);
Rotate_Right(23);
while (!Object(right,30))
Motors_Forward(PWM,PWM);
while (Object(right,30))
Wall_Follow(right,10);
while (!Object(front2,15))
Motors_Forward(PWM, PWM);
Rotate_Right(23);
while (Object(left,30))
Wall_Follow(left,14);
Go_Forward(12);
Rotate_Left(23);
Go_Forward(50);
break;
}
break;
}
case 2:
{
while (!Object(front2,13))
Motors_Forward(PWM,PWM);
Rotate_Right(23);
while (!Object(right,30))
Motors_Forward(PWM, PWM);
while (!Object(front2,15))
Wall_Follow(right,12);
Rotate_Left(23);
while (Object(right,30))
Wall_Follow(right,14);
distance_temp = distance_traveled;
while (distance_traveled - distance_temp < 10)
Motors_Forward(PWM,PWM);
Rotate_Left(23);
while (!Object(left,30))
Motors_Forward(PWM,PWM);
while (!Object(front2,14))
Wall_Follow(left,3);
Motors_Stop();
LEDs_Blink(12000);
Go_Back(40);
switch (exit)
{
case 1:
Rotate_Left(46);
Go_Forward(50);
break;
case 2:
Rotate_Right(23);
while(!Object(right,30))
Motors_Forward(PWM, PWM);
while(!Object(front2,15))
Wall_Follow(right,14);
Rotate_Left(23);
while (!Object(left,30))
Motors_Forward(PWM,PWM);
while (Object(right,30))
Wall_Follow(left,10);
Go_Forward(10);
Rotate_Right(23);
Go_Forward(50);
break;
case 3:
Rotate_Left(23);
while (!Object(left,30))
Motors_Forward(PWM,PWM);
while (!Object(front2,15))
Wall_Follow(left,14);
Rotate_Right(23);
while (!Object(right,30))
Motors_Forward(PWM,PWM);
while (Object(right,20))
Wall_Follow(right,10);
while (!Object(front2,15))
Motors_Forward(PWM, PWM);
Rotate_Right(23);
while (Object(left,30))
Wall_Follow(left,14);
Go_Forward(12);
Rotate_Left(23);
Go_Forward(50);
break;
}
break;
}
case 3:
{
while (!Object(front2,13))
Motors_Forward(PWM,PWM);
Rotate_Right(23);
while (!Object(right,30))
Motors_Forward(PWM, PWM);
while (!Object(front2,15))
Wall_Follow(right,12);
Rotate_Left(23);
while (Object(right,30))
Wall_Follow(right,14);
while (!Object(right,30))
Motors_Forward(PWM,PWM);
distance_temp = distance_traveled;
while (distance_traveled - distance_temp < 10)
Wall_Follow(right,14);
Rotate_Left(23);
while (!Object(left,30))
Motors_Forward(PWM,PWM);
while (!Object(front2,14))
Wall_Follow(left,3);
Motors_Stop();
LEDs_Blink(12000);
while (!Object(back,10))
Motors_Back(PWM,PWM);
switch (exit)
{
case 1:
Rotate_Left(23);
while (Object(left,30))
Wall_Follow(left,14);
Go_Forward(10);
Rotate_Left(23);
Go_Forward(50);
break;
case 2:
Rotate_Right(23);
while(!Object(front2,15))
Wall_Follow(right,14);
Rotate_Left(23);
while (!Object(left,30))
Motors_Forward(PWM,PWM);
while (Object(right,30))
Wall_Follow(left,10);
Go_Forward(10);
Rotate_Right(23);
Go_Forward(50);
break;
case 3:
Rotate_Left(23);
while (Object(left,30))
Wall_Follow(left,14);
while (!Object(left,30))
Motors_Forward(PWM,PWM);
while (!Object(front2,15))
Wall_Follow(left,14);
Rotate_Right(23);
while (!Object(right,30))
Motors_Forward(PWM,PWM);
while (Object(right,30))
Wall_Follow(right,10);
while (!Object(front2,15))
Motors_Forward(PWM, PWM);
Rotate_Right(23);
while (Object(left,30))
Wall_Follow(left,14);
Go_Forward(12);
Rotate_Left(23);
Go_Forward(50);
break;
}
break;
}
case 4:
{
while (!Object(front2,13))
Motors_Forward(PWM,PWM);
Rotate_Left(23);
while (!Object(left,30))
Motors_Forward(PWM, PWM);
while (!Object(front2,15))
Wall_Follow(left,12);
Rotate_Right(23);
while (Object(left,30))
Wall_Follow(left,14);
while (!Object(left,30))
Motors_Forward(PWM,PWM);
distance_temp = distance_traveled;
while (distance_traveled - distance_temp < 13)
Wall_Follow(left,14);
Rotate_Right(23);
while (!Object(right,30))
Motors_Forward(PWM,PWM);
while (!Object(front2,14))
Wall_Follow(right,3);
Motors_Stop();
LEDs_Blink(12000);
while (!Object(back,10))
Motors_Back(PWM,PWM);
switch (exit)
{
case 1:
Rotate_Right(23);
while (Object(right,30))
Wall_Follow(right,14);
while (!Object(right,30))
Motors_Forward(PWM,PWM);
while (!Object(front2,15))
Wall_Follow(right,14);
Rotate_Left(23);
while (!Object(left,30))
Motors_Forward(PWM,PWM);
while (Object(left,30))
Wall_Follow(left,10);
while (!Object(front2,15))
Motors_Forward(PWM, PWM);
Rotate_Left(23);
while (Object(right,30))
Wall_Follow(right,14);
Go_Forward(12);
Rotate_Right(23);
Go_Forward(50);
break;
case 2:
Rotate_Left(23);
while(!Object(front2,15))
Wall_Follow(left,14);
Rotate_Right(23);
while (!Object(right,30))
Motors_Forward(PWM,PWM);
while (Object(left,30))
Wall_Follow(right,10);
Go_Forward(10);
Rotate_Left(23);
Go_Forward(50);
break;
case 3:
Rotate_Right(23);
while (Object(right,30))
Wall_Follow(right,14);
Go_Forward(10);
Rotate_Right(23);
Go_Forward(50);
break;
}
break;
}
case 5:
{
while (!Object(front2,13))
Motors_Forward(PWM,PWM);
Rotate_Left(23);
while (!Object(left,30))
Motors_Forward(PWM, PWM);
while (!Object(front2,15))
Wall_Follow(left,12);
Rotate_Right(23);
while (Object(left,30))
Wall_Follow(left,14);
distance_temp = distance_traveled;
while (distance_traveled - distance_temp < 10)
Motors_Forward(PWM,PWM);
Rotate_Right(23);
while (!Object(right,30))
Motors_Forward(PWM,PWM);
while (!Object(front2,14))
Wall_Follow(right,3);
Motors_Stop();
LEDs_Blink(12000);
Go_Back(40);
switch (exit)
{
case 1:
Rotate_Right(23);
while (!Object(right,30))
Motors_Forward(PWM,PWM);
while (!Object(front2,30))
Wall_Follow(right,14);
Rotate_Left(23);
while (!Object(left,30))
Motors_Forward(PWM,PWM);
while (Object(left,30))
Wall_Follow(left,10);
while (!Object(front2,15))
Motors_Forward(PWM, PWM);
Rotate_Left(23);
while (Object(right,30))
Wall_Follow(right,14);
Go_Forward(12);
Rotate_Right(23);
Go_Forward(50);
break;
case 2:
Rotate_Left(23);
while(!Object(left,30))
Motors_Forward(PWM, PWM);
while(!Object(front2,15))
Wall_Follow(left,14);
Rotate_Right(23);
while (!Object(right,30))
Motors_Forward(PWM,PWM);
while (Object(left,30))
Wall_Follow(right,10);
Go_Forward(10);
Rotate_Left(23);
Go_Forward(50);
break;
case 3:
Rotate_Right(46);
Go_Forward(50);
break;
}
break;
}
case 6:
{
while (!Object(front2,13))
Motors_Forward(PWM,PWM);
Rotate_Left(23);
while (!Object(left,30))
Motors_Forward(PWM, PWM);
while (!Object(front2,15))
Wall_Follow(left,12);
Rotate_Right(23);
distance_temp = distance_traveled;
while (distance_traveled - distance_temp < 33)
Wall_Follow(left,14);
Rotate_Right(23);
while (!Object(right,30))
Motors_Forward(PWM,PWM);
while (!Object(front2,14))
Wall_Follow(right,3);
Motors_Stop();
LEDs_Blink(12000);
while (!Object(back,10))
Motors_Back(PWM,PWM);
switch (exit)
{
case 1:
Rotate_Right(23);
while (!Object(front2,15))
Wall_Follow(right,14);
Rotate_Left(23);
while (!Object(left,30))
Motors_Forward(PWM,PWM);
while (Object(left,30))
Wall_Follow(left,10);
while (!Object(front2,15))
Motors_Forward(PWM, PWM);
Rotate_Left(23);
while (Object(right,30))
Wall_Follow(right,14);
Go_Forward(12);
Rotate_Right(23);
Go_Forward(50);
break;
case 2:
Rotate_Left(23);
while (Object(left,30))
Wall_Follow(left,14);
while(!Object(left,30))
Motors_Forward(PWM, PWM);
while(!Object(front2,15))
Wall_Follow(left,14);
Rotate_Right(23);
while (!Object(right,30))
Motors_Forward(PWM,PWM);
while (Object(left,30))
Wall_Follow(right,10);
Go_Forward(10);
Rotate_Left(23);
Go_Forward(50);
break;
case 3:
Rotate_Left(23);
while (Object(left,30))
Wall_Follow(left,14);
Go_Forward(10);
Rotate_Left(23);
Go_Forward(50);
break;
}
break;
}
}
Motors_Stop();
}
