//*****************************************************************************
//
//! Control robot to go straight forward by following wall
//!
//!
//! \return true if left/right wall is detected
//! false if no left/right wall is detected
//
static bool Forward()
{
LED1_OFF();LED2_ON();LED3_OFF();
//bluetooth_print("%d\r\n",avrSpeed);
if ((!isWallLeft) || (!isWallRight))
{
forwardUpdateByWall();
return true;
}
forwardUpdate();
if (avrSpeed<AVG_SPEED_FWD_FAST-20)
avrSpeed+=20;
else if (avrSpeed>AVG_SPEED_FWD_FAST)
avrSpeed=AVG_SPEED_FWD_FAST;
if (isWallRight)
{
pid_wallfollow(leftError,rightError, avrSpeed,WALL_FOLLOW_RIGHT);
}
else if (isWallLeft)
{
pid_wallfollow(leftError,rightError, avrSpeed,WALL_FOLLOW_LEFT);
}
else
{
speed_set(MOTOR_RIGHT, avrSpeed);
speed_set(MOTOR_LEFT, avrSpeed);
}
return false;
}
void pid_Wallfollow_process(void)
{
if (ControlFlag)
{
ControlFlag = false;
pid_wallfollow((float)IR_get_calib_value(IR_CALIB_BASE_LEFT) - (float)IR_GetIrDetectorValue(1), (float)IR_get_calib_value(IR_CALIB_BASE_RIGHT) - (float)IR_GetIrDetectorValue(2), 500);
bluetooth_print("IR: %d, %d, %d, %d\r\n", IR_GetIrDetectorValue(0), IR_GetIrDetectorValue(1), IR_GetIrDetectorValue(2), IR_GetIrDetectorValue(3));
}
}
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;
}
}
}
//*****************************************************************************
//
//! Control two motor to make robot turn back 180 degree.
//!
//! \param fwdPulse is the distance robot will go straight before turn right
//!, the robot will stand between the next cell of maze.
//! \param avrSpeedLeft is the speed of left motor.
//! \param avrSpeedRight is the speed of left motor.
//! \param NumPulse is the total pulse of two encoder after turn
//! \param resetEnc is the reset value for encoder after turning back
//! \return true if finish
//! false if not
//
static bool TurnBack(int fwdPulse, int avrSpeedLeft,int avrSpeedRight,int turnPulse,
int resetEnc)
{
LED1_ON();LED2_ON();LED3_ON();
switch (CtrlStep)
{
case 1:
{
posLeftTmp = qei_getPosLeft();
avrSpeedTmp = avrSpeed;
CtrlStep++;
}
case 2://go forward a litte bit
{
if (abs(qei_getPosLeft()-posLeftTmp)<fwdPulse)
{
avrSpeed = ((abs(fwdPulse + posLeftTmp - qei_getPosLeft()) / (fwdPulse / avrSpeedTmp)) / 2)
+ (abs(avrSpeedLeft) + abs(avrSpeedRight)) / 2;
if (isWallRight)
pid_wallfollow(leftError,rightError,avrSpeed,WALL_FOLLOW_RIGHT);
else if (isWallLeft)
pid_wallfollow(leftError,rightError,avrSpeed,WALL_FOLLOW_LEFT);
else
{
speed_set(MOTOR_RIGHT, avrSpeed);
speed_set(MOTOR_LEFT, avrSpeed);
}
}
else
{
pid_reset(&pid_wall_left);
pid_reset(&pid_wall_right);
forwardUpdate();
CtrlStep++;
avrSpeed = avrSpeedTmp;
}
break;
}
case 3:
posLeftTmp=qei_getPosLeft();
posRightTmp=qei_getPosRight();
CtrlStep++;
case 4://turing 90 degree
{
#ifdef TEST_TURNBACK_FWD
speed_Enable_Hbridge(false);
#endif
if ((abs(qei_getPosLeft()-posLeftTmp)+abs(qei_getPosRight()-posRightTmp))<turnPulse)
{
speed_set(MOTOR_RIGHT, avrSpeedRight);
speed_set(MOTOR_LEFT, avrSpeedLeft);
}
else
{
currentDir=(currentDir+3)%4;
CtrlStep++;
}
break;
}
case 5:
posLeftTmp=qei_getPosLeft();
posRightTmp=qei_getPosRight();
CtrlStep++;
case 6://turning another 90 degree
{
#ifdef TEST_TURNBACK_TURN1
speed_Enable_Hbridge(false);
#endif
if ((abs(qei_getPosLeft()-posLeftTmp)+abs(qei_getPosRight()-posRightTmp))<turnPulse)
{
speed_set(MOTOR_RIGHT, -avrSpeedLeft);
speed_set(MOTOR_LEFT, -avrSpeedRight);
}
else
{
#ifdef TEST_TURNBACK_TURN2
speed_Enable_Hbridge(false);
#endif
currentDir=(currentDir+3)%4;
clearPosition();
qei_setPosLeft(resetEnc);
qei_setPosRight(resetEnc);
forwardUpdate();
CtrlStep=1;
return true;
}
break;
}
}
return false;
}
//*****************************************************************************
//
//! Control two motor to make robot turn left 90 degree
//!
//! \param fwdPulse is the distance robot will go straight before turn right
//!, the robot will stand between the next cell of maze.
//! \param avrSpeedLeft is the speed of left motor.
//! \param avrSpeedRight is the speed of right motor.
//! \param turnPulse is the total pulse of two encoder after turn
//! \param resetEnc is reset value for encoder after turning 90 degree, ignore this if you don't want to estimate position
//! \return true if finish
//! false if not
//
//*****************************************************************************
static bool TurnLeft(int fwdPulse,int avrSpeedLeft,int avrSpeedRight,int turnPulse,
int resetEnc)
{
static int vt,vp;
LED1_ON();LED2_OFF();LED3_OFF();
// bluetooth_print("LS %d\r\n",CtrlStep);
switch (CtrlStep)
{
case 1:
posLeftTmp=qei_getPosLeft();
CtrlStep++;
avrSpeedTmp=avrSpeed;
case 2://go straight
if ((abs(qei_getPosLeft()-posLeftTmp)<fwdPulse) ||
(isWallFrontLeft && isWallFrontRight &&
(IR_GetIrDetectorValue(3)>IR_get_calib_value(IR_CALIB_BASE_FRONT_RIGHT))&&
(IR_GetIrDetectorValue(0)>IR_get_calib_value(IR_CALIB_BASE_FRONT_LEFT))))
{
if (qei_getPosLeft()<fwdPulse+posLeftTmp)
avrSpeed = ((abs(fwdPulse + posLeftTmp - qei_getPosLeft()) / (fwdPulse / avrSpeedTmp)) / 2)
+ (abs(avrSpeedLeft) + abs(avrSpeedRight)) / 2;
else
avrSpeed = (abs(avrSpeedLeft) + abs(avrSpeedRight)) / 2;
if (isWallRight)
pid_wallfollow(leftError,rightError,avrSpeed,WALL_FOLLOW_RIGHT);
else
{
speed_set(MOTOR_RIGHT, avrSpeed);
speed_set(MOTOR_LEFT, avrSpeed);
}
}
else
{
#ifdef TEST_TURNLEFT_MOVE1
speed_Enable_Hbridge(false);
#endif
pid_reset(&pid_wall_right);
pid_reset(&pid_wall_left);
forwardUpdate();
CtrlStep++;
avrSpeed=avrSpeedTmp;
}
break;
case 3:
posLeftTmp=qei_getPosLeft();
posRightTmp=qei_getPosRight();
CtrlStep++;
vp=1;
vt=1;
case 4://turn 90 degree
if (abs(qei_getPosLeft()-posLeftTmp) + abs(qei_getPosRight()-posRightTmp) < turnPulse)
{
speed_set(MOTOR_RIGHT, avrSpeedRight);
speed_set(MOTOR_LEFT, -avrSpeedLeft);
if((abs(qei_getPosRight()-posRightTmp)>(turnPulse*0.8*vp/8)) && (vp<9))
{
if (avrSpeedRight>=24)
avrSpeedRight-=24;
vp++;
}
if((abs(qei_getPosLeft()-posLeftTmp)>(turnPulse*0.2*vt/8)) && (vt<9))
{
if (avrSpeedLeft>=4)
avrSpeedLeft-=4;
vt++;
}
}
else
{
#ifdef TEST_TURNLEFT_TURN
speed_Enable_Hbridge(false);
#endif
currentDir=(currentDir+3)%4;
clearPosition();
qei_setPosLeft(resetEnc);
qei_setPosRight(resetEnc);
forwardUpdate();
CtrlStep=1;
pid_reset(&pid_wall_left);
pid_reset(&pid_wall_right);
speed_set(MOTOR_LEFT, avrSpeed);
speed_set(MOTOR_RIGHT, avrSpeed);
return true;
}
break;
}
return false;
}