// ============================================================================================ 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)); }