int main(void) {
Seawolf_loadConfig("../conf/seawolf.conf");
Seawolf_init("Pitch PID");
PID* pid;
char data[64];
double mv;
bool paused = Var_get("PitchPID.Paused");
Notify_filter(FILTER_MATCH, "UPDATED PitchPID.Coefficients");
Notify_filter(FILTER_MATCH, "UPDATED PitchPID.Heading");
Notify_filter(FILTER_MATCH, "UPDATED PitchPID.Paused");
Notify_filter(FILTER_MATCH, "UPDATED IMU");
pid = PID_new(Var_get("PitchPID.Heading"),
Var_get("PitchPID.p"),
Var_get("PitchPID.i"),
Var_get("PitchPID.d"));
dataOut(0.0);
while(true) {
Notify_get(NULL, data);
double pitch = Var_get("SEA.Pitch");
if(strcmp(data, "PitchPID.Coefficients") == 0) {
PID_setCoefficients(pid,
Var_get("PitchPID.p"),
Var_get("PitchPID.i"),
Var_get("PitchPID.d"));
PID_resetIntegral(pid);
} else if(strcmp(data, "PitchPID.Heading") == 0) {
PID_setSetPoint(pid, Var_get("PitchPID.Heading"));
mv = PID_update(pid, pitch);
if(paused) {
Var_set("PitchPID.Paused", 0.0);
}
} else if(strcmp(data, "PitchPID.Paused") == 0) {
bool p = Var_get("PitchPID.Paused");
if(p == paused) {
continue;
}
paused = p;
if(paused) {
dataOut(0.0);
Notify_send("PIDPAUSED", "Pitch");
}
} else if(strcmp(data, "IMU") == 0 && paused == false) {
mv = PID_update(pid, pitch);
}
if(paused == false) {
dataOut(mv);
}
}
Seawolf_close();
return 0;
}
uint16_t controlLoop(void){
int16_t setValue=0;
int16_t feedbackValue=0;
static int16_t old_pwm=0;
// read adc
setValue = ADC_readAndWAIT(0);
feedbackValue = ADC_readAndWAIT(1);
// apply slope formula to the feedback curve
//feedbackValue = (uint16_t)((double)feedbackValue*slopeValue);
/*#ifdef DEBUG
sprintf(bufferDummy,"%4x, %4x\n\r",setValue,feedbackValue);
USART1_sendStr(bufferDummy);
#endif*/
// Calculate error
int16_t pidret = PID_update(setValue,feedbackValue, 1);
int16_t newpwm = old_pwm + pidret;
old_pwm = newpwm;
// check for limits
if (newpwm < CURRENT_MINPWM) newpwm = CURRENT_MINPWM;
else if (newpwm > CURRENT_MAXPWM) newpwm = CURRENT_MAXPWM;
/*
// #ifdef DEBUG
// sprintf(bufferDummy,"%4x\n\r",newpwm);
// USART1_sendStr(bufferDummy);
// #endif
*/
return newpwm;
}
