void hugOpstacle() {
while(!(inRange(440, 20) && turnCount == 0)) {
int right = Sensor_getDistance(Sensor_RIGHT);
int left = Sensor_getDistance(Sensor_LEFT);
int front = (Sensor_getDistance(Sensor_FRONT_LEFT) +
Sensor_getDistance(Sensor_FRONT_RIGHT)) / 2;
if(right < 10)
Motor_steerLeft(0.9);
else if(rechts < 30) {
if(voor > 20)
Motor_Forward();
else
Motor_TurnLeft();
}
else if(rechts < 40)
Motor_steerRight(0.9);
else
Motor_turnRight();
calculateTurnCount();
}
}
void Golfer_turn_left()
{
u08 L_speed, R_speed;
ADC_update(); //check the VR value
delay_ms(20);
L_speed = TURN_LEFT_LEFT_BASE - 2;
R_speed = TURN_LEFT_RIGHT_BASE + 2;
Motor_TurnLeft(L_speed, R_speed); //motor.c
while(1)
{
if((front_sensor & 0b00111100) == 0)
break;
}
while(1)
{
if(front_sensor == 0b00111000 || front_sensor == 0b00011000)
{
break;
}
}
Motor_stop();
defl_adjust();
}
void Golfer_turn_left_special()
{
u08 L_speed, R_speed;
ADC_update(); //check the VR value
delay_ms(20);
L_speed = TURN_LEFT_LEFT_BASE - 2;
R_speed = TURN_LEFT_RIGHT_BASE + 2;
Motor_TurnLeft(L_speed, R_speed); //motor.c
while(1)
{
if(back_sensor==0b00111000 || back_sensor==0b00011100 || back_sensor==0b00011000)
break;
}
Motor_stop();
// Motor_forward(VR[0] / ADJUST_SPEED_DENOMINATOR, VR[0] / ADJUST_SPEED_DENOMINATOR);
// while((middle_sensor & 0b00000010) == 0b00000000)
{
;
}
// Motor_stop();
Motor_TurnLeft(L_speed - 1, R_speed + 1);
while(1)
{
if((front_sensor & 0b00011000) == 0)
break;
}
while(1)
{
if(front_sensor == 0b00111000 || front_sensor == 0b00011000)
{
break;
}
}
Motor_stop();
}
void defl_adjust()
{
u08 L_speed, R_speed, tracking_speed;
s08 fro_patt, back_patt, defl;
//first phase
tracking_speed = VR[0] / SPEED_TURNING_DENOMINATOR;
L_speed = tracking_speed;
R_speed = tracking_speed;
fro_patt = patt_ana(front_sensor);
back_patt = patt_ana(back_sensor);
if(fro_patt < 4 && fro_patt > -4) //front sensor value valid
{
}
else //front sensor invalid
{
fro_patt = 0;
}
if(back_patt < 4 && back_patt > -4) //back sensor value valid
{
}
else //front sensor invalid
{
back_patt = 0;
}
defl = fro_patt - back_patt;
if(defl < 0) // left deflection
{
Motor_TurnLeft(L_speed, R_speed);
}
if(defl > 0) //right deflection
{
Motor_TurnRight(L_speed, R_speed);
}
while(1)
{
fro_patt = patt_ana(front_sensor);
back_patt = patt_ana(back_sensor);
if(fro_patt < 4 && fro_patt > -4) //front sensor value valid
{
;
}
else //front sensor invalid
{
Motor_stop();
return;
}
if(back_patt < 4 && back_patt > -4) //back sensor value valid
{
;
}
else //front sensor invalid
{
Motor_stop();
return;
}
defl = fro_patt - back_patt;
if (defl == 0)
break;
}
Motor_stop();
/////////////////////////////////////////////////////////////
///second phase
tracking_speed = VR[0] / ADJUST_SPEED_DENOMINATOR;
L_speed = tracking_speed;
R_speed = tracking_speed;
fro_patt = patt_ana(front_sensor);
back_patt = patt_ana(back_sensor);
if(fro_patt < 4 && fro_patt > -4) //front sensor value valid
{
}
else //front sensor invalid
{
Motor_stop();
return;
}
if(back_patt < 4 && back_patt > -4) //back sensor value valid
{
}
else //front sensor invalid
{
// Motor_TurnRight(13, 13);
// delay_ms(50);
// Motor_stop();
Motor_stop();
return;
}
defl = fro_patt - back_patt;
if(defl < 0) // left deflection
{
Motor_TurnLeft(L_speed, R_speed);
}
if(defl > 0) //right deflection
{
Motor_TurnRight(L_speed, R_speed);
}
while(1)
{
fro_patt = patt_ana(front_sensor);
back_patt = patt_ana(back_sensor);
if(fro_patt < 4 && fro_patt > -4) //front sensor value valid
{
;
}
else //front sensor invalid
{
Motor_stop();
return;
}
if(back_patt < 4 && back_patt > -4) //back sensor value valid
{
;
}
else //front sensor invalid
{
Motor_stop();
return;
}
defl = fro_patt - back_patt;
if (defl == 0)
break;
}
Motor_stop();
}
