Piano keys controling segbot

//#############################################################################
// FILE:   labstarter_main.c
//
// TITLE:  Final Project
//#############################################################################

// Included Files
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <string.h>
#include <math.h>
#include <limits.h>
#include "F28x_Project.h"
#include "driverlib.h"
#include "device.h"
#include "f28379dSerial.h"
#include "dsp.h"
#include "fpu32/fpu_rfft.h"

#define PI          3.1415926535897932384626433832795
#define TWOPI       6.283185307179586476925286766559
#define HALFPI      1.5707963267948966192313216916398

// variables initialization
uint16_t adcb0result=0;
uint16_t pingpong=0, inputindex=0;
uint16_t runping=0, runpong=0;
uint16_t ADCBcounter=0;
uint32_t histgram[512];
int16_t ignoretime=0, ignorecount=0;


// variables from lab7
uint16_t LEDdisplaynum = 0;
int16_t gyroxraw = 0;
int16_t gyroyraw = 0;
int16_t gyrozraw = 0;
int16_t accelxraw = 0;
int16_t accelyraw = 0;
int16_t accelzraw = 0;
float adca0, adca1;
float gyrox=0.0, gyroy=0.0, gyroz=0.0, accelx=0.0, accely=0.0, accelz=0.0;

float LeftWheel=0, RightWheel=0, leftd=0, rightd=0;
float uleft=5.0, uright=5.0;
float vleft=0, vright=0;

// lab7 exercise 2
float accelx_offset = 0;
float accely_offset = 0;
float accelz_offset = 0;
float gyrox_offset  = 0;
float gyroy_offset  = 0;
float gyroz_offset  = 0;
float accelzBalancePoint = -0.77;
int16 IMU_data[9];
uint16_t temp=0;
int16_t doneCal = 0;
float tilt_value    = 0;
float tilt_array[4] = {0, 0, 0, 0};
float gyro_value    = 0;
float gyro_array[4] = {0, 0, 0, 0};
float LeftWheelArray[4] = {0,0,0,0};
float RightWheelArray[4] = {0,0,0,0};
// Kalman Filter vars
float T = 0.001;        //sample rate, 1ms
float Q = 0.01; // made global to enable changing in runtime
float R = 25000;//50000;
float kalman_tilt = 0;
float kalman_P = 22.365;
int16_t SpibNumCalls = -1;
float pred_P = 0;
float kalman_K = 0;
float ubal;
int32_t timecount = 0;
int16_t calibration_state = 0;
int32_t calibration_count = 0;

// lab7 exercise 4
float whldiff_k,turnref_k,errdiff_k,integralerr_k,fboffset=0.85,turn,turnrate_k;
float whldiff_k1, turnref_k1,vwhldiff_k, vwhldiff_k1,errdiff_k1,integralerr_k1,turnrate_k1;



//*****************************************************************************
// the defines for FFT
//*****************************************************************************
#define RFFT_STAGES     10
#define RFFT_SIZE       (1 << RFFT_STAGES)

//*****************************************************************************
// the globals
//*****************************************************************************
#ifdef __cplusplus
#pragma DATA_SECTION("FFT_buffer_2")
#else
#pragma DATA_SECTION(pwrSpec, "FFT_buffer_2")
#endif
float pwrSpec[(RFFT_SIZE/2)+1];
float maxpwr = 0;
int16_t maxpwrindex = 0;

#ifdef __cplusplus
#pragma DATA_SECTION("FFT_buffer_2")
#else
#pragma DATA_SECTION(test_output, "FFT_buffer_2")
#endif
float test_output[RFFT_SIZE];


#ifdef __cplusplus
#pragma DATA_SECTION("FFT_buffer_1")
#else
#pragma DATA_SECTION(ping_input, "FFT_buffer_1")
#endif
float ping_input[RFFT_SIZE];

#ifdef __cplusplus
#pragma DATA_SECTION("FFT_buffer_1")
#else
#pragma DATA_SECTION(pong_input, "FFT_buffer_1")
#endif
float pong_input[RFFT_SIZE];


#ifdef __cplusplus
#pragma DATA_SECTION("FFT_buffer_2")
#else
#pragma DATA_SECTION(RFFTF32Coef,"FFT_buffer_2")
#endif //__cplusplus
//! \brief Twiddle Factors
//!
float RFFTF32Coef[RFFT_SIZE];


//! \brief Object of the structure RFFT_F32_STRUCT
//!
RFFT_F32_STRUCT rfft;

//! \brief Handle to the RFFT_F32_STRUCT object
//!
RFFT_F32_STRUCT_Handle hnd_rfft = &rfft;


// Interrupt Service Routines predefinition
__interrupt void cpu_timer0_isr(void);
__interrupt void cpu_timer1_isr(void);
__interrupt void cpu_timer2_isr(void);
__interrupt void SWI_isr(void);
__interrupt void ADCA_ISR(void);
__interrupt void ADCB_ISR(void);
__interrupt void SPIB_isr(void);

void setEPWM2A(float controleffort);
void setEPWM2B(float controleffort);
void setupSpib(void);
void init_eQEPs(void);
float readEncLeft(void);
float readEncRight(void);
void SetLEDRowsOnOff(uint16_t number);

void serialRXA(serial_t *s, char data);

// Count variables
uint32_t numTimer0calls = 0;
uint32_t numSWIcalls = 0;
uint32_t numRXA = 0;
uint16_t UARTPrint = 0;


void main(void)
{
    // PLL, WatchDog, enable Peripheral Clocks
    // This example function is found in the F2837xD_SysCtrl.c file.
    InitSysCtrl();

    InitGpio();

    // Blue LED on LuanchPad
    GPIO_SetupPinMux(31, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(31, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPASET.bit.GPIO31 = 1;

    // Red LED on LaunchPad
    GPIO_SetupPinMux(34, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(34, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPBSET.bit.GPIO34 = 1;

    // LED1 and PWM Pin
    GPIO_SetupPinMux(22, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(22, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPACLEAR.bit.GPIO22 = 1;

    // LED2
    GPIO_SetupPinMux(94, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(94, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPCCLEAR.bit.GPIO94 = 1;

    // LED3
    GPIO_SetupPinMux(95, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(95, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPCCLEAR.bit.GPIO95 = 1;

    // LED4
    GPIO_SetupPinMux(97, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(97, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPDCLEAR.bit.GPIO97 = 1;

    // LED5
    GPIO_SetupPinMux(111, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(111, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPDCLEAR.bit.GPIO111 = 1;

    // LED6
    GPIO_SetupPinMux(130, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(130, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPECLEAR.bit.GPIO130 = 1;

    // LED7
    GPIO_SetupPinMux(131, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(131, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPECLEAR.bit.GPIO131 = 1;

    // LED8
    GPIO_SetupPinMux(25, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(25, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPACLEAR.bit.GPIO25 = 1;

    // LED9
    GPIO_SetupPinMux(26, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(26, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPACLEAR.bit.GPIO26 = 1;

    // LED10
    GPIO_SetupPinMux(27, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(27, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPACLEAR.bit.GPIO27 = 1;

    // LED11
    GPIO_SetupPinMux(60, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(60, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPBCLEAR.bit.GPIO60 = 1;

    // LED12
    GPIO_SetupPinMux(61, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(61, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPBCLEAR.bit.GPIO61 = 1;

    // LED13
    GPIO_SetupPinMux(157, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(157, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPECLEAR.bit.GPIO157 = 1;

    // LED14
    GPIO_SetupPinMux(158, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(158, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPECLEAR.bit.GPIO158 = 1;

    // LED15
    GPIO_SetupPinMux(159, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(159, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPECLEAR.bit.GPIO159 = 1;

    // LED16
    GPIO_SetupPinMux(160, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(160, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPFCLEAR.bit.GPIO160 = 1;

    //DRV8874 #1 DIR  Direction
    GPIO_SetupPinMux(29, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(29, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPASET.bit.GPIO29 = 1;

    //DRV8874 #2 DIR  Direction
    GPIO_SetupPinMux(32, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(32, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPBSET.bit.GPIO32 = 1;

    //MPU9250  CS  Chip Select
    GPIO_SetupPinMux(66, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(66, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPCSET.bit.GPIO66 = 1;

    //WIZNET  CS  Chip Select
    GPIO_SetupPinMux(125, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(125, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPDSET.bit.GPIO125 = 1;

    //SPIRAM  CS  Chip Select
    GPIO_SetupPinMux(19, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(19, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPASET.bit.GPIO19 = 1;

    //PushButton 1
    GPIO_SetupPinMux(4, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(4, GPIO_INPUT, GPIO_PULLUP);

    //PushButton 2
    GPIO_SetupPinMux(5, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(5, GPIO_INPUT, GPIO_PULLUP);

    //PushButton 3
    GPIO_SetupPinMux(6, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(6, GPIO_INPUT, GPIO_PULLUP);

    //PushButton 4
    GPIO_SetupPinMux(7, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(7, GPIO_INPUT, GPIO_PULLUP);

    //Joy Stick Pushbutton
    GPIO_SetupPinMux(8, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(8, GPIO_INPUT, GPIO_PULLUP);

    // Clear all interrupts and initialize PIE vector table:
    // Disable CPU interrupts
    DINT;

    // Initialize the PIE control registers to their default state.
    // The default state is all PIE interrupts disabled and flags
    // are cleared.
    // This function is found in the F2837xD_PieCtrl.c file.
    InitPieCtrl();

    // Disable CPU interrupts and clear all CPU interrupt flags:
    IER = 0x0000;
    IFR = 0x0000;

    // Initialize the PIE vector table with pointers to the shell Interrupt
    // Service Routines (ISR).
    // This will populate the entire table, even if the interrupt
    // is not used in this example.  This is useful for debug purposes.
    // The shell ISR routines are found in F2837xD_DefaultIsr.c.
    // This function is found in F2837xD_PieVect.c.
    InitPieVectTable();

    // Interrupts that are used in this example are re-mapped to
    // ISR functions found within this project
    EALLOW;  // This is needed to write to EALLOW protected registers
    PieVectTable.TIMER0_INT = &cpu_timer0_isr;
    PieVectTable.TIMER1_INT = &cpu_timer1_isr;
    PieVectTable.TIMER2_INT = &cpu_timer2_isr;
    PieVectTable.SCIA_RX_INT = &RXAINT_recv_ready;
    PieVectTable.SCIC_RX_INT = &RXCINT_recv_ready;
    PieVectTable.SCID_RX_INT = &RXDINT_recv_ready;
    PieVectTable.SCIA_TX_INT = &TXAINT_data_sent;
    PieVectTable.SCIC_TX_INT = &TXCINT_data_sent;
    PieVectTable.SCID_TX_INT = &TXDINT_data_sent;
    PieVectTable.ADCA1_INT=&ADCA_ISR;
    PieVectTable.ADCB1_INT=&ADCB_ISR;
    PieVectTable.SPIB_RX_INT = &SPIB_isr;
    PieVectTable.EMIF_ERROR_INT = &SWI_isr;
    EDIS;    // This is needed to disable write to EALLOW protected registers


    // Initialize the CpuTimers Device Peripheral. This function can be
    // found in F2837xD_CpuTimers.c
    InitCpuTimers();

    // Configure CPU-Timer 0, 1, and 2 to interrupt every second:
    // 200MHz CPU Freq, 1 second Period (in uSeconds)
    ConfigCpuTimer(&CpuTimer0, 200, 1000);
    ConfigCpuTimer(&CpuTimer1, 200, 4000);
    ConfigCpuTimer(&CpuTimer2, 200, 40000);

    // Enable CpuTimer Interrupt bit TIE
    CpuTimer0Regs.TCR.all = 0x4000;
    CpuTimer1Regs.TCR.all = 0x4000;
    CpuTimer2Regs.TCR.all = 0x4000;

    init_serial(&SerialA,115200,serialRXA);
//    init_serial(&SerialC,115200,serialRXC);
//    init_serial(&SerialD,115200,serialRXD);

    // epwm5 setup
    EALLOW;
    EPwm5Regs.ETSEL.bit.SOCAEN = 0; // Disable SOC on A group
    EPwm5Regs.TBCTL.bit.CTRMODE = 3; // freeze counter
    EPwm5Regs.ETSEL.bit.SOCASEL = 2; // Select Event when counter equal to PRD
    EPwm5Regs.ETPS.bit.SOCAPRD = 1; // Generate pulse on 1st event (“pulse” is the same as “trigger”)
    EPwm5Regs.TBCTR = 0x0; // Clear counter
    EPwm5Regs.TBPHS.bit.TBPHS = 0x0000; // Phase is 0
    EPwm5Regs.TBCTL.bit.PHSEN = 0; // Disable phase loading
    EPwm5Regs.TBCTL.bit.CLKDIV = 0; // divide by 1  50Mhz Clock
    EPwm5Regs.TBPRD = 50000;  // Set Period to 1ms sample.  Input clock is 50MHz.
    // Notice here that we are not setting CMPA or CMPB because we are not using the PWM signal
    EPwm5Regs.ETSEL.bit.SOCAEN = 1; //enable SOCA
    EPwm5Regs.TBCTL.bit.CTRMODE = 0; //unfreeze, and enter up count mode
    EDIS;

    // epwm4 setup
    EALLOW;
    EPwm4Regs.ETSEL.bit.SOCAEN = 0; // Disable SOC on A group
    EPwm4Regs.TBCTL.bit.CTRMODE = 3; // freeze counter
    EPwm4Regs.ETSEL.bit.SOCASEL = 2; // Select Event when counter equal to PRD
    EPwm4Regs.ETPS.bit.SOCAPRD = 1; // Generate pulse on 1st event (“pulse” is the same as “trigger”)
    EPwm4Regs.TBCTR = 0x0; // Clear counter
    EPwm4Regs.TBPHS.bit.TBPHS = 0x0000; // Phase is 0
    EPwm4Regs.TBCTL.bit.PHSEN = 0; // Disable phase loading
    EPwm4Regs.TBCTL.bit.CLKDIV = 0; // divide by 1  50Mhz Clock
    EPwm4Regs.TBPRD = 5000;  // Set Period to 1ms sample.  Input clock is 50MHz.
    // Notice here that we are not setting CMPA or CMPB because we are not using the PWM signal
    EPwm4Regs.ETSEL.bit.SOCAEN = 1; //enable SOCA
    EPwm4Regs.TBCTL.bit.CTRMODE = 0; //unfreeze, and enter up count mode
    EDIS;


    EPwm2Regs.TBCTL.bit.CLKDIV=0;
    EPwm2Regs.TBCTL.bit.CTRMODE=0;
    EPwm2Regs.TBCTL.bit.FREE_SOFT=3;
    EPwm2Regs.TBCTL.bit.PHSEN=0;
    EPwm2Regs.TBCTR=0;
    EPwm2Regs.CMPA.bit.CMPA=0;
    EPwm2Regs.CMPB.bit.CMPB=0;
    EPwm2Regs.TBPRD=2500;
    EPwm2Regs.AQCTLA.bit.CAU=1;
    EPwm2Regs.AQCTLA.bit.ZRO=2;
    EPwm2Regs.AQCTLB.bit.CBU=1;
    EPwm2Regs.AQCTLB.bit.ZRO=2;
    EPwm2Regs.TBPHS.bit.TBPHS=0;

    GPIO_SetupPinMux(2, GPIO_MUX_CPU1, 1);
    GPIO_SetupPinMux(3, GPIO_MUX_CPU1, 1);

    EALLOW;  // Below are protected registers
    GpioCtrlRegs.GPAPUD.bit.GPIO2 = 1; // For EPWM2A
    GpioCtrlRegs.GPAPUD.bit.GPIO3 = 1; // For EPWM2B
    EDIS;

    EALLOW;//write configurations for all ADCs  ADCA, ADCB, ADCC, ADCD
    AdcaRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
    AdcSetMode(ADC_ADCA, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE);  //read calibration settings
    AdcaRegs.ADCCTL1.bit.INTPULSEPOS = 1;
    AdcaRegs.ADCCTL1.bit.ADCPWDNZ = 1;

    AdcbRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
    AdcSetMode(ADC_ADCB, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE);  //read calibration settings
    AdcbRegs.ADCCTL1.bit.INTPULSEPOS = 1;
    AdcbRegs.ADCCTL1.bit.ADCPWDNZ = 1;
    DELAY_US(1000);
    //Select the channels to convert and end of conversion flag
    //Many statements commented out,  To be used when using ADCA or ADCB
    AdcaRegs.ADCSOC0CTL.bit.CHSEL = 2;  //SOC0 will convert Channel you choose Does not have to be A0
    AdcaRegs.ADCSOC0CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    AdcaRegs.ADCSOC0CTL.bit.TRIGSEL = 13;// EPWM5 ADCSOCA or another trigger you choose will trigger SOC0
    AdcaRegs.ADCSOC1CTL.bit.CHSEL = 3;  //SOC1 will convert Channel you choose Does not have to be A1
    AdcaRegs.ADCSOC1CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    AdcaRegs.ADCSOC1CTL.bit.TRIGSEL = 13;// EPWM5 ADCSOCA or another trigger you choose will trigger SOC1
    AdcaRegs.ADCINTSEL1N2.bit.INT1SEL = 1; //set to last SOC that is converted and it will set INT1 flag ADCA1
    AdcaRegs.ADCINTSEL1N2.bit.INT1E = 1;   //enable INT1 flag//AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //make sure INT1 flag is cleared
    AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //make sure INT1 flag is cleared

    //ADCB
    AdcbRegs.ADCSOC0CTL.bit.CHSEL = 4;  //SOC0 will convert Channel you choose Does not have to be B0
    AdcbRegs.ADCSOC0CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    AdcbRegs.ADCSOC0CTL.bit.TRIGSEL = 11;  // EPWM5 ADCSOCA or another trigger you choose will trigger SOC0
    //AdcbRegs.ADCSOC1CTL.bit.CHSEL = ???;  //SOC1 will convert Channel you choose Does not have to be B1
    //AdcbRegs.ADCSOC1CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    //AdcbRegs.ADCSOC1CTL.bit.TRIGSEL = ???;  // EPWM5 ADCSOCA or another trigger you choose will trigger SOC1
    //AdcbRegs.ADCSOC2CTL.bit.CHSEL = ???;  //SOC2 will convert Channel you choose Does not have to be B2
    //AdcbRegs.ADCSOC2CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    //AdcbRegs.ADCSOC2CTL.bit.TRIGSEL = ???;  // EPWM5 ADCSOCA or another trigger you choose will trigger SOC2
    //AdcbRegs.ADCSOC3CTL.bit.CHSEL = ???;  //SOC3 will convert Channel you choose Does not have to be B3
    //AdcbRegs.ADCSOC3CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    //AdcbRegs.ADCSOC3CTL.bit.TRIGSEL = ???;  // EPWM5 ADCSOCA or another trigger you choose will trigger SOC3
    AdcbRegs.ADCINTSEL1N2.bit.INT1SEL = 0; //set to last SOC that is converted and it will set INT1 flag ADCB1
    AdcbRegs.ADCINTSEL1N2.bit.INT1E = 1;   //enable INT1 flag//AdcbRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //make sure INT1 flag is cleared
    AdcbRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //make sure INT1 flag is cleared
    EDIS;

    init_eQEPs();

    setupSpib();

    // Enable CPU int1 which is connected to CPU-Timer 0, CPU int13
    // which is connected to CPU-Timer 1, and CPU int 14, which is connected
    // to CPU-Timer 2:  int 12 is for the SWI.  
    IER |= M_INT1;
    IER |= M_INT8;  // SCIC SCID
    IER |= M_INT9;  // SCIA
    IER |= M_INT12;
    IER |= M_INT13;
    IER |= M_INT14;
    IER |= M_INT6;

    // Enable TINT0 in the PIE: Group 1 interrupt 7
    PieCtrlRegs.PIEIER1.bit.INTx7 = 1;
    // Enable SWI in the PIE: Group 12 interrupt 9
    PieCtrlRegs.PIEIER12.bit.INTx9 = 1;

    PieCtrlRegs.PIEIER6.bit.INTx3 = 1;   // spib
    PieCtrlRegs.PIEIER1.bit.INTx1 = 1;   // adca
    PieCtrlRegs.PIEIER1.bit.INTx2 = 1;   // interrupt for ADCB


    int16_t i = 0;
    float samplePeriod = 0.0002;

    // Clear input buffers:
    for(i=0; i < RFFT_SIZE; i++){
        ping_input[i] = 0.0f;
    }

    for (i=0;i<RFFT_SIZE;i++) {
        ping_input[i] = sin(125*2*PI*i*samplePeriod)+2*sin(2400*2*PI*i*samplePeriod);
    }
    hnd_rfft->FFTSize   = RFFT_SIZE;
    hnd_rfft->FFTStages = RFFT_STAGES;
    hnd_rfft->InBuf     = &ping_input[0];  //Input buffer
    hnd_rfft->OutBuf    = &test_output[0];  //Output buffer
    hnd_rfft->MagBuf    = &pwrSpec[0];  //Magnitude buffer

    hnd_rfft->CosSinBuf = &RFFTF32Coef[0];  //Twiddle factor buffer
    RFFT_f32_sincostable(hnd_rfft);         //Calculate twiddle factor

    for (i=0; i < RFFT_SIZE; i++){
          test_output[i] = 0;               //Clean up output buffer
    }

    for (i=0; i <= RFFT_SIZE/2; i++){
         pwrSpec[i] = 0;                //Clean up magnitude buffer
    }

// run fft 10 times just to under what the FFT does
//    int16_t tries = 0;
//    while(tries < 10) {
//        hnd_rfft->InBuf     = &ping_input[0];  //Input buffer
//        RFFT_f32(hnd_rfft);                     //Calculate real FFT
//
//#ifdef __TMS320C28XX_TMU__ //defined when --tmu_support=tmu0 in the project
//        // properties
//        RFFT_f32_mag_TMU0(hnd_rfft);            //Calculate magnitude
//#else
//        RFFT_f32_mag(hnd_rfft);                 //Calculate magnitude
//#endif
//        maxpwr = 0;
//        maxpwrindex = 0;
//
//        for (i=0;i<(RFFT_SIZE/2);i++) {
//            if (pwrSpec[i]>maxpwr) {
//                maxpwr = pwrSpec[i];
//                maxpwrindex = i;
//            }
//        }
//
//        tries++;
//        for (i=0;i<RFFT_SIZE;i++) {
//            ping_input[i] = sin((125 + tries*125)*2*PI*i*samplePeriod)+2*sin((2400-tries*200)*2*PI*i*samplePeriod);
//        }
//    }

    // Enable global Interrupts and higher priority real-time debug events
    EINT;  // Enable Global interrupt INTM
    ERTM;  // Enable Global realtime interrupt DBGM

    
    // IDLE loop. Just sit and loop forever (optional):
    int counter=0;
    while(1)
    {

        if (UARTPrint == 1 ) {
            serial_printf(&SerialA,"maxpwr:%.2f  maxpwrindex:%d counter: %d\r\n",maxpwr,maxpwrindex,counter);
            UARTPrint = 0;
        }
        int base=90, off=16,threshold=155;

        if (runping == 1) {
            counter++;
            runping = 0;
            hnd_rfft->InBuf     = &ping_input[0];  //Input buffer
            RFFT_f32(hnd_rfft);                     //Calculate real FFT

#ifdef __TMS320C28XX_TMU__ //defined when --tmu_support=tmu0 in the project
            // properties
            RFFT_f32_mag_TMU0(hnd_rfft);            //Calculate magnitude
#else
            RFFT_f32_mag(hnd_rfft);                 //Calculate magnitude
#endif
            maxpwr = 0;
            maxpwrindex = 0;

            for (i=3;i<(RFFT_SIZE/2);i++) {
                if (pwrSpec[i]>maxpwr) {
                    maxpwr = pwrSpec[i];
                    maxpwrindex = i;
                }
            }
            // Where you will make decisions depending on what maxpwr and maxpwrindex


            if(maxpwr>threshold && maxpwr<800 && ignorecount>=ignoretime){
                //fboffset = -0.5;
                if(maxpwrindex>=60){
                    turnref_k = turnref_k - 1.5;
                    //turnrate_k -=0.2;
                    ignorecount=0;
                    ignoretime=5;
                } else if(maxpwrindex<34 && maxpwrindex>=30){
                    //fboffset =0.3;
                    turnref_k = turnref_k +1.5;
                    //turnrate_k +=0.2;
                    ignorecount=0;
                    ignoretime=5;
                } else if(maxpwrindex<30 && maxpwrindex>16){

                    fboffset -= 0.035;
                    ignorecount=0;
                    ignoretime=5;
                } else if(maxpwrindex<=15){
                      //r
                    fboffset += 0.035;
                    ignorecount=0;
                    ignoretime=25;
                } else{
                    if(fboffset>0.85){
                        fboffset-=0.2;
                    } else if(fboffset<0.85){
                        fboffset+=0.2;
                    }
                }
            } else {
                turnrate_k = 0;
//                if(fboffset>0.8){
//                    fboffset-=0.2;
//                } else if(fboffset<0.8){
//                    fboffset+=0.2;
//                }
                ignorecount++;
            }

            int i;
            for(i=1;i<16;i++){
                if(maxpwr>base+off*i && maxpwr<=base+off*(i+1)){
                    SetLEDRowsOnOff(i);
                }
            }
        }

        if (runpong == 1) {
            counter++;
            runpong = 0;
            hnd_rfft->InBuf     = &pong_input[0];  //Input buffer
            RFFT_f32(hnd_rfft);                     //Calculate real FFT

#ifdef __TMS320C28XX_TMU__ //defined when --tmu_support=tmu0 in the project
            // properties
            RFFT_f32_mag_TMU0(hnd_rfft);            //Calculate magnitude
#else
            RFFT_f32_mag(hnd_rfft);                 //Calculate magnitude
#endif
            maxpwr = 0;
            maxpwrindex = 0;

            for (i=3;i<(RFFT_SIZE/2);i++) {
                if (pwrSpec[i]>maxpwr) {
                    maxpwr = pwrSpec[i];
                    maxpwrindex = i;
                }
            }
            // Where you will make decisions depending on what maxpwr and maxpwrindex
            if(maxpwr>threshold && maxpwr<800 && ignorecount>=ignoretime){
                //fboffset = -0.5;
                if(maxpwrindex>=60){
                    turnref_k = turnref_k - 1.5;
                    //turnrate_k -=0.2;
                    ignorecount=0;
                    ignoretime=5;
                } else if(maxpwrindex<34 && maxpwrindex>=30){
                    //fboffset =0.3;
                    turnref_k = turnref_k +1.5;
                    //turnrate_k +=0.2;
                    ignorecount=0;
                    ignoretime=5;
                } else if(maxpwrindex<30 && maxpwrindex>16){

                    fboffset -= 0.035;
                    ignorecount=0;
                    ignoretime=5;
                } else if(maxpwrindex<=15){
                    //r
                    fboffset += 0.035;
                    ignorecount=0;
                    ignoretime=25;
                } else{
                    if(fboffset>0.85){
                      fboffset-=0.2;
                    } else if(fboffset<0.85){
                      fboffset+=0.2;
                    }
                }
            } else {
                turnrate_k = 0;
//                if(fboffset>0.8){
//                    fboffset-=0.2;
//                } else if(fboffset<0.8){
//                    fboffset+=0.2;
//                }
                ignorecount++;
            }
            int i;
            for(i=1;i<16;i++){
                if(maxpwr>base+off*i && maxpwr<=base+off*(i+1)){
                    SetLEDRowsOnOff(i);
                }
            }
        }
        if(maxpwr>110){
            histgram[maxpwrindex]++;
        }

    }
}





// SWI_isr,  Using this interrupt as a Software started interrupt
float k1 = -60.0, k2 = -4.5, k3 = -1.1;
float Kp = 3.0, Ki = 20.0, Kd = 0.02;
float vleft_k1,vright_k1,LeftWheel_k1,RightWheel_k1;
__interrupt void SWI_isr(void) {

    // These three lines of code allow SWI_isr, to be interrupted by other interrupt functions
    // making it lower priority than all other Hardware interrupts.
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP12;
    asm("       NOP");                    // Wait one cycle
    EINT;                                 // Clear INTM to enable interrupts



    // Insert SWI ISR Code here.......
    vleft=0.6*vleft_k1+100*(LeftWheel-LeftWheel_k1);
    vright=0.6*vright_k1+100*(RightWheel-RightWheel_k1);

    ubal=-k1*tilt_value-k2*gyro_value-k3*(vleft+vright)/2.0;
    uleft = ubal/2;
    uright = ubal/2;

    vleft_k1=vleft;
    vright_k1=vright;
    LeftWheel_k1=LeftWheel;
    RightWheel_k1=RightWheel;

    // lab7 exercise 4.1
    whldiff_k=LeftWheel-RightWheel;
    vwhldiff_k=0.333*vwhldiff_k1+166.667*(whldiff_k-whldiff_k1);
    // 4.2
    //turnref_k=turnref_k+0.004*(turnrate_k+turnrate_k1)/2;
    errdiff_k = turnref_k-whldiff_k;
    integralerr_k=integralerr_k+0.004*(errdiff_k+errdiff_k1)/2;

    turn = Kp*errdiff_k + Ki*integralerr_k - Kd*vwhldiff_k;
    if(fabs(turn)>=3){ //  prevent wind-up
        integralerr_k=integralerr_k1;
    }
   // saturate turn between -4 and 4
    if(turn>=4){
        turn=4;
    }
    if(turn<=-4){
        turn=-4;
    }

    uright=ubal/2.0-turn+fboffset;
    uleft=ubal/2.0+turn+fboffset;

    setEPWM2B(-uleft);
    setEPWM2A(uright);

    // save states
    whldiff_k1=whldiff_k;
    vwhldiff_k1=vwhldiff_k;
    turnref_k1=turnref_k;
    errdiff_k1=errdiff_k;
    integralerr_k1=integralerr_k;
    turnrate_k1=turnrate_k;

    numSWIcalls++;

    // Blink a number of LEDS
//    GpioDataRegs.GPATOGGLE.bit.GPIO27 = 1;
//    GpioDataRegs.GPBTOGGLE.bit.GPIO60 = 1;
//    GpioDataRegs.GPBTOGGLE.bit.GPIO61 = 1;
//    GpioDataRegs.GPETOGGLE.bit.GPIO157 = 1;
//    GpioDataRegs.GPETOGGLE.bit.GPIO158 = 1;
    
    DINT;
}

// cpu_timer0_isr - CPU Timer0 ISR
__interrupt void cpu_timer0_isr(void)
{
    CpuTimer0.InterruptCount++;

    numTimer0calls++;

//    if ((numTimer0calls%50) == 0) {
//        PieCtrlRegs.PIEIFR12.bit.INTx9 = 1;  // Manually cause the interrupt for the SWI
//    }


    // Blink LaunchPad Red LED
    //GpioDataRegs.GPBTOGGLE.bit.GPIO34 = 1;
    // Blink a number of LEDS
//    GpioDataRegs.GPATOGGLE.bit.GPIO7 = 1;
//    GpioDataRegs.GPATOGGLE.bit.GPIO8 = 1;
//    GpioDataRegs.GPATOGGLE.bit.GPIO9 = 1;
//    GpioDataRegs.GPATOGGLE.bit.GPIO24 = 1;
//    GpioDataRegs.GPATOGGLE.bit.GPIO25 = 1;
//    GpioDataRegs.GPATOGGLE.bit.GPIO26 = 1;

//    if ((numTimer0calls%250) == 0) {
//        displayLEDletter(LEDdisplaynum);
//        LEDdisplaynum++;
//        if (LEDdisplaynum == 0xFFFF) {  // prevent roll over exception
//            LEDdisplaynum = 0;
//        }
//    }
    // Acknowledge this interrupt to receive more interrupts from group 1
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
}

// cpu_timer1_isr - CPU Timer1 ISR
__interrupt void cpu_timer1_isr(void)
{
    // Blink a number of LEDS
//    GpioDataRegs.GPATOGGLE.bit.GPIO22 = 1;
//    GpioDataRegs.GPBTOGGLE.bit.GPIO52 = 1;
//    GpioDataRegs.GPCTOGGLE.bit.GPIO67 = 1;
//    GpioDataRegs.GPCTOGGLE.bit.GPIO94 = 1;
//    GpioDataRegs.GPCTOGGLE.bit.GPIO95 = 1;
//    GpioDataRegs.GPDTOGGLE.bit.GPIO97 = 1;

    CpuTimer1.InterruptCount++;
}

// cpu_timer2_isr CPU Timer2 ISR
__interrupt void cpu_timer2_isr(void)
{


    // Blink LaunchPad Blue LED
    //GpioDataRegs.GPATOGGLE.bit.GPIO31 = 1;
    // Blink a number of LEDS
//    GpioDataRegs.GPDTOGGLE.bit.GPIO111 = 1;
//    GpioDataRegs.GPETOGGLE.bit.GPIO130 = 1;
//    GpioDataRegs.GPETOGGLE.bit.GPIO131 = 1;
//    GpioDataRegs.GPATOGGLE.bit.GPIO4 = 1;
//    GpioDataRegs.GPATOGGLE.bit.GPIO5 = 1;
//    GpioDataRegs.GPATOGGLE.bit.GPIO6 = 1;
    CpuTimer2.InterruptCount++;
//  if ((CpuTimer2.InterruptCount % 50) == 0) {
//      UARTPrint = 1;
//  }
}

__interrupt void ADCA_ISR (void){
    //adca0result = AdcaResultRegs.ADCRESULT0;
    //adca1result = AdcaResultRegs.ADCRESULT1;
    //adca0=adca0result*3.0/4095.0;
    //adca1=adca1result*3.0/4095.0;

    GpioDataRegs.GPCCLEAR.bit.GPIO66  = 1;
    SpibRegs.SPIFFRX.bit.RXFFIL = 8;
    SpibRegs.SPITXBUF=(0x8000)|(0x3A00);
    SpibRegs.SPITXBUF=0;
    SpibRegs.SPITXBUF=0;
    SpibRegs.SPITXBUF=0;

    SpibRegs.SPITXBUF=0;

    SpibRegs.SPITXBUF=0;
    SpibRegs.SPITXBUF=0;
    SpibRegs.SPITXBUF=0;
    //ADCAcounter++;
//    if((ADCAcounter%100)==0){
//        UARTPrint = 1;
//    }

    AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1;  //clear interrupt flag
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
}

float yk, dac0;
__interrupt void ADCB_ISR (void){
    GpioDataRegs.GPBSET.bit.GPIO52  = 1;
    adcb0result = AdcbResultRegs.ADCRESULT0;

    yk=adcb0result*3.0/4095.0;

    if(pingpong==0){
        ping_input[inputindex]=yk;
        inputindex++;
        if(inputindex>=1024){
            inputindex=0;
            runping=1;
            pingpong=1;
        }

    } else{
        pong_input[inputindex]=yk;
        inputindex++;
        if(inputindex>=1024){
            inputindex=0;
            runpong=1;
            pingpong=0;
        }
    }

    ADCBcounter++;
    if((ADCBcounter%400)==0){
        UARTPrint = 1;
    }

    AdcbRegs.ADCINTFLGCLR.bit.ADCINT1 = 1;  //clear interrupt flag
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
    GpioDataRegs.GPBCLEAR.bit.GPIO52  = 1;
}

void init_eQEPs(void) {

    // setup eQEP1 pins for input
    EALLOW;
    //Disable internal pull-up for the selected output pins for reduced power consumption
    GpioCtrlRegs.GPAPUD.bit.GPIO20 = 1;    // Disable pull-up on GPIO20 (EQEP1A)
    GpioCtrlRegs.GPAPUD.bit.GPIO21 = 1;    // Disable pull-up on GPIO21 (EQEP1B)
    GpioCtrlRegs.GPAQSEL2.bit.GPIO20 = 2;   // Qual every 6 samples
    GpioCtrlRegs.GPAQSEL2.bit.GPIO21 = 2;   // Qual every 6 samples
    EDIS;
    // This specifies which of the possible GPIO pins will be EQEP1 functional pins.
    // Comment out other unwanted lines.
    GPIO_SetupPinMux(20, GPIO_MUX_CPU1, 1);
    GPIO_SetupPinMux(21, GPIO_MUX_CPU1, 1);
    EQep1Regs.QEPCTL.bit.QPEN = 0;    // make sure eqep in reset
    EQep1Regs.QDECCTL.bit.QSRC = 0;   // Quadrature count mode
    EQep1Regs.QPOSCTL.all = 0x0;      // Disable eQep Position Compare
    EQep1Regs.QCAPCTL.all = 0x0;      // Disable eQep Capture
    EQep1Regs.QEINT.all = 0x0;        // Disable all eQep interrupts
    EQep1Regs.QPOSMAX = 0xFFFFFFFF;   // use full range of the 32 bit count
    EQep1Regs.QEPCTL.bit.FREE_SOFT = 2;  // EQep uneffected by emulation suspend in Code Composer
    EQep1Regs.QPOSCNT = 0;
    EQep1Regs.QEPCTL.bit.QPEN = 1;    // Enable EQep


    // setup QEP2 pins for input
    EALLOW;
    //Disable internal pull-up for the selected output pinsfor reduced power consumption
    GpioCtrlRegs.GPBPUD.bit.GPIO54 = 1;    // Disable pull-up on GPIO54 (EQEP2A)
    GpioCtrlRegs.GPBPUD.bit.GPIO55 = 1;    // Disable pull-up on GPIO55 (EQEP2B)
    GpioCtrlRegs.GPBQSEL2.bit.GPIO54 = 2;   // Qual every 6 samples
    GpioCtrlRegs.GPBQSEL2.bit.GPIO55 = 2;   // Qual every 6 samples
    EDIS;
    GPIO_SetupPinMux(54, GPIO_MUX_CPU1, 5); // set GPIO54 and eQep2A
    GPIO_SetupPinMux(55, GPIO_MUX_CPU1, 5); // set GPIO54 and eQep2B
    EQep2Regs.QEPCTL.bit.QPEN = 0;   // make sure qep reset
    EQep2Regs.QDECCTL.bit.QSRC = 0;  // Quadrature count mode
    EQep2Regs.QPOSCTL.all = 0x0;     // Disable eQep Position Compare
    EQep2Regs.QCAPCTL.all = 0x0;     // Disable eQep Capture
    EQep2Regs.QEINT.all = 0x0;       // Disable all eQep interrupts
    EQep2Regs.QPOSMAX = 0xFFFFFFFF;  // use full range of the 32 bit count.
    EQep2Regs.QEPCTL.bit.FREE_SOFT = 2;  // EQep uneffected by emulation suspend
    EQep2Regs.QPOSCNT = 0;
    EQep2Regs.QEPCTL.bit.QPEN = 1;   // Enable EQep
}

float readEncLeft(void) {
    int32_t raw = 0;
    uint32_t QEP_maxvalue = 0xFFFFFFFFU;  //4294967295U

    raw = EQep1Regs.QPOSCNT;
    if (raw >= QEP_maxvalue/2) raw -= QEP_maxvalue;  // I don't think this is needed and never true

    // 5 North South magnet poles in the encoder disk so 5 square waves per one revolution of the
    // DC motor's back shaft.  Then Quadrature Decoder mode multiplies this by 4 so 20 counts per one rev // of the DC motor's back shaft.  Then the gear motor's gear ratio is 30:1.
    return (raw*(PI/10.0)/30.0);
}

float readEncRight(void) {

    int32_t raw = 0;
    uint32_t QEP_maxvalue = 0xFFFFFFFFU;  //4294967295U  -1 32bit signed int

    raw = EQep2Regs.QPOSCNT;
    if (raw >= QEP_maxvalue/2) raw -= QEP_maxvalue;  // I don't think this is needed and never true

    // 5 North South magnet poles in the encoder disk so 5 square waves per one revolution of the
    // DC motor's back shaft.  Then Quadrature Decoder mode multiplies this by 4 so 20 counts per one rev // of the DC motor's back shaft.  Then the gear motor's gear ratio is 30:1.
    return (raw*(PI/10.0)/30.0);
}

void setEPWM2A(float controleffort){
    if(controleffort>10){
        controleffort=10;
    }
    if(controleffort<-10){
        controleffort=-10;
    }
    EPwm2Regs.CMPA.bit.CMPA=(float)EPwm2Regs.TBPRD*((float)controleffort+10.0)/(float)20;
...

This file has been truncated, please download it to see its full contents.