//***************************************************************************** // //! Update robot position when moving in straight line //! //! \param none //! //! //! \return none // //***************************************************************************** static void forwardUpdate() { if (qei_getPosLeft()-encLeftTmp>CELL_ENC) { encLeftTmp += CELL_ENC; switch (currentDir) { case 0: robotY++; break; case 1: robotX++; break; case 2: robotY--; break; case 3: robotX--; break; } #ifdef _DEBUG_POS_ bluetooth_print("pos %d %d %d",robotX,robotY,currentDir); #endif } }
//***************************************************************************** // //! 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; }
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)); } }
/** * @brief Init battery sense * @note this function must call to calculate speed control */ void ProcessSpeedControl(void) { int32_t Velocity[2]; // SetPoint = 250; if (qei_getVelocity(0, &Velocity[0]) == true) { udk = STR_Indirect(Theta, RealSpeedSet[0], Velocity[0]); SetPWM(PWM_MOTOR_RIGHT, DEFAULT, udk); Uocluong(udk, Velocity[0], Theta, Theta_); #ifdef _DEBUG_SPEED_ bluetooth_print("Right: %d\r\n", Velocity[0]); #endif } if (qei_getVelocity(1, &Velocity[1]) == true) { udk = STR_Indirect2(Theta2, RealSpeedSet[1], Velocity[1]); SetPWM(PWM_MOTOR_LEFT, DEFAULT, udk); Uocluong2(udk, Velocity[1], Theta2, Theta2_); #ifdef _DEBUG_SPEED_ bluetooth_print("Left: %d\r\n", Velocity[1]); #endif } }
void IR_load_calib_value(int* IRData) { ir_calib_values.BaseFrontLeft=IRData[0]; bluetooth_print("IR_CALIB_BASE_FRONT_LEFT: %d\r\n", ir_calib_values.BaseFrontLeft); ir_calib_values.BaseFrontRight=IRData[1]; bluetooth_print("IR_CALIB_BASE_FRONT_RIGHT: %d\r\n", ir_calib_values.BaseFrontRight); ir_calib_values.BaseLeft=IRData[2]; bluetooth_print("IR_CALIB_BASE_LEFT: %d\r\n", ir_calib_values.BaseLeft); ir_calib_values.BaseRight=IRData[3]; bluetooth_print("IR_CALIB_BASE_RIGHT: %d\r\n", ir_calib_values.BaseRight); ir_calib_values.MaxFrontLeft=IRData[4]; bluetooth_print("IR_CALIB_MAX_FRONT_LEFT: %d\r\n", ir_calib_values.MaxFrontLeft); ir_calib_values.MaxFrontRight=IRData[5]; bluetooth_print("IR_CALIB_MAX_FRONT_RIGHT: %d\r\n", ir_calib_values.MaxFrontRight); ir_calib_values.MaxLeft=IRData[6]; bluetooth_print("IR_CALIB_MAX_LEFT: %d\r\n", ir_calib_values.MaxLeft); ir_calib_values.MaxRight=IRData[7]; bluetooth_print("IR_CALIB_MAX_RIGHT: %d\r\n", ir_calib_values.MaxRight); }
bool IR_set_calib_value(IR_CALIB select) { switch (select) { case IR_CALIB_BASE_FRONT_LEFT: ir_calib_values.BaseFrontLeft = IR_GetIrDetectorValue(0); bluetooth_print("IR_CALIB_BASE_FRONT_LEFT: %d\r\n", ir_calib_values.BaseFrontLeft); break; case IR_CALIB_BASE_FRONT_RIGHT: ir_calib_values.BaseFrontRight = IR_GetIrDetectorValue(3); bluetooth_print("IR_CALIB_BASE_FRONT_RIGHT: %d\r\n", ir_calib_values.BaseFrontRight); break; case IR_CALIB_BASE_LEFT: ir_calib_values.BaseLeft = IR_GetIrDetectorValue(1); bluetooth_print("IR_CALIB_BASE_LEFT: %d\r\n", ir_calib_values.BaseLeft); break; case IR_CALIB_BASE_RIGHT: ir_calib_values.BaseRight = IR_GetIrDetectorValue(2); bluetooth_print("IR_CALIB_BASE_RIGHT: %d\r\n", ir_calib_values.BaseRight); break; case IR_CALIB_MAX_FRONT_LEFT: ir_calib_values.MaxFrontLeft = IR_GetIrDetectorValue(0); bluetooth_print("IR_CALIB_MAX_FRONT_LEFT: %d\r\n", ir_calib_values.MaxFrontLeft); break; case IR_CALIB_MAX_FRONT_RIGHT: ir_calib_values.MaxFrontRight = IR_GetIrDetectorValue(3); bluetooth_print("IR_CALIB_MAX_FRONT_RIGHT: %d\r\n", ir_calib_values.MaxFrontRight); break; case IR_CALIB_MAX_LEFT: ir_calib_values.MaxLeft = IR_GetIrDetectorValue(1); bluetooth_print("IR_CALIB_MAX_LEFT: %d\r\n", ir_calib_values.MaxLeft); break; case IR_CALIB_MAX_RIGHT: ir_calib_values.MaxRight = IR_GetIrDetectorValue(2); bluetooth_print("IR_CALIB_MAX_RIGHT: %d\r\n", ir_calib_values.MaxRight); break; default: return false; } return true; }