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;
}
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;
}