//Calibrate gyro and initialize PID controllers
void
pre_auton(){
//Allow gyro to settle and then init/calibrate (Takes a total of around 2 seconds)
delay(1100);
gyroInit(gyro, 1);
//These NEED to be tuned
pidInit(gyroAnglePid, 2, 0, 0.15, 2, 20.0); //No idea if these are any good, they need to be tuned a TON
pidInit(gyroRatePid, 10, 0, 0, 0, 360.00); //need to be tuned
}
void controllerInit(void)
{
pidInit(&pidRollRate, 0, PID_ROLL_RATE_KP, PID_ROLL_RATE_KI, PID_ROLL_RATE_KD, IMU_UPDATE_DT);
pidInit(&pidPitchRate, 0, PID_PITCH_RATE_KP, PID_PITCH_RATE_KI, PID_PITCH_RATE_KD, IMU_UPDATE_DT);
pidInit(&pidYawRate, 0, PID_YAW_RATE_KP, PID_YAW_RATE_KI, PID_YAW_RATE_KD, IMU_UPDATE_DT);
pidSetIntegralLimit(&pidRollRate, PID_ROLL_RATE_INTEGRATION_LIMIT);
pidSetIntegralLimit(&pidPitchRate, PID_PITCH_RATE_INTEGRATION_LIMIT);
pidSetIntegralLimit(&pidYawRate, PID_YAW_RATE_INTEGRATION_LIMIT);
}
void activateConfig(void)
{
static imuRuntimeConfig_t imuRuntimeConfig;
activateControlRateConfig();
resetAdjustmentStates();
useRcControlsConfig(
currentProfile->modeActivationConditions,
&masterConfig.escAndServoConfig,
¤tProfile->pidProfile
);
useGyroConfig(&masterConfig.gyroConfig, currentProfile->pidProfile.gyro_soft_lpf_hz);
#ifdef TELEMETRY
telemetryUseConfig(&masterConfig.telemetryConfig);
#endif
useFailsafeConfig(&masterConfig.failsafeConfig);
setAccelerationZero(&masterConfig.accZero);
setAccelerationGain(&masterConfig.accGain);
setAccelerationFilter(currentProfile->pidProfile.acc_soft_lpf_hz);
mixerUseConfigs(
#ifdef USE_SERVOS
currentProfile->servoConf,
¤tProfile->gimbalConfig,
#endif
&masterConfig.flight3DConfig,
&masterConfig.escAndServoConfig,
&masterConfig.mixerConfig,
&masterConfig.rxConfig
);
imuRuntimeConfig.dcm_kp_acc = masterConfig.dcm_kp_acc / 10000.0f;
imuRuntimeConfig.dcm_ki_acc = masterConfig.dcm_ki_acc / 10000.0f;
imuRuntimeConfig.dcm_kp_mag = masterConfig.dcm_kp_mag / 10000.0f;
imuRuntimeConfig.dcm_ki_mag = masterConfig.dcm_ki_mag / 10000.0f;
imuRuntimeConfig.small_angle = masterConfig.small_angle;
imuConfigure(&imuRuntimeConfig, ¤tProfile->pidProfile);
pidInit();
#ifdef NAV
navigationUseConfig(&masterConfig.navConfig);
navigationUsePIDs(¤tProfile->pidProfile);
navigationUseRcControlsConfig(¤tProfile->rcControlsConfig);
navigationUseRxConfig(&masterConfig.rxConfig);
navigationUseFlight3DConfig(&masterConfig.flight3DConfig);
navigationUseEscAndServoConfig(&masterConfig.escAndServoConfig);
#endif
#ifdef BARO
useBarometerConfig(&masterConfig.barometerConfig);
#endif
}
int main(void) {
xyInit();
pidInit();
motorInit();
orientationInit();
debugPrint("Initialized Hardware");
addTask(&flightTask);
addTask(&statusTask);
addMenuCommand('m', motorToggleString, &motorToggle);
addMenuCommand('w', motorForwardString, &motorForward);
addMenuCommand('a', motorLeftString, &motorLeft);
addMenuCommand('s', motorBackwardString, &motorBackward);
addMenuCommand('d', motorRightString, &motorRight);
addMenuCommand('x', motorUpString, &motorUp);
addMenuCommand('y', motorDownString, &motorDown);
addMenuCommand('p', controlToggleString, &controlToggle);
addMenuCommand('n', parameterChangeString, ¶meterChange);
addMenuCommand('z', zeroString, &zeroOrientation);
addMenuCommand('o', silentString, &silent);
addMenuCommand('r', sensorString, &printRaw);
xyLed(LED_RED, LED_OFF);
xyLed(LED_GREEN, LED_ON);
debugPrint("Starting Tasks");
for(;;) {
tasks();
}
return 0;
}
void initPidControl()
{
pidInit(&balanceControl.pidPitch);
pidInit(&balanceControl.pidSpeed);
#if 1
//balanceControl.pidPitch.outMax = 64000;
//balanceControl.pidPitch.outMin = -64000;
balanceControl.pidPitch.iMax = 1000;
balanceControl.pidPitch.iMin = -1000;
#endif
balanceControl.pidSpeed.iMax = 1000;
balanceControl.pidSpeed.iMin = -1000;
kalmanPitch.setAngle(imu.euler.pitch);
balanceControl.factorL = 1;
balanceControl.factorR = 1;
balanceControl.spinSpeed = 0;
}
void pidSetKoefficients(float p, float i, float d){
//koefficients should be positive
//one could add a check here but it's waste of flash
//since we know what we should set :D
// sampling at 1sec makes everything simpler :)
Pk = p;
Ik = i; // * sample time in seconds
Dk = d; // / sample time in seconds
pidInit();
}
//Gyro turn to target angle
void gyroTurn(float target)
{
if(abs(target) < 40)
pidInit(gyroPid, 3, 0, 0.15, 3, 1270);
bool atGyro = false;
long atTargetTime = nPgmTime;
long timer = nPgmTime;
pidReset(gyroPid);
gyroResetAngle();
while(!atGyro)
{
//Calculate the delta time from the last iteration of the loop
float dT = (float)(nPgmTime - timer)/1000;
//Calculate the current angle of the gyro
float angle = gyroAddAngle(dT);
//Reset loop timer
timer = nPgmTime;
//Calculate the output of the PID controller and output to drive motors
float error = (float)target - angle;
float driveOut = pidExecute(gyroPid, error);
driveL(-driveOut);
driveR(driveOut);
//Stop the turn function when the angle has been within 3 degrees of the desired angle for 350ms
if(abs(error) > 3)
atTargetTime = nPgmTime;
if(nPgmTime - atTargetTime > 350)
{
atGyro = true;
driveL(0);
driveR(0);
}
}
//Reinitialize the PID constants to their original values in case they were changed
pidInit(gyroPid, 2, 0, 0.15, 2, 1270);
}
void attitudeControllerInit(const float updateDt)
{
if(isInit)
return;
//TODO: get parameters from configuration manager instead
pidInit(&pidRollRate, 0, PID_ROLL_RATE_KP, PID_ROLL_RATE_KI, PID_ROLL_RATE_KD,
updateDt, ATTITUDE_RATE, ATTITUDE_RATE_LPF_CUTOFF_FREQ, ATTITUDE_RATE_LPF_ENABLE);
pidInit(&pidPitchRate, 0, PID_PITCH_RATE_KP, PID_PITCH_RATE_KI, PID_PITCH_RATE_KD,
updateDt, ATTITUDE_RATE, ATTITUDE_RATE_LPF_CUTOFF_FREQ, ATTITUDE_RATE_LPF_ENABLE);
pidInit(&pidYawRate, 0, PID_YAW_RATE_KP, PID_YAW_RATE_KI, PID_YAW_RATE_KD,
updateDt, ATTITUDE_RATE, ATTITUDE_RATE_LPF_CUTOFF_FREQ, ATTITUDE_RATE_LPF_ENABLE);
pidSetIntegralLimit(&pidRollRate, PID_ROLL_RATE_INTEGRATION_LIMIT);
pidSetIntegralLimit(&pidPitchRate, PID_PITCH_RATE_INTEGRATION_LIMIT);
pidSetIntegralLimit(&pidYawRate, PID_YAW_RATE_INTEGRATION_LIMIT);
pidInit(&pidRoll, 0, PID_ROLL_KP, PID_ROLL_KI, PID_ROLL_KD, updateDt,
ATTITUDE_RATE, ATTITUDE_LPF_CUTOFF_FREQ, ATTITUDE_LPF_ENABLE);
pidInit(&pidPitch, 0, PID_PITCH_KP, PID_PITCH_KI, PID_PITCH_KD, updateDt,
ATTITUDE_RATE, ATTITUDE_LPF_CUTOFF_FREQ, ATTITUDE_LPF_ENABLE);
pidInit(&pidYaw, 0, PID_YAW_KP, PID_YAW_KI, PID_YAW_KD, updateDt,
ATTITUDE_RATE, ATTITUDE_LPF_CUTOFF_FREQ, ATTITUDE_LPF_ENABLE);
pidSetIntegralLimit(&pidRoll, PID_ROLL_INTEGRATION_LIMIT);
pidSetIntegralLimit(&pidPitch, PID_PITCH_INTEGRATION_LIMIT);
pidSetIntegralLimit(&pidYaw, PID_YAW_INTEGRATION_LIMIT);
isInit = true;
}
void control_quadrotor_attitude_init( const struct attitude_pid_quat_params *tilt_rate,
const struct attitude_pid_quat_params *tilt_angle,
const struct attitude_pid_quat_params *yaw_rate,
const struct attitude_pid_quat_params *yaw_angle,
const struct attitude_control_quat_params *control)
{
control_initFilter();
controlData.pitchRate = pidInit(&tilt_rate->p, &tilt_rate->i, &tilt_rate->d, &tilt_rate->f,
&tilt_rate->max_p, &tilt_rate->max_i, &tilt_rate->max_d, &tilt_rate->max_o,
0, 0, 0, 0);
controlData.rollRate = pidInit (&tilt_rate->p, &tilt_rate->i, &tilt_rate->d, &tilt_rate->f,
&tilt_rate->max_p, &tilt_rate->max_i, &tilt_rate->max_d, &tilt_rate->max_o,
0, 0, 0, 0);
controlData.yawRate = pidInit (&yaw_rate->p, &yaw_rate->i, &yaw_rate->d, &yaw_rate->f,
&yaw_rate->max_p, &yaw_rate->max_i, &yaw_rate->max_d, &yaw_rate->max_o,
0, 0, 0, 0);
controlData.pitchAngle = pidInit(&tilt_angle->p, &tilt_angle->i, &tilt_angle->d, &tilt_angle->f,
&tilt_angle->max_p, &tilt_angle->max_i, &tilt_angle->max_d, &tilt_angle->max_o,
0, 0, 0, 0);
controlData.rollAngle = pidInit(&tilt_angle->p, &tilt_angle->i, &tilt_angle->d, &tilt_angle->f,
&tilt_angle->max_p, &tilt_angle->max_i, &tilt_angle->max_d, &tilt_angle->max_o,
0, 0, 0, 0);
controlData.yawAngle = pidInit(&yaw_angle->p, &yaw_angle->i, &yaw_angle->d, &yaw_angle->f,
&yaw_angle->max_p, &yaw_angle->max_i, &yaw_angle->max_d, &yaw_angle->max_o,
0, 0, 0, 0);
}
void controlInit(void) {
AQ_NOTICE("Control init\n");
memset((void *)&controlData, 0, sizeof(controlData));
#ifdef USE_QUATOS
quatosInit(AQ_INNER_TIMESTEP, AQ_OUTER_TIMESTEP);
#endif
utilFilterInit3(controlData.userPitchFilter, AQ_INNER_TIMESTEP, 0.1f, 0.0f);
utilFilterInit3(controlData.userRollFilter, AQ_INNER_TIMESTEP, 0.1f, 0.0f);
utilFilterInit3(controlData.navPitchFilter, AQ_INNER_TIMESTEP, 0.125f, 0.0f);
utilFilterInit3(controlData.navRollFilter, AQ_INNER_TIMESTEP, 0.125f, 0.0f);
controlData.pitchRatePID = pidInit(&p[CTRL_TLT_RTE_P], &p[CTRL_TLT_RTE_I], &p[CTRL_TLT_RTE_D], &p[CTRL_TLT_RTE_F], &p[CTRL_TLT_RTE_PM],
&p[CTRL_TLT_RTE_IM], &p[CTRL_TLT_RTE_DM], &p[CTRL_TLT_RTE_OM], 0, 0, 0, 0);
controlData.rollRatePID = pidInit(&p[CTRL_TLT_RTE_P], &p[CTRL_TLT_RTE_I], &p[CTRL_TLT_RTE_D], &p[CTRL_TLT_RTE_F], &p[CTRL_TLT_RTE_PM],
&p[CTRL_TLT_RTE_IM], &p[CTRL_TLT_RTE_DM], &p[CTRL_TLT_RTE_OM], 0, 0, 0, 0);
controlData.yawRatePID = pidInit(&p[CTRL_YAW_RTE_P], &p[CTRL_YAW_RTE_I], &p[CTRL_YAW_RTE_D], &p[CTRL_YAW_RTE_F], &p[CTRL_YAW_RTE_PM],
&p[CTRL_YAW_RTE_IM], &p[CTRL_YAW_RTE_DM], &p[CTRL_YAW_RTE_OM], 0, 0, 0, 0);
controlData.pitchAnglePID = pidInit(&p[CTRL_TLT_ANG_P], &p[CTRL_TLT_ANG_I], &p[CTRL_TLT_ANG_D], &p[CTRL_TLT_ANG_F], &p[CTRL_TLT_ANG_PM],
&p[CTRL_TLT_ANG_IM], &p[CTRL_TLT_ANG_DM], &p[CTRL_TLT_ANG_OM], 0, 0, 0, 0);
controlData.rollAnglePID = pidInit(&p[CTRL_TLT_ANG_P], &p[CTRL_TLT_ANG_I], &p[CTRL_TLT_ANG_D], &p[CTRL_TLT_ANG_F], &p[CTRL_TLT_ANG_PM],
&p[CTRL_TLT_ANG_IM], &p[CTRL_TLT_ANG_DM], &p[CTRL_TLT_ANG_OM], 0, 0, 0, 0);
controlData.yawAnglePID = pidInit(&p[CTRL_YAW_ANG_P], &p[CTRL_YAW_ANG_I], &p[CTRL_YAW_ANG_D], &p[CTRL_YAW_ANG_F], &p[CTRL_YAW_ANG_PM],
&p[CTRL_YAW_ANG_IM], &p[CTRL_YAW_ANG_DM], &p[CTRL_YAW_ANG_OM], 0, 0, 0, 0);
controlTaskStack = aqStackInit(CONTROL_STACK_SIZE, "CONTROL");
controlData.controlTask = CoCreateTask(controlTaskCode, (void *)0, CONTROL_PRIORITY, &controlTaskStack[CONTROL_STACK_SIZE-1],
CONTROL_STACK_SIZE);
}
int main(void) {
halInit();
chSysInit();
sdStart(&SD1, &serialConfig);
pilotSetup();
powerSetup();
motorsSetup();
imuSetup();
pidInit(&pitchPID, 1.0 / 20000.0, 1.0 / 10000, 0);
pidInit(&rollPID, 1.0 / 20000.0, 1.0 / 10000, 0);
pidInit(&yawPID, 1.0 / 20000.0, 1.0 / 10000, 0);
while (TRUE) {
printf("yaw rate %f\r\n", imuGetYawRate());
printf("pitch %f\r\n", imuGetPitch());
printf("roll %f\r\n", imuGetRoll());
// double pitch = pilotGetPitch() - pidUpdate(&pitchPID, 0.0, gyroGetPitchRotation());
// double roll = pilotGetRoll() - pidUpdate(&rollPID, 0, gyroGetRollRotation());
// double yaw = pilotGetYaw() - pidUpdate(&yawPID, 0, gyroGetYawRotation());
// motorsSetControl(CLIP(pitch), CLIP(roll), CLIP(yaw), pilotGetThrottle());
// uint16_t throttle = receiverGetRaw(THROTTLE_CH);
// uint16_t pitch = receiverGetRaw(PITCH_CH);
// uint16_t roll = receiverGetRaw(ROLL_CH);
// uint16_t yaw = receiverGetRaw(YAW_CH);
// printf("%u, %u, %u, %u\r\n", throttle, pitch, roll, yaw);
// printf("%u\r\n", receiverGetRaw(4));
// printf("%f, %f, %f\r\n", receiverGetDouble(PITCH_CH), receiverGetDouble(ROLL_CH), receiverGetDouble(YAW_CH));
// printf("%d, %d, %d\r\n", gyroGetPitchRotation(), gyroGetRollRotation(), gyroGetYawRotation());
// printf("%f, %f, %f\r\n", pitch, roll, yaw);
// printf("%f, %f, %f\r\n", pitchPID.integral, rollPID.integral, yawPID.integral);
chThdSleepSeconds(1);
}
}
task flw_tsk_FeedForwardCntrl(){
pidReset(_fly.flyPID);
pidInit(_fly.flyPID, 0.6, 0.05, 0, 0, 999999);
int integralLimit;
if(_fly.flyPID.kI == 0){
integralLimit = 0;
} else{
integralLimit = 13.0 / _fly.flyPID.kI;
}
float output = 0;
float initTime = nPgmTime;
while(true){
fw_ButtonControl();
//
//we do not want to zero our error sum when we cross
if(abs(_fly.flyPID.errorSum) > integralLimit){
_fly.flyPID.errorSum = integralLimit * _fly.flyPID.errorSum/(abs(_fly.flyPID.errorSum));
}
_fly.currSpeed = FwCalculateSpeed();
float outVal = pidExecute(_fly.flyPID, _fly.setPoint - _fly.currSpeed);
float dTime = nPgmTime - initTime;
initTime = nPgmTime;
playTone(_fly.currSpeed/2,2);
output = _fly.pred + outVal;
if(output < 0){
output = 0;
}
if(output > 127){
output = 127;
}
if((_fly.currSpeed <= 50 && _fly.setPoint == 0) && vexRT[Btn7L] == 1){
output = -90;
_updateFlyWheel(output);
wait1Msec(4000);
_updateFlyWheel(0);
wait1Msec(10000);
}
_updateFlyWheel(output);
delay(FW_LOOP_SPEED);
}
}
//Calibrate gyro and initialize PID controller
void pre_auton()
{
//Set gyro port to analog port 1
gyroSetPort(in2);
//Allow gyro to settle and then calibrate (Takes a total of around 3 seconds)
delay(1100);
// SensorValue[calibrationInProgress] = 1;
gyroCalibrate();
// SensorValue[calibrationInProgress] = 0;
/*Initialize PID controller for gyro
* kP = 2, kI = 0, kD = 0.15
* epsilon = 0, slewRate = 1270
*/
pidInit(gyroPid, 2, 0, 0.15, 0, 1270);
}
void activateConfig(void)
{
#ifndef USE_OSD_SLAVE
initRcProcessing();
resetAdjustmentStates();
pidInit(currentPidProfile);
useRcControlsConfig(currentPidProfile);
useAdjustmentConfig(currentPidProfile);
#ifdef USE_NAV
gpsUsePIDs(currentPidProfile);
#endif
failsafeReset();
setAccelerationTrims(&accelerometerConfigMutable()->accZero);
accInitFilters();
imuConfigure(throttleCorrectionConfig()->throttle_correction_angle);
#endif // USE_OSD_SLAVE
}
void navInit(void) {
int i = 0;
AQ_NOTICE("Nav init\n");
memset((void *)&navData, 0, sizeof(navData));
navData.speedNPID = pidInit(&p[NAV_SPEED_P], &p[NAV_SPEED_I], 0, 0, &p[NAV_SPEED_PM], &p[NAV_SPEED_IM], 0, &p[NAV_SPEED_OM], 0, 0, 0, 0);
navData.speedEPID = pidInit(&p[NAV_SPEED_P], &p[NAV_SPEED_I], 0, 0, &p[NAV_SPEED_PM], &p[NAV_SPEED_IM], 0, &p[NAV_SPEED_OM], 0, 0, 0, 0);
navData.distanceNPID = pidInit(&p[NAV_DIST_P], &p[NAV_DIST_I], 0, 0, &p[NAV_DIST_PM], &p[NAV_DIST_IM], 0, &p[NAV_DIST_OM], 0, 0, 0, 0);
navData.distanceEPID = pidInit(&p[NAV_DIST_P], &p[NAV_DIST_I], 0, 0, &p[NAV_DIST_PM], &p[NAV_DIST_IM], 0, &p[NAV_DIST_OM], 0, 0, 0, 0);
navData.altSpeedPID = pidInit(&p[NAV_ALT_SPED_P], &p[NAV_ALT_SPED_I], 0, 0, &p[NAV_ALT_SPED_PM], &p[NAV_ALT_SPED_IM], 0,
&p[NAV_ALT_SPED_OM], 0, 0, 0, 0);
navData.altPosPID = pidInit(&p[NAV_ALT_POS_P], &p[NAV_ALT_POS_I], 0, 0, &p[NAV_ALT_POS_PM], &p[NAV_ALT_POS_IM], 0, &p[NAV_ALT_POS_OM], 0, 0,
0, 0);
navData.mode = NAV_STATUS_MANUAL;
navData.ceilingAlt = 0.0f;
navData.setCeilingFlag = 0;
navData.setCeilingReached = 0;
navData.homeActionFlag = 0;
navData.distanceToHome = 0.0f;
navData.headFreeMode = NAV_HEADFREE_OFF;
navData.hfUseStoredReference = 0;
navSetHoldHeading(AQ_YAW);
navSetHoldAlt(ALTITUDE, 0);
// HOME
navData.missionLegs[i].type = NAV_LEG_HOME;
navData.missionLegs[i].targetRadius = 0.10f;
navData.missionLegs[i].loiterTime = 0;
navData.missionLegs[i].poiAltitude = ALTITUDE;
i++;
// land
navData.missionLegs[i].type = NAV_LEG_LAND;
navData.missionLegs[i].maxHorizSpeed = 1.0f;
navData.missionLegs[i].poiAltitude = 0.0f;
i++;
}
void navInit(void) {
AQ_NOTICE("Nav init\n");
memset((void *)&navData, 0, sizeof(navData));
navData.speedNPID = pidInit(NAV_SPEED_P, NAV_SPEED_I, 0, 0, NAV_SPEED_PM, NAV_SPEED_IM, 0, NAV_SPEED_OM);
navData.speedEPID = pidInit(NAV_SPEED_P, NAV_SPEED_I, 0, 0, NAV_SPEED_PM, NAV_SPEED_IM, 0, NAV_SPEED_OM);
navData.distanceNPID = pidInit(NAV_DIST_P, NAV_DIST_I, 0, 0, NAV_DIST_PM, NAV_DIST_IM, 0, NAV_DIST_OM);
navData.distanceEPID = pidInit(NAV_DIST_P, NAV_DIST_I, 0, 0, NAV_DIST_PM, NAV_DIST_IM, 0, NAV_DIST_OM);
navData.altSpeedPID = pidInit(NAV_ALT_SPED_P, NAV_ALT_SPED_I, 0, 0, NAV_ALT_SPED_PM, NAV_ALT_SPED_IM, 0, NAV_ALT_SPED_OM);
navData.altPosPID = pidInit(NAV_ALT_POS_P, NAV_ALT_POS_I, 0, 0, NAV_ALT_POS_PM, NAV_ALT_POS_IM, 0, NAV_ALT_POS_OM);
navData.mode = NAV_STATUS_MANUAL;
navData.headFreeMode = NAV_HEADFREE_OFF;
navSetHoldHeading(AQ_YAW);
navSetHoldAlt(ALTITUDE, 0);
navLoadDefaultMission();
}
void controllerInit()
{
if(isInit)
return;
//TODO: get parameters from configuration manager instead
pidInit(&pidRollRate, 0, PID_ROLL_RATE_KP, PID_ROLL_RATE_KI, PID_ROLL_RATE_KD, IMU_UPDATE_DT);
pidInit(&pidPitchRate, 0, PID_PITCH_RATE_KP, PID_PITCH_RATE_KI, PID_PITCH_RATE_KD, IMU_UPDATE_DT);
pidInit(&pidYawRate, 0, PID_YAW_RATE_KP, PID_YAW_RATE_KI, PID_YAW_RATE_KD, IMU_UPDATE_DT);
pidSetIntegralLimit(&pidRollRate, PID_ROLL_RATE_INTEGRATION_LIMIT);
pidSetIntegralLimit(&pidPitchRate, PID_PITCH_RATE_INTEGRATION_LIMIT);
pidSetIntegralLimit(&pidYawRate, PID_YAW_RATE_INTEGRATION_LIMIT);
pidInit(&pidRoll, 0, PID_ROLL_KP, PID_ROLL_KI, PID_ROLL_KD, IMU_UPDATE_DT);
pidInit(&pidPitch, 0, PID_PITCH_KP, PID_PITCH_KI, PID_PITCH_KD, IMU_UPDATE_DT);
pidInit(&pidYaw, 0, PID_YAW_KP, PID_YAW_KI, PID_YAW_KD, IMU_UPDATE_DT);
pidSetIntegralLimit(&pidRoll, PID_ROLL_INTEGRATION_LIMIT);
pidSetIntegralLimit(&pidPitch, PID_PITCH_INTEGRATION_LIMIT);
pidSetIntegralLimit(&pidYaw, PID_YAW_INTEGRATION_LIMIT);
isInit = true;
}
static void stabilizerAltHoldUpdate(void)
{
// Get altitude hold commands from pilot
commanderGetAltHold(&altHold, &setAltHold, &altHoldChange);
// Get barometer height estimates
//TODO do the smoothing within getData
ms5611GetData(&pressure, &temperature, &aslRaw);
asl = asl * aslAlpha + aslRaw * (1 - aslAlpha);
aslLong = aslLong * aslAlphaLong + aslRaw * (1 - aslAlphaLong);
// Estimate vertical speed based on successive barometer readings. This is ugly :)
vSpeedASL = deadband(asl - aslLong, vSpeedASLDeadband);
// Estimate vertical speed based on Acc - fused with baro to reduce drift
vSpeed = constrain(vSpeed, -vSpeedLimit, vSpeedLimit);
vSpeed = vSpeed * vBiasAlpha + vSpeedASL * (1.f - vBiasAlpha);
vSpeedAcc = vSpeed;
// Reset Integral gain of PID controller if being charged
if (!pmIsDischarging())
{
altHoldPID.integ = 0.0;
}
// Altitude hold mode just activated, set target altitude as current altitude. Reuse previous integral term as a starting point
if (setAltHold)
{
// Set to current altitude
altHoldTarget = asl;
// Cache last integral term for reuse after pid init
const float pre_integral = altHoldPID.integ;
// Reset PID controller
pidInit(&altHoldPID, asl, altHoldKp, altHoldKi, altHoldKd,
ALTHOLD_UPDATE_DT);
// TODO set low and high limits depending on voltage
// TODO for now just use previous I value and manually set limits for whole voltage range
// pidSetIntegralLimit(&altHoldPID, 12345);
// pidSetIntegralLimitLow(&altHoldPID, 12345); /
altHoldPID.integ = pre_integral;
// Reset altHoldPID
altHoldPIDVal = pidUpdate(&altHoldPID, asl, false);
}
// In altitude hold mode
if (altHold)
{
// Update target altitude from joy controller input
altHoldTarget += altHoldChange / altHoldChange_SENS;
pidSetDesired(&altHoldPID, altHoldTarget);
// Compute error (current - target), limit the error
altHoldErr = constrain(deadband(asl - altHoldTarget, errDeadband),
-altHoldErrMax, altHoldErrMax);
pidSetError(&altHoldPID, -altHoldErr);
// Get control from PID controller, dont update the error (done above)
// Smooth it and include barometer vspeed
// TODO same as smoothing the error??
altHoldPIDVal = (pidAlpha) * altHoldPIDVal + (1.f - pidAlpha) * ((vSpeedAcc * vSpeedAccFac) +
(vSpeedASL * vSpeedASLFac) + pidUpdate(&altHoldPID, asl, false));
// compute new thrust
actuatorThrust = max(altHoldMinThrust, min(altHoldMaxThrust,
limitThrust( altHoldBaseThrust + (int32_t)(altHoldPIDVal*pidAslFac))));
// i part should compensate for voltage drop
}
else
{
altHoldTarget = 0.0;
altHoldErr = 0.0;
altHoldPIDVal = 0.0;
}
}
task autonomous()
{
clearTimer(T1);
pidInit(flywheel, 0.01, 0.03, 0.001, 3, 50);//p, i, d, epsilon, slew
unsigned long lastTime = nPgmTime;
resetMotorEncoder(flywheelLeftBot);
float rpm, lastRpm1, lastRpm2, lastRpm3, lastRpm4;
int counter = 0;
bool btn7DPressed;
while (true)
{
if(1==1)
{
dT = (float)(nPgmTime - lastTime)/1000;
lastTime = nPgmTime;
if(dT != 0)
{
rpm = 60.00*25*(((float)getMotorEncoder(flywheelLeftBot))/dT)/360;
}
else
{
rpm = 0;
}
resetMotorEncoder(flywheelLeftBot);
lastRpm4 = lastRpm3;
lastRpm3 = lastRpm2;
lastRpm2 = lastRpm1;
lastRpm1 = rpm;
rpmAvg = (rpm + lastRpm1 + lastRpm2 + lastRpm3 + lastRpm4) / 5;
if(flywheel.errorSum > 18000)
{
flywheel.errorSum = 18000;
}
if(rpmAvg >= sp && !set)
{
set = true;
}
if( rpm <= sp - 700)
{
set = false;
}
out = pidExecute(flywheel, sp-rpmAvg);
if(vexRT(Btn7U) == 1)
{
btn7DPressed = false;
pidReset(flywheel);
out = 0;
}
if(out < 1)
{
out = 1;
}
driveFlywheel(out);
}
// debugStrea
if(time1[T1] >= 7250 && time1[T1] <= 9000)
{
motor[conveyor] = 127;
motor[conveyor2] = 127;
}
}
/*dank memes
*/
/*
motor[flywheelLeftTop] = 63;
motor[flywheelLeftBot] = 63;
motor[flywheelRightTop] = 63;
motor[flywheelRightBot] = 63;
wait1Msec(2500);
motor[conveyor] = 127;
wait1Msec(1000);
motor[conveyor] = 0;
wait1Msec(1500);
motor[conveyor] = 127;
wait1Msec(1000);
motor[conveyor] = 0;
wait1Msec(1000);
motor[flywheelLeftTop] = 65;
motor[flywheelLeftBot] = 65;
motor[flywheelRightTop] = 65;
motor[flywheelRightBot] = 65;
motor[conveyor] = 127;
wait1Msec(1000);
motor[conveyor] = 0;
wait1Msec(1000);
motor[conveyor] = 127;
wait1Msec(1000);
motor[conveyor] = 0;
*/
//PROGRAMMING SKILLS
/* while(1==1)
{
motor[flywheelLeftTop] = 63;
motor[flywheelLeftBot] = 63;
motor[flywheelRightTop] = 63;
motor[flywheelRightBot] = 63;
motor[conveyor] = 127;
}*/
}
static void stabilizerAltHoldUpdate() {
// Get the time
float altitudeError = 0;
static float altitudeError_i = 0;
float instAcceleration = 0;
float deltaVertSpeed = 0;
static uint32_t timeStart = 0;
static uint32_t timeCurrent = 0;
// Get altitude hold commands from pilot
commanderGetAltHold(&altHold, &setAltHold, &altHoldChange);
// Get barometer height estimates
//TODO do the smoothing within getData
ms5611GetData(&pressure, &temperature, &aslRaw);
// Compute the altitude
altitudeError = aslRaw - estimatedAltitude;
altitudeError_i = fmin(50.0, fmax(-50.0, altitudeError_i + altitudeError));
instAcceleration = deadband(accWZ, vAccDeadband) * 9.80665 + altitudeError_i * altEstKi;
deltaVertSpeed = instAcceleration * ALTHOLD_UPDATE_DT + (altEstKp1 * ALTHOLD_UPDATE_DT) * altitudeError;
estimatedAltitude += (vSpeedComp * 2.0 + deltaVertSpeed) * (ALTHOLD_UPDATE_DT / 2) + (altEstKp2 * ALTHOLD_UPDATE_DT) * altitudeError;
vSpeedComp += deltaVertSpeed;
aslLong = estimatedAltitude; // Override aslLong
// Estimate vertical speed based on Acc - fused with baro to reduce drift
vSpeedComp = constrain(vSpeedComp, -vSpeedLimit, vSpeedLimit);
// Reset Integral gain of PID controller if being charged
if (!pmIsDischarging()) {
altHoldPID.integ = 0.0;
}
// Altitude hold mode just activated, set target altitude as current altitude. Reuse previous integral term as a starting point
if (setAltHold) {
// Set to current altitude
altHoldTarget = estimatedAltitude;
// Set the start time
timeStart = xTaskGetTickCount();
timeCurrent = 0;
// Reset PID controller
pidInit(&altHoldPID, estimatedAltitude, altHoldKp, altHoldKi, altHoldKd, ALTHOLD_UPDATE_DT);
// Cache last integral term for reuse after pid init
// const float pre_integral = hoverPID.integ;
// Reset the PID controller for the hover controller. We want zero vertical velocity
pidInit(&hoverPID, 0, hoverKp, hoverKi, hoverKd, ALTHOLD_UPDATE_DT);
pidSetIntegralLimit(&hoverPID, 3);
// TODO set low and high limits depending on voltage
// TODO for now just use previous I value and manually set limits for whole voltage range
// pidSetIntegralLimit(&altHoldPID, 12345);
// pidSetIntegralLimitLow(&altHoldPID, 12345); /
// hoverPID.integ = pre_integral;
}
// In altitude hold mode
if (altHold) {
// Get current time
timeCurrent = xTaskGetTickCount();
// Update target altitude from joy controller input
altHoldTarget += altHoldChange / altHoldChange_SENS;
pidSetDesired(&altHoldPID, altHoldTarget);
// Compute error (current - target), limit the error
altHoldErr = constrain(deadband(estimatedAltitude - altHoldTarget, errDeadband), -altHoldErrMax, altHoldErrMax);
pidSetError(&altHoldPID, -altHoldErr);
// Get control from PID controller, dont update the error (done above)
float altHoldPIDVal = pidUpdate(&altHoldPID, estimatedAltitude, false);
// Get the PID value for the hover
float hoverPIDVal = pidUpdate(&hoverPID, vSpeedComp, true);
float thrustValFloat;
// Use different weights depending on time into altHold mode
if (timeCurrent > 150) {
// Compute the mixture between the alt hold and the hover PID
thrustValFloat = 0.5*hoverPIDVal + 0.5*altHoldPIDVal;
} else {
// Compute the mixture between the alt hold and the hover PID
thrustValFloat = 0.1*hoverPIDVal + 0.9*altHoldPIDVal;
}
// float thrustVal = 0.5*hoverPIDVal + 0.5*altHoldPIDVal;
uint32_t thrustVal = altHoldBaseThrust + (int32_t)(thrustValFloat*pidAslFac);
// compute new thrust
actuatorThrust = max(altHoldMinThrust, min(altHoldMaxThrust, limitThrust( thrustVal )));
// i part should compensate for voltage drop
} else {
altHoldTarget = 0.0;
altHoldErr = 0.0;
}
}
void testCalibratePID_MCStyle(int sr,int sl){
initEncoders();
regelverzoegerung=100;
pidInit();
pidSetSpeed(sr,sl);
}
task usercontrol()
{
pidReset(flywheel);
debugStreamClear;
float sp = 2050;
// User control code here, inside the loop
int currentFlywheelSpeed;
// pidInit(flywheel, 0.10, 0.01, 0.00001, 3, 50);
pidInit(flywheel, 0.01, 0.03, 0.001, 3, 50);
unsigned long lastTime = nPgmTime;
resetMotorEncoder(flywheelLeftBot);
float rpm, lastRpm1, lastRpm2, lastRpm3, lastRpm4;
int counter = 0;
bool btn7DPressed;
while (true)
{
if(vexRT[btn7D] == 1)
{
btn7DPressed = true;
}
if(btn7DPressed == true)
{
dT = (float)(nPgmTime - lastTime)/1000;
lastTime = nPgmTime;
if(dT != 0)
{
rpm = 60.00*25*(((float)getMotorEncoder(flywheelLeftBot))/dT)/360;
}
else
{
rpm = 0;
}
resetMotorEncoder(flywheelLeftBot);
lastRpm4 = lastRpm3;
lastRpm3 = lastRpm2;
lastRpm2 = lastRpm1;
lastRpm1 = rpm;
rpmAvg = (rpm + lastRpm1 + lastRpm2 + lastRpm3 + lastRpm4) / 5;
if(flywheel.errorSum > 18000)
{
flywheel.errorSum = 18000;
}
if(rpmAvg >= sp && !set)
{
set = true;
}
if( rpm <= sp - 700)
{
set = false;
}
out = pidExecute(flywheel, sp-rpmAvg);
if(vexRT(Btn7U) == 1)
{
btn7DPressed = false;
pidReset(flywheel);
out = 0;
}
if(out < 1)
{
out = 1;
}
driveFlywheel(out);
}
/*
//motor[Motor1] = 127;
motor[flywheelLeftTop] = 68;
motor[flywheelLeftBot] = 68;
motor[flywheelRightTop] = 68;
motor[flywheelRightBot] = 68;
currentFlywheelSpeed = 68;
}
if(vexRT[Btn7L] == 1)
{
currentFlywheelSpeed -= 2;
motor[flywheelLeftTop] = currentFlywheelSpeed;
motor[flywheelLeftBot] = currentFlywheelSpeed;
motor[flywheelRightTop] = currentFlywheelSpeed;
motor[flywheelRightBot] = currentFlywheelSpeed;
wait1Msec(400);
}
if(vexRT[Btn7R] == 1)
{
currentFlywheelSpeed += 2;
motor[flywheelLeftTop] = currentFlywheelSpeed;
motor[flywheelLeftBot] = currentFlywheelSpeed;
motor[flywheelRightTop] = currentFlywheelSpeed;
motor[flywheelRightBot] = currentFlywheelSpeed;
wait1Msec(400);
}
if(vexRT[Btn7U] == 1)
{
// motor[Motor1] = 0;
motor[flywheelLeftTop] = 0;
motor[flywheelLeftBot] = 0;
motor[flywheelRightTop] = 0;
motor[flywheelRightBot] = 0;
}
if(vexRT[Btn8L] == 1)
{
motor[flywheelLeftTop] = 48;
motor[flywheelLeftBot] = 48;
motor[flywheelRightTop] = 48;
motor[flywheelRightBot] = 48;
currentFlywheelSpeed = 48;
}*/
if(vexRT[Btn8D] == 1)
{
motor[conveyor] = 127;
motor[conveyor2] = 127;
}
if(vexRT[Btn8R] == 1)
{
motor[conveyor] = 0;
motor[conveyor2] = 0;
}
if(vexRT[Btn8U] == 1)
{
motor[conveyor] = -127;
motor[conveyor2] = -127;
}
/* motor[driveFrontRight] = vexRT[Ch1] + vexRT[Ch4];
motor[driveFrontLeft] = vexRT[Ch1] - vexRT[Ch4];
motor[driveBackRight] = vexRT[Ch1] - vexRT[Ch4];
motor[driveBackLeft] = vexRT[Ch1] + vexRT[Ch4];*/
motor[driveFrontRight] = vexRT[Ch2];
motor[driveBackRight] = vexRT[Ch2];
motor[driveFrontLeft] = vexRT[Ch3];
motor[driveBackLeft] = vexRT[Ch3];
while(vexRT(Btn5U)) //Left strafe
{
motor[driveFrontRight] = 127;
motor[driveBackRight] = -127;
motor[driveFrontLeft] = -127;
motor[driveBackLeft] = 127;
}
while(vexRT(Btn6U))//RIGHT STRAFE
{
motor[driveFrontRight] = -127;
motor[driveBackRight] = 127;
motor[driveFrontLeft] = 127;
motor[driveBackLeft] = -127;
}
/*
if(vexRT[Ch2] > 2)
{
motor[driveFrontRight] = 127;
motor[driveFrontLeft] = -127;
motor[driveBackRight] = 127;
motor[driveBackLeft] = -127;
}
if(vexRT[Ch2] < -2)
{
motor[driveFrontRight] = -127;
motor[driveFrontLeft] = 127;
motor[driveBackRight] = -127;
motor[driveBackLeft] = 127;
}
*/
writeDebugStreamLine("%f", rpm);
wait1Msec(20);
}
}