//*****************************************************************************
//
//! 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;
}
