//*****************************************************************************
//
//! 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 robot_cs_update(void* dummy)
{
double vx_t,vy_t,omegaz_t;
double vx_r,vy_r;
double alpha;
double _ca,_sa;
hrobot_vector_t hvec;
robot_cs_t *rcs = dummy;
// if CS inactivated, just quit
if(!rcs->active)
return;
// if CS was previously inactive, we need a little hack for quadramps
if(rcs->reactivated)
{
int32_t consign;
pid_reset(&pid_x);
pid_reset(&pid_y);
pid_reset(&pid_angle);
consign = cs_get_consign(&csm_angle);
qramp_angle.previous_var = 0;
qramp_angle.previous_out = consign;
qramp_angle.previous_in = consign;
rcs->reactivated = 0;
}
// compute control system first level (x,y,a)
cs_manage(&csm_x);
cs_manage(&csm_y);
cs_manage(&csm_angle);
// transform output vector from table coords to robot coords
vx_t = cs_get_out(&csm_x);
vy_t = cs_get_out(&csm_y);
omegaz_t = cs_get_out(&csm_angle);
hposition_get(rcs->hpm, &hvec);
alpha = -hvec.alpha;
_ca = cos(alpha);
_sa = sin(alpha);
vx_r = vx_t*_ca - vy_t*_sa;
vy_r = vx_t*_sa + vy_t*_ca;
// set second level consigns
hrobot_set_motors(rcs->hrs, vx_r, vy_r, omegaz_t);
}
void charger_reset(CHARGER& charger, int pwm){
charger.pwm=(double)pwm;
analogWrite(P_PWM,pwm);
delay(TIMER_TICK);
digitalWrite(P_SW, LOW);
pid_reset(pid);
}
static void do_set_enabled(bool enabled)
{
contrtemp.enabled = enabled;
/* Reset the temp controller. */
pid_reset(&contrtemp.pid);
timer_set_now(&contrtemp.dt_timer);
contrtemp_set_boost_mode(TEMPBOOST_NORMAL);
/* Reset current controller to no-current. */
contrcurr_set_setpoint(float_to_fixpt(CONTRCURR_NEGLIM));
}
int32 pid_set_target(uint8 nr, pid_type target)
{
if (nr > PID_NR - 1) return -1;
if (target < CRITICAL_SPEED * hall_get_res())
{
pid_reset(nr);
}
pid[nr].target = target * hall_get_res();
return 0;
}
void beacon_calibre_pos(void)
{
sensorboard.flags &= ~DO_CS;
/* init beacon pos */
pwm_ng_set(BEACON_PWM, 100);
/* find rising edge of the mirror*/
wait_ms(100);
while (sensor_get(BEACON_POS_SENSOR));
wait_ms(100);
while (!sensor_get(BEACON_POS_SENSOR));
pwm_ng_set(BEACON_PWM, 0);
beacon_reset_pos();
pid_reset(&sensorboard.beacon.pid);
encoders_spi_set_value_beacon(BEACON_ENCODER, BEACON_OFFSET_CALIBRE);
cs_set_consign(&sensorboard.beacon.cs, 0);
sensorboard.flags |= DO_CS;
}
/*************************************************
名称：calculate(float pitch, float roll, float yaw, u8 throttle, u8 rudder, u8 elevator, u8 aileron, float gx, float gy, float gz)
功能：系统pid运算
输入参数：
float pitch 当前俯仰角度
float roll 当前倾斜角度
float yaw 当前偏航角度
u8 throttle 油门给定值
u8 rudder 偏航给定值
u8 elevator 俯仰给定值
u8 aileron 倾斜给定值
float gx x轴角速度
float gy y轴角速度
float gz z轴角速度
输出参数：电机控制量
返回值： 无
**************************************************/
void calculate(float pitch, float roll, float yaw, u8 throttle, u8 rudder, u8 elevator, u8 aileron, float gx, float gy, float gz)
{
u16 thr = 0;
s16 pitch_out = 0, roll_out = 0, yaw_out = 0;
s16 m1 = 0, m2 = 0, m3 = 0, m4 = 0;
if(elevator > 0) // 后
{
if(elevator >= 0x80)
{
elevator -= 0x80;
pid_pitch.zero_err = elevator * 0.2;
}
else // 前
{
pid_pitch.zero_err = -(elevator * 0.2);
}
}
else
{
pid_pitch.zero_err = 0;
}
if(aileron > 0) // 右
{
if(aileron >= 0x80)
{
aileron -= 0x80;
pid_roll.zero_err = -(aileron * 0.2);
}
else // 左
{
pid_roll.zero_err = aileron * 0.2;
}
}
else
{
pid_roll.zero_err = 0;
}
if(rudder > 0) // 右偏
{
if(rudder >= 0x80)
{
rudder -= 0x80;
pid_yaw.zero_err = rudder * 20.0;
}
else // 左偏
{
pid_yaw.zero_err = -rudder * 20.0;
}
}
else
{
pid_yaw.zero_err = 0;
}
roll_out = (s16)pid_calculate(&pid_roll, roll, gx);
pitch_out = (s16)pid_calculate(&pid_pitch, pitch, gy);
yaw_out = (s16)pid_calculate(&pid_yaw, yaw, gz);
if(throttle > 0) // 油门
{
thr = throttle * 7.0;
m1 = thr - pitch_out + roll_out + yaw_out;
m2 = thr + pitch_out + roll_out - yaw_out;
m3 = thr + pitch_out - roll_out + yaw_out;
m4 = thr - pitch_out - roll_out - yaw_out;
}
else
{
m1 = 0;
m2 = 0;
m3 = 0;
m4 = 0;
pid_reset();
}
motor_control(m1, m2, m3, m4);
}
void yaw_ctrl_reset(void)
{
pid_reset(&controller);
}
uint8_t findPosition(uint8_t type)
{
disableSpinning();
if(type == REDSTART)
{
asserv_set_vitesse_low(&asserv);
trajectory_goto_d(&traj, END, -200);
while(!trajectory_is_ended(&traj))
{
if(REPOSITIONING)
{
trajectory_reinit(&traj);
asserv_stop(&asserv);
}
}
trajectory_goto_d(&traj, END, 20 - DIST_CENTRE);//dist centre = 12.2
while(!trajectory_is_ended(&traj));
trajectory_goto_arel(&traj, END, -90.0);
while(!trajectory_is_ended(&traj));
trajectory_goto_d(&traj, END, -200);
while(!trajectory_is_ended(&traj))
{
if(REPOSITIONING)
{
trajectory_reinit(&traj);
asserv_stop(&asserv);
}
}
trajectory_goto_d(&traj, END, 20 - DIST_CENTRE);
while(!trajectory_is_ended(&traj));
//asserv_set_distance()
cli();
printf("pos asserv a : %lu %lu \n",U_ASSERV_ANGLE,U_PM_ANGLE);
position_set_xya_cm_deg(&pos,fxx_from_double(20.0),fxx_from_double(20.0), fxx_from_double(0.0));
wait_ms(100);
printf("pos x : %ld pos y %ld \n",pos.x,pos.y);
printf("pos sum x : %lf pos y %lf \n",pos.sum_x,pos.sum_y);
asserv_stop(&asserv);//attention bug bizare avant
asserv_set_vitesse_fast(&asserv);
}
else if(type == YELLOWSTART)// team == YELLOW
{
asserv_set_vitesse_low(&asserv);
trajectory_goto_d(&traj, END, -200);
while(!trajectory_is_ended(&traj))
{
if(REPOSITIONING)
{
trajectory_reinit(&traj);
asserv_stop(&asserv);
}
}
trajectory_goto_d(&traj, END, 20 - DIST_CENTRE);//dist centre = 12.2
while(!trajectory_is_ended(&traj));
trajectory_goto_arel(&traj, END, 90.0);
while(!trajectory_is_ended(&traj));
trajectory_goto_d(&traj, END, -200);
while(!trajectory_is_ended(&traj))
{
if(REPOSITIONING)
{
trajectory_reinit(&traj);
asserv_stop(&asserv);
}
}
trajectory_goto_d(&traj, END, 20 - DIST_CENTRE);
while(!trajectory_is_ended(&traj));
cli();
printf("pos asserv a : %lu %lu \n",U_ASSERV_ANGLE,U_PM_ANGLE);
position_set_xya_cm_deg(&pos,fxx_from_double(20.0),fxx_from_double(280.0), fxx_from_double(0.0));
asserv_stop(&asserv);
asserv_set_vitesse_fast(&asserv);
}
else if(type == REDPAINT)
{
// asserv_set_vitesse_low(&asserv);
// double eps = 0;
// set_startCoor(position_get_coor_eps(&pos, &eps));
// printf("start pos: %d eps %lf \n",position_get_coor_eps(&pos, &eps),eps);
// set_goalCoor(G_LENGTH * 1 + 7);
// if (eps > EPSILON)
// {
// go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
// }
// while(!astarMv() && !actionIsFailed());
// if(actionIsFailed())return DONE;
asserv_set_vitesse_normal(&asserv);
trajectory_goto_a(&traj, END, 90);
trajectory_goto_d(&traj, END, -40);
disableSpinning();
enableSpinning();// a tester
while(!trajectory_is_ended(&traj))
{
if(REPOSITIONING)
{
trajectory_reinit(&traj);
asserv_stop(&asserv);
}
}
enableSpinning();
//if(actionIsFailed())return DONE;
trajectory_goto_d(&traj, END, 20 - DIST_CENTRE);
//while(!trajectory_is_ended(&traj));
// if(actionIsFailed())return DONE;
cli();
printf("pos asserv a : %lu %lu \n",U_ASSERV_ANGLE,U_PM_ANGLE);
position_set_xya_cm_deg(&pos,fxx_from_double(20.0),fxx_from_double(160.0), fxx_from_double(90.0));
asserv_stop(&asserv);
asserv_set_vitesse_normal(&asserv);
}
else if(type == YELLOWPAINT)
{
// double eps = 0;
// set_startCoor(position_get_coor_eps(&pos, &eps));
// printf("start pos: %d eps %lf \n",position_get_coor_eps(&pos, &eps),eps);
// set_goalCoor(G_LENGTH * 1 + 8);
// if (eps > EPSILON)
// {
// go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
// }
// while(!astarMv() && !actionIsFailed());
// if(actionIsFailed())return DONE;
asserv_set_vitesse_normal(&asserv);
trajectory_goto_a(&traj, END, -90);
trajectory_goto_d(&traj, END, -40);
disableSpinning();
enableSpinning();// a tester
while(!trajectory_is_ended(&traj))
{
if(REPOSITIONING)
{
trajectory_reinit(&traj);
asserv_stop(&asserv);
}
}
enableSpinning();
//if(actionIsFailed())return DONE;
trajectory_goto_d(&traj, END, 20 - DIST_CENTRE);
while(!trajectory_is_ended(&traj));
//if(actionIsFailed())return DONE;
cli();
printf("pos asserv a : %lu %lu \n",U_ASSERV_ANGLE,U_PM_ANGLE);
position_set_xya_cm_deg(&pos,fxx_from_double(20.0),fxx_from_double(160.0), fxx_from_double(-90.0));
asserv_stop(&asserv);
asserv_set_vitesse_normal(&asserv);
}
//printf("pos asserv a : %lu %lu \n",U_ASSERV_ANGLE,U_PM_ANGLE);
quadramp_reset(&asserv);
control_reset(&asserv);
pid_reset(&asserv);
diff_reset(&asserv);
printf("pos x : %ld pos y %ld \n",pos.x,pos.y);
printf("pos sum x : %lf pos y %lf \n",pos.sum_x,pos.sum_y);
sei();
enableSpinning();
return DONE;
}
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;
}
void u_speed_reset(void)
{
pid_reset(&ctrl);
}
/**
* @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;
}
// the ctbot class
void native_ctbot_motor_invoke(u08_t mref) {
// JAVA: void setLeft(int val)
if(mref == NATIVE_METHOD_setLeft) {
nvm_int_t val = stack_pop_int();
cli();
pwm_motor_l=val;
pid_mode &= ~PID_LEFT_ENABLED;
sei();
}
// JAVA: void setRight(int val)
else if(mref == NATIVE_METHOD_setRight) {
nvm_int_t val = stack_pop_int();
cli();
pwm_motor_r=val;
pid_mode &= ~PID_RIGHT_ENABLED;
sei();
}
else if(mref == NATIVE_METHOD_setLeftSpeed) {
nvm_int_t val = stack_pop_int();
cli();
pid_left_value=val;
pid_mode |= PID_LEFT_ENABLED;
sei();
}
// JAVA: void setRight(int val)
else if(mref == NATIVE_METHOD_setRightSpeed) {
nvm_int_t val = stack_pop_int();
cli();
pid_right_value=val;
pid_mode |= PID_RIGHT_ENABLED;
sei();
}
// JAVA: int getLeft()
else if(mref == NATIVE_METHOD_getLeft) {
nvm_int_t val = pwm_motor_l;
stack_push(val);
}
// JAVA: int getRight()
else if(mref == NATIVE_METHOD_getRight) {
nvm_int_t val = pwm_motor_r;
stack_push(val);
}
// JAVA: int getLeft()
else if(mref == NATIVE_METHOD_getLeftSpeed) {
nvm_int_t val = pwm_motor_l;
stack_push(val);
}
// JAVA: int getRight()
else if(mref == NATIVE_METHOD_getRightSpeed) {
nvm_int_t val = pwm_motor_r;
stack_push(val);
}
// JAVA: void stopt()
else if(mref == NATIVE_METHOD_stop) {
cli();
pwm_motor_r = 0;
pwm_motor_l = 0;
pid_mode &= ~(PID_RIGHT_ENABLED|PID_LEFT_ENABLED);
pid_reset();
sei();
}
}
void att_ctrl_reset(void)
{
FOR_EACH(i, controllers)
pid_reset(&controllers[i]);
}
void u_ctrl_reset(void)
{
pid_reset(&ctrl);
}
