/**
* @brief Init Wall follow controller
* @param pid param
*/
void pid_Wallfollow_init(PID_PARAMETERS pid_param)
{
pid_init();
pid_set_parameters(pid_param);
// if (pid_TimerID != 0xff)
// {
// TIMER_UnregisterEvent(pid_TimerID);
// }
pid_Runtimeout(&Pid_process_callback, pid_param.Ts);
ui32_msLoop = pid_param.Ts;
}
void pid_Wallfollow_process(void)
{
if (ControlFlag)
{
static int i;
pid_Runtimeout(&pid_process_callback, ui32_msLoop);
ControlFlag = false;
leftError=(float)IR_get_calib_value(IR_CALIB_BASE_LEFT) - (float)IR_GetIrDetectorValue(1);
rightError=(float)IR_get_calib_value(IR_CALIB_BASE_RIGHT) - (float)IR_GetIrDetectorValue(2);
isWallLeft = IR_GetIrDetectorValue(1)<IR_get_calib_value(IR_CALIB_MAX_LEFT);
isWallRight = IR_GetIrDetectorValue(2)<IR_get_calib_value(IR_CALIB_MAX_RIGHT);
isWallFrontLeft = IR_GetIrDetectorValue(0)<IR_get_calib_value(IR_CALIB_MAX_FRONT_LEFT);
isWallFrontRight = IR_GetIrDetectorValue(3)<IR_get_calib_value(IR_CALIB_MAX_FRONT_RIGHT);
switch(eMove)
{
case FORWARD:
switch (moveStage)
{
case 1:
if (Forward())
moveStage++;
if (isWallFrontLeft| isWallFrontRight)
{
preMove=eMove;
eMove=getMove(isWallLeft,isWallFrontLeft|isWallFrontRight,isWallRight);
}
break;
case 2:
posLeftTmp=qei_getPosLeft();
moveStage++;
i=1;
avrSpeedTmp=avrSpeed;
case 3://slow down
forwardUpdate();
if (!isWallLeft)
{
rqTurnLeft=true;
}
if (!isWallRight)
{
rqTurnRight=true;
}
if ((abs(qei_getPosLeft()-posLeftTmp)<5000)
&& (!isWallFrontLeft) && (!isWallFrontRight))
{
if ((abs(qei_getPosLeft()-posLeftTmp)>i*500) && (avrSpeed>AVG_SPEED_FWD-40))
{
avrSpeed -= 10;
i++;
}
if (isWallLeft|isWallRight)
pid_wallfollow(leftError,rightError, avrSpeed,WALL_FOLLOW_AUTO);
else
{
pid_reset(&pid_wall_left);
pid_reset(&pid_wall_right);
speed_set(MOTOR_RIGHT, avrSpeed);
speed_set(MOTOR_LEFT, avrSpeed);
}
}
else
{
#ifdef TEST_FORWARD_MOVE
speed_Enable_Hbridge(false);
#endif
preMove=eMove;
eMove=getMove(!rqTurnLeft,isWallFrontLeft|isWallFrontRight,!rqTurnRight);
if (eMove==FORWARD)
avrSpeed=AVG_SPEED_FWD;
rqTurnLeft=false;
rqTurnRight=false;
moveStage=1;
}
break;
}
break;
case TURN_LEFT:
switch (moveStage)
{
case 1:
if (preMove!=FORWARD)//after turning left or right
//test
// ____
// | |
// | | |
fwdPulse=6000;
else if ((preMove==FORWARD) && (avrSpeed<AVG_SPEED_FWD_FAST))
//after turning back
//test
// ___
// |__ |
// |__|
fwdPulse=5500;
else//after moving forward
//test
// ___
// ___ |
// | |
// |_|
fwdPulse=4500;
moveStage++;
case 2:
if (TurnLeft(fwdPulse,60,240,7800,1700+CELL_ENC))
{
moveStage++;
}
break;
case 3:
posLeftTmp=qei_getPosLeft();
moveStage++;
case 4:
//go straight a little bit to check wall
forwardUpdate();
if (abs(qei_getPosLeft()-posLeftTmp)<2000)
{
if (abs(qei_getPosLeft()-posLeftTmp)>1500)
{
if (!isWallRight)
rqTurnRight=1;
if (!isWallLeft)
rqTurnLeft=1;
}
avrSpeed=AVG_SPEED_FWD_SLOW;
if (isWallLeft|isWallRight)
pid_wallfollow(leftError,rightError, avrSpeed,WALL_FOLLOW_AUTO);
else
{
pid_reset(&pid_wall_left);
pid_reset(&pid_wall_right);
speed_set(MOTOR_RIGHT, avrSpeed);//left motor is faster
speed_set(MOTOR_LEFT, avrSpeed);
}
}
else
{
#ifdef TEST_TURNLEFT_MOVE3
speed_Enable_Hbridge(false);
#endif
//time to check front wall
preMove=eMove;
eMove=getMove(!rqTurnLeft,isWallFrontLeft|isWallFrontRight,!rqTurnRight);
rqTurnLeft=false;
rqTurnRight=false;
moveStage=1;
pid_reset(&pid_wall_left);
pid_reset(&pid_wall_right);
}
}
break;
case TURN_RIGHT:
switch (moveStage)
{
case 1:
if (preMove!=FORWARD)//after turning left or right
//test
// ____
// | |
// | | |
fwdPulse=6000;
else if ((preMove==FORWARD) && (avrSpeed<AVG_SPEED_FWD_FAST))
//after turning back
//test
// ____
// | ____
// |__|
fwdPulse=5500;
else//after moving forward
//test
// _____
// | _____
// | |
// |_|
fwdPulse=4500;
moveStage++;
case 2:
if (TurnRight(fwdPulse,200,40,8000,1700+CELL_ENC))
{
moveStage++;
}
break;
case 3:
posLeftTmp=qei_getPosLeft();
moveStage++;
case 4:
forwardUpdate();
if (abs(qei_getPosLeft()-posLeftTmp)<1000)
{
if (abs(qei_getPosLeft()-posLeftTmp)>500)
{
if (!isWallRight)
rqTurnRight=1;
if (!isWallLeft)
rqTurnLeft=1;
}
avrSpeed=AVG_SPEED_FWD_SLOW;
if (isWallLeft|isWallRight)
pid_wallfollow(leftError,rightError, avrSpeed,WALL_FOLLOW_AUTO);
else
{
pid_reset(&pid_wall_left);
pid_reset(&pid_wall_right);
speed_set(MOTOR_RIGHT, avrSpeed);
speed_set(MOTOR_LEFT, avrSpeed);
}
}
else
{
#ifdef TEST_TURNRIGHT_MOVE3
speed_Enable_Hbridge(false);
#endif
preMove=eMove;
eMove=getMove(!rqTurnLeft,isWallFrontLeft|isWallFrontRight,!rqTurnRight);
rqTurnLeft=false;
rqTurnRight=false;
moveStage=1;
pid_reset(&pid_wall_left);
pid_reset(&pid_wall_right);
}
}
break;
case TURN_BACK:
switch (moveStage)
{
case 1:
if (preMove==FORWARD)
fwdPulse=8000;
else
fwdPulse=9000;
moveStage++;
case 2:
if (TurnBack(fwdPulse,-140,60,8000,13000))
{
//rotate more if we still detect front wall: do it yourself ;D
moveStage++;
}
break;
case 3:
if (move(-9000,-9000,AVG_SPEED_BWD,AVG_SPEED_BWD))
{
#ifdef TEST_TURNBACK_BACKWARD
speed_Enable_Hbridge(false);
#endif
forwardUpdate();
avrSpeed = AVG_SPEED_FWD_SLOW;
preMove=eMove;
eMove=FORWARD;
moveStage = 1;
}
}
break;
}
}
}
static void pid_process_callback(void)
{
pid_TimerID = INVALID_TIMER_ID;
ControlFlag = true;
pid_Runtimeout(&pid_process_callback, ui32_msLoop);
}
/**
* @brief Init Wall follow controller
*/
static void pid_Wallfollow_init()
{
ui32_msLoop = pid_wall_left.Ts * 1000;
pid_Runtimeout(&pid_process_callback, ui32_msLoop);
}
