//*****************************************************************************
//
//! Move robot a little bit (error +-500 pulses). Robot velocity is equal 0 after moving.
//! Make sure this function return true before calling it again.
//!
//! \param deltaLeft distance left motor will go
//! \param deltaRight distance right motor will go
//! \param velLeftMax max left velocity
//! \param velRightMax max right velocity
//!
//! \return true if done
//
//*****************************************************************************
static bool move(int deltaLeft,int deltaRight,int velLeftMax, int velRightMax)
{
static int origLeft, origRight;
static bool done = true;
int currentLeft=qei_getPosLeft();
int currentRight=qei_getPosRight();
if (done)
{
done=false;
origLeft=currentLeft;
origRight=currentRight;
}
//bluetooth_print("move: %5d %5d\r\n",origLeft,currentLeft);
if (abs(origLeft+deltaLeft-currentLeft)>500)
{
pid_posLeft.PID_Saturation = velLeftMax;
speed_set(MOTOR_LEFT, pid_process(&pid_posLeft,origLeft+deltaLeft-currentLeft));
pid_posRight.PID_Saturation = velRightMax;
speed_set(MOTOR_RIGHT, pid_process(&pid_posRight,origRight+deltaRight-currentRight));
done=false;
}
else
{
done = true;
pid_reset(&pid_posLeft);
pid_reset(&pid_posRight);
}
#ifdef _DEBUG_MOVE
bluetooth_print("move: %5d %5d %5d %5d\r\n",(int)left, (int)right,(int)pid_posLeft.u,(int)pid_posRight.u);
#endif
return done;
}
//*****************************************************************************
//
//! 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;
}
/**
* @brief Wall follow controller
*/
static bool pid_wallfollow(float delta_IR_left, float delta_IR_right, float averageSpeed)
{
static float error, u;
int32_t set_speed[2];
switch (e_wall_follow_select)
{
case WALL_FOLLOW_NONE: //Do nothing
return true;
case WALL_FOLLOW_LEFT:
error = delta_IR_left;
break;
case WALL_FOLLOW_RIGHT:
error = delta_IR_right;
break;
case WALL_FOLLOW_BOTH:
error = delta_IR_left - delta_IR_right;
break;
default:
return false;
}
u = pid_process(error);
set_speed[0] = averageSpeed + (int32_t)(u / 2);
set_speed[1] = averageSpeed - (int32_t)(u / 2);
speed_set(MOTOR_RIGHT, set_speed[0]);
speed_set(MOTOR_LEFT, set_speed[1]);
return true;
}
static void
tour1d_speed_set_display(gdouble slidepos, displayd *dsp)
{
cpaneld *cpanel = &dsp->cpanel;
cpanel->t1d.slidepos = slidepos;
speed_set(slidepos, &cpanel->t1d.step, &dsp->t1d.delta);
}
void ButtonHandler(void)
{
switch (system_GetState())
{
case SYSTEM_INITIALIZE:
speed_Enable_Hbridge(false);
system_SetState(SYSTEM_CALIB_SENSOR);
IR_Calib_Step = 0;
LED1_ON();
LED2_ON();
LED3_ON();
break;
case SYSTEM_CALIB_SENSOR:
speed_Enable_Hbridge(false);
system_SetState(SYSTEM_SAVE_CALIB_SENSOR);
case SYSTEM_SAVE_CALIB_SENSOR:
system_SetState(SYSTEM_ESTIMATE_MOTOR_MODEL);
speed_Enable_Hbridge(true);
speed_set(MOTOR_LEFT,500);
speed_set(MOTOR_RIGHT, 500);
break;
case SYSTEM_ESTIMATE_MOTOR_MODEL:
// system_SetState(SYSTEM_SAVE_MOTOR_MODEL);
system_SetState(SYSTEM_WAIT_TO_RUN);
speed_Enable_Hbridge(false);
break;
case SYSTEM_WAIT_TO_RUN:
speed_Enable_Hbridge(true);
system_SetState(SYSTEM_RUN_SOLVE_MAZE);
break;
case SYSTEM_RUN_SOLVE_MAZE:
case SYSTEM_RUN_IMAGE_PROCESSING:
system_SetState(SYSTEM_WAIT_TO_RUN);
speed_Enable_Hbridge(false);
break;
default:
break;
}
}
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;
}
/**
* @brief Wall follow controller
*/
static bool pid_wallfollow(float delta_IR_left, float delta_IR_right, float averageSpeed,
WALL_FOLLOW_SELECT wall_follow_select)
{
static float error, u, rightFirst=1;
static WALL_FOLLOW_SELECT preSelect=WALL_FOLLOW_NONE;
int32_t set_speed[2];
if (preSelect!=WALL_FOLLOW_NONE)
if (preSelect!=wall_follow_select)
{
if (preSelect==WALL_FOLLOW_RIGHT)
pid_reset(&pid_wall_right);
else if (preSelect==WALL_FOLLOW_LEFT)
pid_reset(&pid_wall_left);
}
switch (wall_follow_select)
{
case WALL_FOLLOW_NONE: //Do nothing
return true;
case WALL_FOLLOW_LEFT:
{
error = -delta_IR_left;
u = pid_process(&pid_wall_left,error);
}
break;
case WALL_FOLLOW_RIGHT:
{
error = delta_IR_right;
u = pid_process(&pid_wall_right,error);
}
break;
case WALL_FOLLOW_BOTH:
{
error = delta_IR_right - delta_IR_left;
u = pid_process(&pid_wall_right,error);
}
break;
case WALL_FOLLOW_AUTO:
if (rightFirst)
{
if (isWallRight)
{
error = delta_IR_right;
u = pid_process(&pid_wall_right,error);
}
else if (isWallLeft)
{
pid_reset(&pid_wall_right);
error = -delta_IR_left;
u = pid_process(&pid_wall_left,error);
rightFirst=0;
}
}
else
{
if (isWallLeft)
{
error = -delta_IR_left;
u = pid_process(&pid_wall_left,error);
}
else if (isWallRight)
{
pid_reset(&pid_wall_left);
error = delta_IR_right;
u = pid_process(&pid_wall_right,error);
rightFirst=1;
}
}
break;
default:
return false;
}
preSelect = wall_follow_select;
set_speed[0] = averageSpeed + (int32_t)(u / 2);
set_speed[1] = averageSpeed - (int32_t)(u / 2);
speed_set(MOTOR_RIGHT, set_speed[0]);
speed_set(MOTOR_LEFT, set_speed[1]);
return true;
}
void usb_iso_received()
{
// Byte 0-3: First packet control bits (0xAAAAAAAA)
// Byte 4-5: Scanning speed in pps (big endian)
// Byte 6-7: Size of frame in points (big endian)
// Byte 8-X: Point data
// Each point: 8 MSB of x, 4 LSB of x + 4 MSB of y, 8 LSB of y, r, g, b, i (7 bytes total)
// Byte X-4: Last packet control bits (0xBBBBBBBB)
uint8_t* data = usbIsoBufferAddress;
bool processPacket = false;
bool lastPacket = false;
uint8_t intraFramePos = 0;
if (!newFrameReady)
{
//if control bits indicate first packet in frame
if ( (data[0] == 0xAA) &&
(data[1] == 0xAA) &&
(data[2] == 0xAA) &&
(data[3] == 0xAA) )
{
outputSpeed = ( (data[4] << 8) + data[5] );
newFrameSize = ( ((data[6] << 8) + data[7]) * 7 + 4);
if ((outputSpeed > MAXSPEED) || (outputSpeed < MINSPEED))
{
//error: wrong speed
}
else if (newFrameSize > MAXFRAMESIZE)
{
//error: wrong size
}
else
{
intraFramePos = 8;
newFramePos = 0;
processPacket = true;
}
}
else if (newFramePos != 0)
{
processPacket = true;
}
}
if (processPacket)
{
uint16_t bytesToCopy = 512 - intraFramePos;
//if last packet in frame expected
if ( (newFrameSize - newFramePos) <= bytesToCopy )
{
//adjust copy size and position
bytesToCopy = newFrameSize - newFramePos;
lastPacket = true;
}
//TODO copy data, below works?
memcpy(&newFrameAddress[newFramePos], &data[intraFramePos], bytesToCopy);
newFramePos += bytesToCopy;
//if last packet in frame expected
if (lastPacket)
{
//if control bytes indicates last packet in frame
uint8_t* frameEnd = newFrameAddress+newFrameSize;
if ( ( *(frameEnd-0) == 0xBB) &&
( *(frameEnd-1) == 0xBB) &&
( *(frameEnd-2) == 0xBB) &&
( *(frameEnd-3) == 0xBB) )
{
//frame successfully received
newFrameReady = true;
if (!playing)
{
playing = true;
speed_set(outputSpeed);
}
}
else
{
//faulty frame, discard
newFramePos = 0;
}
}
}
udi_vendor_iso_out_run(usbIsoBufferAddress, 512, usb_iso_received);
}
