Exemplo n.º 1
0
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
Exemplo n.º 2
0
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);
    }
}