예제 #1
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;
}
예제 #2
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;
}