// ============================================================================================
void _getReading(void)
{
uint32_t temperature, output;
// See if we have got a valid temperature
temperature = sensorReturnReading();
// Only turn the LED off if we've got legal data, otherwise flash it
if (temperature == TEMP_INVALID)
{
ledSetState(LEDFLASH_ERROR);
heaterSetLevel(0, CYCLE_LEN);
isErrored=TRUE;
sensorRequestCallback(); // Get another reading as soon as we can
return;
}
ledSetState(LEDFLASH_NORMAL);
// ...otherwise we've got a valid temp, so do the business with it
if (pidGetSetpoint(&pidInstance) == SETPOINT_IDLE)
output = 0;
else
output=pidCalc(&pidInstance,temperature);
heaterSetLevel(output, CYCLE_LEN);
contribute_log_entry(temperature, heaterGetPercentage(), pidGetSetpoint(&pidInstance), CYCLE_LEN);
// Now wait for a while before doing it all again
timerAdd(&tstate, TIMER_ORIGIN_STATEMACHINE, 0, CYCLE_LEN);
}
void __attribute__((interrupt, no_auto_psv)) _T1Interrupt(void) {
int j;
LED_3 = 1;
interrupt_count++;
if(interrupt_count == 4) {
mpuBeginUpdate();
amsEncoderStartAsyncRead();
} else if(interrupt_count == 5) {
interrupt_count = 0;
if (t1_ticks == T1_MAX) t1_ticks = 0;
t1_ticks++;
pidGetState(); // always update state, even if motor is coasting
for (j = 0; j< NUM_PIDS; j++) {
// only update tracking setpoint if time has not yet expired
if (pidObjs[j].onoff) {
if (pidObjs[j].timeFlag){
if (pidObjs[j].start_time + pidObjs[j].run_time >= t1_ticks){
pidGetSetpoint(j);
}
if(t1_ticks > lastMoveTime){ // turn off if done running all legs
pidObjs[0].onoff = 0;
pidObjs[1].onoff = 0;
}
}
else {
pidGetSetpoint(j);
}
}
}
pidSetControl();
if(pidObjs[0].onoff) {
telemGetPID();
// uart_tx_packet = ppoolRequestFullPacket(sizeof(telemStruct_t));
// if(uart_tx_packet != NULL) {
// //time|Left pstate|Right pstate|Commanded Left pstate| Commanded Right pstate|DCR|DCL|RBEMF|LBEMF|Gyrox|Gyroy|Gyroz|Ax|Ay|Az
// //bytes: 4,4,4,4,4,2,2,2,2,2,2,2,2,2,2
// paySetType(uart_tx_packet->payload, CMD_PID_TELEMETRY);
// paySetStatus(uart_tx_packet->payload, 0);
// paySetData(uart_tx_packet->payload, sizeof(telemStruct_t), (unsigned char *) &telemPIDdata);
// uart_tx_flag = 1;
}
}
LED_3 = 0;
_T1IF = 0;
}
void __attribute__((interrupt, no_auto_psv)) _T1Interrupt(void)
{
// unsigned long time_start, time_end;
// time_start = swatchTic();
if (t1_ticks == T1_MAX) t1_ticks = 0;
t1_ticks++;
if(t1_ticks > lastMoveTime) // turn off if done running
{ // pidSetInput(0, 0, 0); don't reset state when done run, keep for recording telemetry
pidObjs[0].onoff = 0;
// pidSetInput(1, 0, 0);
pidObjs[1].onoff = 0;
}
else // update velocity setpoints if needed - only when running
{ pidGetSetpoint();}
pidGetState();
pidSetControl();
/* better read telemetry at slower tick rate - updated by steering servo */
// time_end = swatchToc();
//Clear Timer1 interrupt flag
_T1IF = 0;
}
// ============================================================================================
uint32_t stateGetSetpoint(void)
// Request to find current set point
{
return pidGetSetpoint(&pidInstance);
}
// ============================================================================================
void stateChangeSetpoint(uint32_t newSetpoint)
// Request to change the setpoint
{
if (pidGetSetpoint(&pidInstance)==newSetpoint) return;
pidSetSetpoint(&pidInstance,newSetpoint);
}
// ============================================================================================
void stateLogsFlushed(void)
// Callback that the logs have been flushed - put the initial information into them
{
//.. log the record interval
logWrite(LOG_RECORD_INTERVAL, sysConfig.recordInterval);
logWrite(LOG_SETPOINT_SET, pidGetSetpoint(&pidInstance));
}
