static void subTaskPidController(timeUs_t currentTimeUs)
{
uint32_t startTime = 0;
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
// PID - note this is function pointer set by setPIDController()
pidController(currentPidProfile, &accelerometerConfig()->accelerometerTrims, currentTimeUs);
DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime);
}
int lineFollowControlLoop(void)
{
line_follow_control.line_follow_error = line_follower.position;
line_follow_control.line_follow_command = (pidController(line_follow_control.line_follow_pid.instance,
line_follow_control.line_follow_error));
return LINE_FOLLOW_CONTROL_E_SUCCESS;
}
int speedControlLoop(void)
{
if (speed_params.sign > 0)
speed_control.current_distance = (encoderGetDist(ENCODER_L) + encoderGetDist(ENCODER_R)) / 2.00;
else
speed_control.current_distance = -1.00 * (encoderGetDist(ENCODER_L) + encoderGetDist(ENCODER_R)) / 2.00;
// speedCompute();
if (speed_control.nb_loop_accel < speed_params.nb_loop_accel)
{
speed_control.nb_loop_accel++;
speed_control.speed_consign += speed_params.accel_dist_per_loop;
speed_control.current_distance_consign += speed_control.speed_consign;
}
else if (speed_control.nb_loop_maint < speed_params.nb_loop_maint)
{
speed_control.nb_loop_maint++;
speed_control.current_distance_consign += speed_control.speed_consign;
}
else if (speed_control.nb_loop_decel < speed_params.nb_loop_decel)
{
speed_control.nb_loop_decel++;
speed_control.speed_consign -= speed_params.decel_dist_per_loop;
speed_control.current_distance_consign += speed_control.speed_consign;
}
else
{
speed_control.end_control = TRUE;
}
speed_control.speed_error = speed_control.current_distance_consign - speed_control.current_distance;//for distance control
if (fabs(speed_control.speed_error) > MAX_SPEED_ERROR)
{
ledPowerErrorBlink(1000, 150, 3);
return SPEED_CONTROL_E_ERROR;
}
if (fabs(speed_control.speed_error) > SPEED_ERROR_LIMITER)
{
if (speed_control.speed_error > 0)
speed_control.speed_error = SPEED_ERROR_LIMITER;
else
speed_control.speed_error = -SPEED_ERROR_LIMITER;
}
speed_control.speed_command = pidController(speed_control.speed_pid.instance, speed_control.speed_error)
* (float) speed_params.sign;
//bluetoothPrintf("speed error: %d \r\n", (int)(speed_control.speed_error * 100.00));
speed_control.old_distance = speed_control.current_distance;
return SPEED_CONTROL_E_SUCCESS;
}
static FAST_CODE void subTaskPidController(timeUs_t currentTimeUs)
{
uint32_t startTime = 0;
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
// PID - note this is function pointer set by setPIDController()
pidController(currentPidProfile, &accelerometerConfig()->accelerometerTrims, currentTimeUs);
DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime);
#ifdef USE_RUNAWAY_TAKEOFF
// Check to see if runaway takeoff detection is active (anti-taz), the pidSum is over the threshold,
// and gyro rate for any axis is above the limit for at least the activate delay period.
// If so, disarm for safety
if (ARMING_FLAG(ARMED)
&& !STATE(FIXED_WING)
&& pidConfig()->runaway_takeoff_prevention
&& !runawayTakeoffCheckDisabled
&& !flipOverAfterCrashMode
&& !runawayTakeoffTemporarilyDisabled
&& (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (calculateThrottleStatus() != THROTTLE_LOW))) {
if (((fabsf(pidData[FD_PITCH].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD)
|| (fabsf(pidData[FD_ROLL].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD)
|| (fabsf(pidData[FD_YAW].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD))
&& ((ABS(gyroAbsRateDps(FD_PITCH)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP)
|| (ABS(gyroAbsRateDps(FD_ROLL)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP)
|| (ABS(gyroAbsRateDps(FD_YAW)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_YAW))) {
if (runawayTakeoffTriggerUs == 0) {
runawayTakeoffTriggerUs = currentTimeUs + RUNAWAY_TAKEOFF_ACTIVATE_DELAY;
} else if (currentTimeUs > runawayTakeoffTriggerUs) {
setArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF);
disarm();
}
} else {
runawayTakeoffTriggerUs = 0;
}
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE, DEBUG_RUNAWAY_TAKEOFF_TRUE);
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY, runawayTakeoffTriggerUs == 0 ? DEBUG_RUNAWAY_TAKEOFF_FALSE : DEBUG_RUNAWAY_TAKEOFF_TRUE);
} else {
runawayTakeoffTriggerUs = 0;
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE, DEBUG_RUNAWAY_TAKEOFF_FALSE);
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE);
}
#endif
#ifdef USE_PID_AUDIO
if (isModeActivationConditionPresent(BOXPIDAUDIO)) {
pidAudioUpdate();
}
#endif
}
int speedControlLoop(void)
{
if (speed_params.sign > 0)
speed_control.current_distance = (encoderGetDistance(&left_encoder) + encoderGetDistance(&right_encoder)) / 2;
else
speed_control.current_distance = -1.0 * (encoderGetDistance(&left_encoder) + encoderGetDistance(&right_encoder)) / 2;
// speedCompute();
if (speed_params.nb_loop_accel > 0)
{
speed_params.nb_loop_accel--;
speed_control.speed_consign += speed_params.accel_dist_per_loop;
speed_control.current_distance_consign += speed_control.speed_consign;
}
else if (speed_params.nb_loop_maint > 0)//(speed_control.current_distance < (speed_params.accel_dist + speed_params.maintain_dist)))//speed_params.nb_loop_maint > 0))//speed_control.current_distance < (speed_params.accel_dist + speed_params.maintain_dist)))
{
speed_params.nb_loop_maint--;
speed_control.current_distance_consign += speed_control.speed_consign;
}
else if (speed_params.nb_loop_decel > 0)
{
speed_params.nb_loop_decel--;
speed_control.speed_consign -= speed_params.decel_dist_per_loop;
speed_control.current_distance_consign += speed_control.speed_consign;
}
else if (speed_params.nb_loop_decel <= 0)
{
speed_control.current_distance_consign = speed_params.distance_consign;
speed_control.end_control = 1;
}
speed_control.speed_error = speed_control.current_distance_consign - speed_control.current_distance; //for distance control
speed_control.speed_command = pidController(speed_control.speed_pid.instance, speed_control.speed_error) * (float)speed_params.sign;
speed_control.old_distance = speed_control.current_distance;
return SPEED_CONTROL_E_SUCCESS;
}
/*******************************************************************
* Function: void moveMap(void)
* Input Variables: void
* Output Return: void
* Overview: moves the robot through the map
********************************************************************/
void moveMap( void )
{
char isDone = 0;
pidController(0,RESET);
switch(currentMove){
case MOVE_LEFT:
move_arc_stwt(POINT_TURN, LEFT_TURN, 30, 30, 0);
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_ON);
TMRSRVC_delay(BRAKE_DELAY);
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_OFF);
break;
case MOVE_FORWARD:
setOdometry(WALL_STEP);
while(!isDone){
checkIR();
isDone = moveWall();
}
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_ON);
TMRSRVC_delay(BRAKE_DELAY);
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_OFF);
// move_arc_stwt(NO_TURN, 45, 10, 10, 0);
break;
case MOVE_RIGHT:
move_arc_stwt(POINT_TURN, RIGHT_TURN, 30, 30, 0);
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_ON);
TMRSRVC_delay(BRAKE_DELAY);
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_OFF);
break;
default:
LCD_printf("Whatz2?!");
break;
}
}
void taskMainPidLoop(void)
{
cycleTime = getTaskDeltaTime(TASK_SELF);
dT = (float)cycleTime * 0.000001f;
imuUpdateAccelerometer();
imuUpdateGyroAndAttitude();
annexCode();
if (masterConfig.rxConfig.rcSmoothing) {
filterRc(isRXDataNew);
}
#if defined(NAV)
if (isRXDataNew) {
updateWaypointsAndNavigationMode();
}
#endif
isRXDataNew = false;
#if defined(NAV)
updatePositionEstimator();
applyWaypointNavigationAndAltitudeHold();
#endif
// If we're armed, at minimum throttle, and we do arming via the
// sticks, do not process yaw input from the rx. We do this so the
// motors do not spin up while we are trying to arm or disarm.
// Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck
#ifndef USE_QUAD_MIXER_ONLY
#ifdef USE_SERVOS
&& !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.servoMixerConfig.tri_unarmed_servo)
#endif
&& masterConfig.mixerMode != MIXER_AIRPLANE
&& masterConfig.mixerMode != MIXER_FLYING_WING
&& masterConfig.mixerMode != MIXER_CUSTOM_AIRPLANE
#endif
) {
rcCommand[YAW] = 0;
}
// Apply throttle tilt compensation
if (!STATE(FIXED_WING)) {
int16_t thrTiltCompStrength = 0;
if (navigationRequiresThrottleTiltCompensation()) {
thrTiltCompStrength = 100;
}
else if (currentProfile->throttle_tilt_compensation_strength && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
thrTiltCompStrength = currentProfile->throttle_tilt_compensation_strength;
}
if (thrTiltCompStrength) {
rcCommand[THROTTLE] = constrain(masterConfig.motorConfig.minthrottle
+ (rcCommand[THROTTLE] - masterConfig.motorConfig.minthrottle) * calculateThrottleTiltCompensationFactor(thrTiltCompStrength),
masterConfig.motorConfig.minthrottle,
masterConfig.motorConfig.maxthrottle);
}
}
pidController(¤tProfile->pidProfile, currentControlRateProfile, &masterConfig.rxConfig);
#ifdef HIL
if (hilActive) {
hilUpdateControlState();
motorControlEnable = false;
}
#endif
mixTable();
#ifdef USE_SERVOS
if (isMixerUsingServos()) {
servoMixer(currentProfile->flaperon_throw_offset, currentProfile->flaperon_throw_inverted);
}
if (feature(FEATURE_SERVO_TILT)) {
processServoTilt();
}
//Servos should be filtered or written only when mixer is using servos or special feaures are enabled
if (isServoOutputEnabled()) {
filterServos();
writeServos();
}
#endif
if (motorControlEnable) {
writeMotors();
}
#ifdef USE_SDCARD
afatfs_poll();
#endif
#ifdef BLACKBOX
if (!cliMode && feature(FEATURE_BLACKBOX)) {
handleBlackbox();
}
#endif
}
/* Function Definitions */
void pidController_step_initialize(void)
{
rt_InitInfAndNaN(8U);
pidController(&pid.p.Kd, &pid.p.Kp, &pid.p.Ki, &pid.xd, &pid.xi);
}
/*******************************************************************
* Function: char moveWall(void)
* Input Variables: void
* Output Return: char
* Overview: This function searches for walls and adjust the
* robots differential steering to attempts to
* follow them
********************************************************************/
char moveWall( void )
{
// Check for walls
BOOL isWall = (ftIR < IR_WALL_F_THRESH)|(bkIR < IR_WALL_B_THRESH)|(rtIR < IR_WALL_R_THRESH)|(ltIR < IR_WALL_L_THRESH);
if(!isWall){
return isWall;
}
// A variable that contains the logic of which wall is imaginary
BOOL isLEFT;
// If there is no wall on our right side
// place an imaginary wall just within the threshold
if(rtIR>IR_WALL_R_THRESH){
rtIR = IR_WALL_R_THRESH-15;
isLEFT = 0;
}
// If there is no wall on our left side
// place an imaginary wall just within the threshold
if(ltIR>IR_WALL_L_THRESH){
ltIR = IR_WALL_L_THRESH-15;
isLEFT = 1;
}
float error;
// Check to see if the wall exists in front of the robot
if(ftIR < IR_WALL_F_THRESH)
{
// if the imaginary wall was on the left side
// then biased the error so that when the robot encounters
// an upcoming corner, the robot will turn away from both walls
if (isLEFT)
{
error = rtIR - (ltIR + (1000/ftIR));
}
// biased the error appropriately for the inverse situation
else
{
error = rtIR - (ltIR - (1000/ftIR));
}
}
// If no front facing walls detected
// the air is simply the right distance minus the left left distance
// this ensures symmetry that the robot will follow in between the two walls
// either one real and one imaginary, or both real
else
{
error = rtIR - ltIR;
}
// Use the PID controller function to calculate error
float effort = pidController(error, 0);
// Limit the control effort to the max allowable effort
if((abs(effort) > MAX_EFFORT)&(effort!=0)){
effort = MAX_EFFORT*(effort/abs(effort));
}
// Calculate the stepper speeds for each wheel using a ratio
float stepper_speed_L = MAX_SPEED/2 + (MAX_SPEED/2)*(effort/MAX_EFFORT);
float stepper_speed_R = MAX_SPEED/2 - (MAX_SPEED/2)*(effort/MAX_EFFORT);
// Move with wall
STEPPER_move_stnb( STEPPER_BOTH,
STEPPER_REV, 50, stepper_speed_L, 450, STEPPER_BRK_OFF, // Left
STEPPER_REV, 50, stepper_speed_R, 450, STEPPER_BRK_OFF ); // Right
// debug LCP print statement
// LCD_clear();
// LCD_printf("bkIR: %3.2f\nmoveWall\nError: %3f\nEffort: %3f\n", bkIR, error, effort);
}
