interrupt void mainISR(void) { uint32_t timer0Cnt; uint32_t timer1Cnt; // read the timer 1 value and update the CPU usage module timer1Cnt = HAL_readTimerCnt(halHandle,1); CPU_USAGE_updateCnts(cpu_usageHandle,timer1Cnt); // read the timer 0 value and update the FEM timer0Cnt = HAL_readTimerCnt(halHandle,0); FEM_updateCnts(femHandle,timer0Cnt); FEM_run(femHandle); // toggle status LED if(gLEDcnt++ > (uint_least32_t)(USER_ISR_FREQ_Hz / LED_BLINK_FREQ_Hz)) { HAL_toggleLed(halHandle,(GPIO_Number_e)HAL_Gpio_LED2); gLEDcnt = 0; } // acknowledge the ADC interrupt HAL_acqAdcInt(halHandle,ADC_IntNumber_1); // convert the ADC data HAL_readAdcData(halHandle,&gAdcData); // run the controller CTRL_run(ctrlHandle,halHandle,&gAdcData,&gPwmData); // write the PWM compare values HAL_writePwmData(halHandle,&gPwmData); // setup the controller CTRL_setup(ctrlHandle); // read the timer 1 value and update the CPU usage module timer1Cnt = HAL_readTimerCnt(halHandle,1); CPU_USAGE_updateCnts(cpu_usageHandle,timer1Cnt); // run the CPU usage module CPU_USAGE_run(cpu_usageHandle); return; } // end of mainISR() function
void main(void) { // Only used if running from FLASH // Note that the variable FLASH is defined by the project #ifdef FLASH // Copy time critical code and Flash setup code to RAM // The RamfuncsLoadStart, RamfuncsLoadEnd, and RamfuncsRunStart // symbols are created by the linker. Refer to the linker files. memCopy((uint16_t *) &RamfuncsLoadStart, (uint16_t *) &RamfuncsLoadEnd, (uint16_t *) &RamfuncsRunStart); #endif // initialize the hardware abstraction layer halHandle = HAL_init(&hal, sizeof(hal)); // check for errors in user parameters USER_checkForErrors(&gUserParams); // initialize the user parameters USER_setParams(&gUserParams); // set the hardware abstraction layer parameters HAL_setParams(halHandle, &gUserParams); // setup faults HAL_setupFaults(halHandle); // yo these are important HAL_initIntVectorTable(halHandle); HAL_enableAdcInts(halHandle); // enable global interrupts HAL_enableGlobalInts(halHandle); // enable debug interrupts HAL_enableDebugInt(halHandle); // disable the gimbal PWMs to get their interrupts HAL_disablePwm(halHandle); // enable SPI A slave and SPI interrupts HAL_enableSpiInt(halHandle); HAL_writeSpiSlaveData(halHandle, 30); // Just some junk for the TX buffer, probably don't need // ayyyydc perspection_adc_init(halHandle); // Start up a timer to calculate angular velocity HAL_startTimer(halHandle, 2); // reset shit haptics_duty_cycle = 0.0; isSystemEnabled = false; lastPosition = 0.0; for (;;) { // HAL_turnLedOn(halHandle, (GPIO_Number_e)HAL_Gpio_LED2); processSpiMessages(); double position = (360.0 * (double)HAL_getQepPosnCounts(halHandle)) / (double) HAL_getQepPosnMaximum(halHandle); uint32_t timeDiff = 0xFFFFFFFF - HAL_readTimerCnt(halHandle, 2); HAL_reloadTimer(halHandle, 2); double velocity = (position - lastPosition) / ((double)timeDiff / ((double)gUserParams.systemFreq_MHz * 1000000.0)); double desired_torque = torque_from_motion(position, velocity); if (desired_torque < -1.0) { desired_torque = -1.0; } else if (desired_torque > 1.0) { desired_torque = 1.0; } adjust_duty_cycle(desired_torque); lastPosition = position; // HAL_turnLedOff(halHandle, (GPIO_Number_e)HAL_Gpio_LED2); } }