void system_Process_System_State(void) { switch (system_GetState()) { case SYSTEM_POWER_UP: break; case SYSTEM_INITIALIZE: break; case SYSTEM_CALIB_SENSOR: break; case SYSTEM_SAVE_CALIB_SENSOR: break; case SYSTEM_ESTIMATE_MOTOR_MODEL: ProcessSpeedControl(); break; case SYSTEM_SAVE_MOTOR_MODEL: break; case SYSTEM_WAIT_TO_RUN: break; case SYSTEM_RUN_SOLVE_MAZE: pid_Wallfollow_process(); ProcessSpeedControl(); break; case SYSTEM_RUN_IMAGE_PROCESSING: LED1_ON(); ProcessSpeedControl(); break; case SYSTEM_ERROR: speed_Enable_Hbridge(false); system_Enable_BoostCircuit(false); IntMasterDisable(); while (1) { LED1_ON(); LED2_ON(); LED3_ON(); ROM_SysCtlDelay(ROM_SysCtlClockGet() / 3); LED1_OFF(); LED2_OFF(); LED3_OFF(); ROM_SysCtlDelay(ROM_SysCtlClockGet() / 3); } // break; } }
void system_Process_System_State(void) { switch (system_GetState()) { case SYSTEM_POWER_UP: break; case SYSTEM_INITIALIZE: break; case SYSTEM_ESTIMATE_MOTOR_MODEL: break; case SYSTEM_SAVE_MOTOR_MODEL: break; case SYSTEM_WAIT_TO_RUN: break; case SYSTEM_RUN_BALANCE: loop(); break; case SYSTEM_RUN_IMAGE_PROCESSING: LED1_ON(); break; case SYSTEM_ERROR: speed_Enable_Hbridge(false); system_Enable_BoostCircuit(false); IntMasterDisable(); while (1) { LED1_ON(); LED2_ON(); LED3_ON(); ROM_SysCtlDelay(ROM_SysCtlClockGet() / 3); LED1_OFF(); LED2_OFF(); LED3_OFF(); ROM_SysCtlDelay(ROM_SysCtlClockGet() / 3); } } }
/** * @brief Control speed * @param select motor select * @param speed motor speed (encoder pulse / 20ms) */ void speed_set(MOTOR_SELECT Select, int32_t speed) { //speed_Enable_Hbridge(true); if (Select == MOTOR_RIGHT) { if (SetPoint[0] != speed) { SetPoint[0] = speed; speed_control_runtimeout(20); } } else if (Select == MOTOR_LEFT) { if (SetPoint[1] != speed) { SetPoint[1] = speed; speed_control_runtimeout(20); } } if (SetPoint[0]==0 && SetPoint[1]==0 ) { speed_Enable_Hbridge(false); } }
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; }