void GrillPid::status(void) const
{
SerialX.print(getSetPoint(), DEC);
Serial_csv();
for (unsigned char i=0; i<TEMP_COUNT; ++i)
{
if (Probes[i]->hasTemperature())
SerialX.print(Probes[i]->Temperature, 1);
else
Serial_char('U');
Serial_csv();
}
SerialX.print(getPidOutput(), DEC);
Serial_csv();
SerialX.print((int)PidOutputAvg, DEC);
Serial_csv();
SerialX.print(LidOpenResumeCountdown, DEC);
}
void production_control_timer(xTimerHandle pxTimer){ // Executed every millisecond
set_debug(DEBUG8, true);
set_led(LED3, true);
// Get Set-Point from supervisor through IOI though FIFO
set_debug(DEBUG7, true);
plantParams.set_point = getSetPoint();
plantParams.theta_des = plantParams.set_point*pi/180;
//xil_printf("%d\n\r", plantParams.set_point); // uncomment if you want to print set-point at each control cycle to reconfig UART
// Read the two encoder sensors
set_debug(DEBUG7, false);
plantParams.encoder_theta = -readEncoder(SS_ENCODER_S) % 4096;
plantParams.thetaR = plantParams.encoder_theta*Kenc;
set_debug(DEBUG7, true);
plantParams.encoder_alpha= 4096+(-readEncoder(SS_ENCODER_P) % 4096);
plantParams.alphaR = plantParams.encoder_alpha*Kenc-pi;
// Calculate the control algorithm if pendulum is upright
set_debug(DEBUG7, false);
if((plantParams.alphaR >= 0 ? plantParams.alphaR:-plantParams.alphaR) < (20.*pi/180)){
// Comment out whatever controller you don't want to use
plantParams.u = calculateKalmanControlSignal(&plantParams);
//plantParams.u = calculateStateFeedbackControlSignal(&plantParams);
}
else {
plantParams.u = 0;
}
// Write voltage to the servo
set_debug(DEBUG7, true);
writeDAC(plantParams.u);
set_debug(DEBUG7, false);
++plantParams.cycle_count;
++cycleCounter;
set_led(LED3, false);
set_debug(DEBUG8, false);
}
