Dancing Segway Robot

//#############################################################################
// FILE:   FinalProject_main.c
//
// TITLE:  Balancing Segbot
//#############################################################################

// 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"

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

#define n_samples   1000 // with a sampling frequency of 10kHz, this runs Goertzl 10x per second with 1000 ADC samples
#define sampling_rate 10000 // 10 kHz
#define NOTE    261.6 // Hz C4
#define NOTE2   466 // Hz Bb4
#define NOTE3   294 // Hz D4

//NOTES FOR SONG
#define C4      262
#define G4      392
#define A3      220
#define G3      196
#define BF3     233 // FLAT
#define F4      349
#define E4      330
#define D4      294
#define F3      174.6
#define E3      165
#define REST    0
#define BF4     466 //FLAT
#define AF4     415 //FLAT
#define F4      349
#define EF4     311 //FLAT
#define DF4     277 //FLAT
#define FS4     370 //SHARP
#define A4      440
#define B4      494
#define C5      523
#define D5      587
#define B3      247

#define songsize    124
#define song2size   28
#define song3size   120

// 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 serialRXA(serial_t *s, char data);
void setupSpib(void); //lab4 initialize SPIB setup
float readEncLeft(void);//lab5 initialize functions
float readEncRight(void);
void init_eQEPs(void);
void setEPWM6A(float controleffort);
void setEPWM6B(float controleffort);
float goertzel_mag(int numSamples,int TARGET_FREQUENCY,int SAMPLING_RATE, float* data);

//Song
float song[songsize] = {C4,C4,C4,C4, A3,A3,A3,A3, A3,A3,A3,A3, REST,REST,A3,A3, C4,C4,D4,D4, G3,G3,G3,G3,
                        G3,G3,G3,G3, REST,REST,REST,REST, G3,G3,A3,A3, BF3,BF3,BF3,BF3, F4,F4,F4,F4, F4,F4,F4,F4,
                        E4,E4,C4,C4, D4,D4,C4,BF3, A3,A3,A3,A3, A3,A3,A3,A3, REST,REST,C4,C4, D4,D4,D4,D4,
                        REST,REST,D4,D4, G4,F4,F4,E4, E4,F4,D4,D4, C4,C4,C4,C4, REST,REST,REST,REST, F3,F3,G3,G3,
                        A3,A3,D4,D4, C4,C4,C4,C4, REST,REST,C4,C4, BF3,BF3,A3,A3, E3,E3,E3,E3, F3,F3,F3,F3, F3,F3,F3,F3};
//Dance
float turndance[songsize] = {0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,6,6, -6,-6,6,6, -4,-4,-4,-4,
                             -4,-4,-4,-4, -4,-4,-4,-4, 6,6,-6,-6, 0,0,0,0, 0,0,0,0, 0,0,6,6,
                             -6,-6,6,6, -10,-10,-10,-10, -10,-10,-10,-10, -10,-10,-10,-10, -10,-10,6,6, -6,-6,6,6,
                             -6,-6,10,10, 10,10,10,10, 10,10,10,10, 10,10,10,10, 10,10,10,10, 0,0,0,0,
                              0,0,-10,-10, -10,-10,-10,-10, -10,-10,6,6, -6,-6,6,6, -6,-6,6,6, -10,-10,-10,-10, -10,-10,-10,-10};

float fwddance[songsize] = {1.5,1.5,1.5,1.5, -0.75,-0.75,-0.75,-0.75, -0.75,-0.75,-0.75,-0.75, -0.75,-0.75,0,0, 0,0,0,0, 0,0,0,0,
                            0,0,0,0, 0,0,0,0, 0,0,0,0, 1,1,1,1, -0.75,-0.75,-0.75,-0.75, -0.75,-0.75,0,0,
                            0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0,
                            0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, .75,.75,-1,-1,
                           .75,.75,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0};
//Song2
float song2[song2size] = {REST,BF4,BF4,BF4, AF4,AF4,BF4,AF4, AF4,AF4,AF4,AF4, F4,F4,AF4,AF4, F4,F4,F4,F4, EF4,EF4,F4,EF4, EF4,DF4,BF3,BF3};
//Dance2
float turndance2[song2size] = {0,0,0,0, 10,10,10,10, 10,10,10,10, -6,-6,6,6, -6,-6,-6,6, -6,6,-6,6, -6,6,-6,6};

float fwddance2[song2size] = {1,1,1,1, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0};

//Song3
float song3[song3size] ={D4,D4,C4, D4,D4,C4,C4, C4,C4,A3,E4, D4,D4,C4,A3, A3,E4,E4,D4, E4,D4,D4,D4,
                         D4,D4,B3,G4, E4,D4,D4,B3, B3,D4,D4,B3, D4,D4,C4,C4, C4,C4,A3,E4, D4,C4,C4,A3,
                         A3,E4,E4,D4, E4,D4,D4,B3, D4,D4,B3,G4, E4,E4,D4,B3, B3,D4,D4,C4, D4,C4,C4,C4,
                         C4,C4,G4,G4, A4,A4,G4,G4, G4,E4,E4,D4, E4,D4,D4,D4, D4,D4,G4,G4, A4,A4,G4,G4,
                         A4,A4,B4,B4, C5,C5,C5,C5, C5,C5,C5,C5, B4,B4,D5,D5, B4,A4,A4,A4, A4,A4,G4,G4, G4,G4};

//Dance3
float turndance3[song3size] = {4,4,4 -4,-4,-4,-4, 4,4,4,4, -4,-4,-4,-4, -4,0,0,0, 0,0,0,0,
                               0,0,0,0, 0,0,0,0, 7,7,7,7, -7,-7,-7,-7, 7,7,7,7, -7,-7,-7,-7,
                               -7,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,6,-6, 6,-6,6,-6,
                               6,-10,-10,-10, -10,-10,-10,-10, -10,6,-6,6, -6,6,-6,6, -6,6,-10,-10, -10,-10,-10,-10,
                               20,20,-20,-20, 6,6,6,6, 6,6,6,6, 0,0,0,0, 0,0,0,0, -6,-6,-6,-6,-6,-6};

float fwddance3[song3size] = {0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,.75,.75,.75, -.75,-.75,-.75,-.75,
                              .75,.75,.75,.75, -.75,-.75,-.75,-.75, -.75,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0,
                              0,.75,.75,.75, -.75,-.75,-.75,-.75, .75,.75,.75,.75, -.75,-.75,-.75,-.75, 0,0,0,0, 0,0,0,0,
                              0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0,
                              0,0,0,0, 0,0,0,0, 0,0,0,0, -1,-1,-1,-1, 1,1,1,1, 1,-1,-1,-1, 0,0};

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

//lab3 ADCA
int16_t adca0result = 0;
int16_t adca1result = 0;
float adca0Volts = 0;
float adca1Volts = 0;
float adca0Filtered = 0;
float adca1Filtered = 0;
int32_t adcacount = 0;

//lab3 variables for interrupt function
int16_t adcb0result = 0;
int32_t adcbcount = 0;
float adcb_arrayPing[n_samples]= {0};
float adcb_arrayPong[n_samples]= {0};
float adcb_arrayDong[n_samples]= {0};
int16_t PingPong = 0;
float goer_result = 0;
float goer_result2 = 0;
float goer_result3 = 0;
int16_t pwm12prd = 50000000/sampling_rate;

int16_t thresh = 300; //tune this value to detect target frequency
int16_t note_detected = 0;
int16_t note2_detected = 0;
int16_t note3_detected = 0;
int16_t RunPing = 0;
int16_t RunPong = 0;
int16_t RunDong = 0;

//lab3
//This function sets DACA to the voltage between 0V and 3V passed to this function.
//If outside 0V to 3V the output is saturated at 0V to 3V
//Example code
//float myu = 2.25;
//setDACA(myu);   // DACA will now output 2.25 Volts
void setDACA(float dacouta0) {
        if (dacouta0 >  3.0) dacouta0 =  3.0;
        if (dacouta0 < 0.0) dacouta0 = 0.0;

        DacaRegs.DACVALS.bit.DACVALS = (int)(dacouta0*4095/3);  // perform scaling of 0-3 to 0-4095
}

void setDACB(float dacouta1) {
        if (dacouta1 >  3.0) dacouta1 =  3.0;
        if (dacouta1 < 0.0) dacouta1 = 0.0;

        DacbRegs.DACVALS.bit.DACVALS = (int)(dacouta1*4095/3);  // perform scaling of 0-3 to 0-4095
}

//lab4
int16_t updown = 0;
int16_t Gvalue = 0;
float accelx = 0;
float accely = 0;
float accelz = 0;
float gyrox = 0;
float gyroy = 0;
float gyroz = 0;
int16_t accelxraw = 0;
int16_t accelyraw = 0;
int16_t accelzraw = 0;
int16_t gyroxraw = 0;
int16_t gyroyraw = 0;
int16_t gyrozraw = 0;

//lab5
float LeftWheel = 0;
float RightWheel = 0;
float RadFt = 9.45;
float xRight = 0;
float xLeft = 0;
float uLeft = 0;
float uRight = 0;
float xLeft_1 = 0;
float xRight_1 = 0;
float vLeft = 0;
float vRight = 0;
float vRef = .5; //target velocity ft/s
float eR = 0; //error
float eL = 0;
float eR_1 = 0;
float eL_1 = 0;
float I_R = 0;
float I_L = 0;
float I_L1 = 0;
float I_R1 = 0;
float eturn = 0;
float turn = 0;
int16_t SPIcnt = 0;

//lab6
// Needed global Variables
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 = -.825; //needs to be fine tuned originally -.76
float accelzBalancePoint = -.8;
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};
float WheelDif = 0;
float WheelDif_1 = 0;
float velWheelDif = 0;
float velWheelDif_1 = 0;
float turnref = 0;
float eDif = 0;
float eDif_1 = 0;
float IntDif = 0;
float IntDif_1;
float FwdBkOffset = 0;
float Kp = 3.0; //proportional gain
float Ki = 20.0; //integral gain
float Kd = .08; //derivative gain
float turnrate = 0;
float turnrate_1 = 0;
float turnref_1 = 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;
int32_t timecount = 0;
int16_t calibration_state = 0;
int32_t calibration_count = 0;

float vLeft_1 = 0;
float vRight_1 = 0;
float LeftWheel_1 = 0;
float RightWheel_1 = 0;
//simulation gains
float K1 = -25;
float K2 = -2.8;
float K3 = -1.0;
float uBal = 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(52, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(52, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPBCLEAR.bit.GPIO52 = 1;

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

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

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

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

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

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

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

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

    // LED11
    GPIO_SetupPinMux(5, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(5, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPACLEAR.bit.GPIO5 = 1;

    // LED12
    GPIO_SetupPinMux(6, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(6, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPACLEAR.bit.GPIO6 = 1;

    // LED13
    GPIO_SetupPinMux(7, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(7, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPACLEAR.bit.GPIO7 = 1;

    // LED14
    GPIO_SetupPinMux(8, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(8, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPACLEAR.bit.GPIO8 = 1;

    // LED15
    GPIO_SetupPinMux(9, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(9, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPACLEAR.bit.GPIO9 = 1;

    // LED16
    GPIO_SetupPinMux(24, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(24, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPACLEAR.bit.GPIO24 = 1;

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

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

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

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

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

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

    // LED23
    GPIO_SetupPinMux(158, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(158, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPECLEAR.bit.GPIO158 = 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;

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

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

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

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

    //Right Motor
    GPIO_SetupPinMux(10, GPIO_MUX_CPU1, 1); // lab2 mux
    //Left Motor
    GPIO_SetupPinMux(11, GPIO_MUX_CPU1, 1); // lab2 mux

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

    EALLOW;
    // Motors
    EPwm6Regs.TBCTL.bit.CTRMODE = 0; // set to up mode
    EPwm6Regs.TBCTL.bit.FREE_SOFT = 2; // free run
    EPwm6Regs.TBCTL.bit.PHSEN = 0; // disable phase loading
    EPwm6Regs.TBCTL.bit.CLKDIV = 0; // divide by 1
    EPwm6Regs.TBCTR = 0; // start timers at 0
    EPwm6Regs.TBPRD = 2500; // set period to 50 us (20 kHz)
    EPwm6Regs.CMPA.bit.CMPA = 0; // start duty cycle at 0
    EPwm6Regs.CMPB.bit.CMPB = 0; // start duty cycle at 0
    EPwm6Regs.AQCTLA.bit.CAU = 1; // clear signal when CMPA is reached
    EPwm6Regs.AQCTLB.bit.CBU = 1; // clear signal when CMPB is reached
    EPwm6Regs.AQCTLA.bit.ZRO = 2; // set pin when TBCTR is zero
    EPwm6Regs.AQCTLB.bit.ZRO = 2; // set pin when TBCTR is zero
    EPwm6Regs.TBPHS.bit.TBPHS = 0;  // set phase to zero

    //lab3 ePWM5 timer with no duty cycle output for ADCA
    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 1 ms sample. fs = 1000 Hz  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

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

    //ePWM9 used for song
    //GPIO_SetupPinMux(16, GPIO_MUX_CPU1, 5);//set up buzzer
    GPIO_SetupPinMux(16, GPIO_MUX_CPU1, 0); //set GPIO16 back to GPIO (not PWM)
    // set up ePWM9A, 50 MHz clock
    EPwm9Regs.TBCTL.bit.CTRMODE = 0; // set to up mode
    EPwm9Regs.TBCTL.bit.FREE_SOFT = 2; // free run
    EPwm9Regs.TBCTL.bit.PHSEN = 0; // disable phase loading
    EPwm9Regs.TBCTL.bit.CLKDIV = 4; // divide by 16 - effective clock 3125000
    EPwm9Regs.TBCTR = 0; // start timers at 0, counts up until equal to TBPRD
    EPwm9Regs.TBPRD = 65535; //startval
    //not using CMPA
    EPwm9Regs.AQCTLA.bit.PRD = 3; //toggle output pin when TBCTR = TBPRD
    EPwm9Regs.AQCTLA.bit.CAU = 0;
    EPwm9Regs.TBPHS.bit.TBPHS = 0;  // set phase to zero

    // write the initialization code for the two EPWM channels that are by
    // default wired though jumpers to the RC servo 3 pin connector

        GPIO_SetupPinMux(14, GPIO_MUX_CPU1, 1);
        GPIO_SetupPinMux(15, GPIO_MUX_CPU1, 1);

        EPwm8Regs.TBCTL.bit.CTRMODE = 0; // set to up mode
        EPwm8Regs.TBCTL.bit.FREE_SOFT = 2; // free run
        EPwm8Regs.TBCTL.bit.PHSEN = 0; // disable phase loading
        EPwm8Regs.TBCTL.bit.CLKDIV = 100; // divide by 16
        EPwm8Regs.TBCTR = 0; // start timers at 0
        //Problem - TBPRD is 16-bits and can only have a value up to 65,535 --> change CLKDIV
        EPwm8Regs.TBPRD = 62500; // 50Hz carrier frequency = 20 ms period
        EPwm8Regs.AQCTLA.bit.CAU = 1; // clear signal when CMPA is reached
        EPwm8Regs.AQCTLB.bit.CBU = 1; // clear signal when CMPB is reached
        EPwm8Regs.AQCTLA.bit.ZRO = 2; // set pin when TBCTR is zero
        EPwm8Regs.AQCTLB.bit.ZRO = 2; // set pin when TBCTR is zero
        EPwm8Regs.TBPHS.bit.TBPHS = 0;  // set phase to zero

    // start CMPA’s value such that it is commanding the RC Servo to
    // 0 degrees (8% duty cycle)

        EPwm8Regs.CMPA.bit.CMPA = .08*EPwm8Regs.TBPRD; // start at 0 deg
        EPwm8Regs.CMPB.bit.CMPB = .08*EPwm8Regs.TBPRD; // start at 0 deg

    EDIS;

    //lab3 ADCA and ADCB
    EALLOW;
    AdcaRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
    AdcbRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
    AdcSetMode(ADC_ADCA, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE);  //read calibration settings
    AdcSetMode(ADC_ADCB, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE);  //read calibration settings
    //Set pulse positions to late
    AdcaRegs.ADCCTL1.bit.INTPULSEPOS = 1;
    AdcbRegs.ADCCTL1.bit.INTPULSEPOS = 1;
    //power up the ADCs
    AdcaRegs.ADCCTL1.bit.ADCPWDNZ = 1;
    AdcbRegs.ADCCTL1.bit.ADCPWDNZ = 1;
    //delay for 1ms to allow ADC time to power up
    DELAY_US(1000);
    //ADCA
    AdcaRegs.ADCSOC0CTL.bit.CHSEL = 2;  //SOC0 will convert Channel A2
    AdcaRegs.ADCSOC0CTL.bit.ACQPS = 14; //sample window is acqps + 1 SYSCLK cycles = 75ns
    AdcaRegs.ADCSOC0CTL.bit.TRIGSEL = 0x0D;// EPWM5 ADCSOCA will trigger SOC0
    AdcaRegs.ADCSOC1CTL.bit.CHSEL = 3;  //SOC1 will convert Channel A3
    AdcaRegs.ADCSOC1CTL.bit.ACQPS = 14; //sample window is acqps + 1 SYSCLK cycles = 75ns
    AdcaRegs.ADCSOC1CTL.bit.TRIGSEL = 0x0D;// EPWM5 ADCSOCA 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
    //ADCB
    AdcbRegs.ADCSOC0CTL.bit.CHSEL = 4;  //SOC0 will convert Channel B4
    AdcbRegs.ADCSOC0CTL.bit.ACQPS = 14; //sample window is acqps + 1 SYSCLK cycles = 75ns
    AdcbRegs.ADCSOC0CTL.bit.TRIGSEL = 0x1B; // EPWM12 ADCSOCA will trigger SOC0
    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
    EDIS;


    //lab5 initialize encoders
    init_eQEPs();

    //lab4 run SPI setup
    setupSpib();

    // 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.SPIB_RX_INT = &SPIB_isr;
    PieVectTable.ADCA1_INT = &ADCA_ISR;
    PieVectTable.ADCB1_INT = &ADCB_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, 150001); //tempo2
    ConfigCpuTimer(&CpuTimer1, 200, 197628); //tempo
    ConfigCpuTimer(&CpuTimer2, 200, 230769); //tempo3

    // 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);

    // 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; //lab4 enable interrupt 6

//   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;
    //group 6 interrupt 3
    PieCtrlRegs.PIEIER6.bit.INTx3 = 1; //lab4 SPI
    // lab3 Enable PIE: Group 1 interrupt 1, ADCA1
    PieCtrlRegs.PIEIER1.bit.INTx1 = 1;
    // Enable PIE: Group 1 interrupt 2, ADCB1
    PieCtrlRegs.PIEIER1.bit.INTx2 = 1;

    AdcbRegs.ADCINTFLGCLR.bit.ADCINT1 = 1;  //clear interrupt flag
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP1; //clear PIE peripheral so processor waits until next interrupt flag

    // 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:
    while(1)
    {
        if (UARTPrint == 1 ) {
            //serial_printf(&SerialA,"Tilt: %f Gyro: %f Left: %f Right: %f\r\n", tilt_value, gyro_value, LeftWheel, RightWheel);
            UARTPrint = 0;
        }
        //after n samples, pass data through Goertzel fxn
        // use Ping Pong buffer

        if(RunPing == 1){
            goer_result = goertzel_mag(n_samples,NOTE,sampling_rate, adcb_arrayPing);
            RunPing = 0;
            if(goer_result > thresh){
                note_detected = 1;
            }
        }
        if(RunPong == 1){
            goer_result2 = goertzel_mag(n_samples,NOTE2,sampling_rate, adcb_arrayPong);
            RunPong = 0;
            if(goer_result2 > thresh){
                note2_detected = 1;
            }
        }
        if(RunDong == 1){
            goer_result3 = goertzel_mag(n_samples,NOTE3,sampling_rate, adcb_arrayDong);
            RunDong = 0;
            if(goer_result3 > thresh){
                note3_detected = 1;
            }
        }
    }
}


// SWI_isr,  Using this interrupt as a Software started interrupt
__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

    //lab6 control with turnrate
    turnref = turnref_1 + (turnrate + turnrate_1)*.004/2.0;
    //lab6 steering
    WheelDif = LeftWheel - RightWheel; //difference between wheel positions, radians
    velWheelDif = .33*velWheelDif_1 + .67*(WheelDif-WheelDif_1)/.004; //raw velocity higher weight than old velocity, less lag
    eDif = turnref - WheelDif;
    IntDif = IntDif_1 + (eDif + eDif_1)*.004/2.0;
    turn = Kp*eDif + Ki*IntDif - Kd*velWheelDif;
    //integral windup guard
    if(fabs(turn)>3){
        IntDif = IntDif_1;
    }
    //saturate turning so the segbot balance dominates
    if(turn>4){
        turn=4;
    }
    if(turn<-4){
        turn=-4;
    }
    //lab6 Balance
    vLeft = .6*vLeft_1 + .4*(LeftWheel - LeftWheel_1)/.004; //rad/s
    vRight = .6*vRight_1 + .4*(RightWheel - RightWheel_1)/.004; //rad/s

    uBal = -K1*tilt_value -K2*gyro_value -K3*(vLeft+vRight)/2;

    //lab6 control
    uLeft = uBal/2 + turn + FwdBkOffset;
    uRight = uBal/2 -turn + FwdBkOffset;

    //pass controleffort through PWM
    setEPWM6A(uLeft);
    setEPWM6B(-uRight);

    //update variables
    vLeft_1 = vLeft;
    vRight_1 = vRight;
    LeftWheel_1 = LeftWheel;
    RightWheel_1 = RightWheel;
    WheelDif_1 = WheelDif;
    velWheelDif_1 = velWheelDif;
    eDif_1 = eDif;
    IntDif_1 = IntDif;
    turnrate_1 = turnrate;
    turnref_1 = turnref;

    numSWIcalls++;

    DINT;

}

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

    if(note2_detected == 1){
        // Blink a number of LEDS if the Goertzel value exceeds the threshold
        GpioDataRegs.GPBTOGGLE.bit.GPIO52 = 1;
        GpioDataRegs.GPCTOGGLE.bit.GPIO94 = 1;
        GpioDataRegs.GPDTOGGLE.bit.GPIO97 = 1;

        GPIO_SetupPinMux(16, GPIO_MUX_CPU1, 5);//set up buzzer

        if (numTimer0calls < song2size){
            //play the song
            if(song2[numTimer0calls]==0){
                GpioDataRegs.GPACLEAR.bit.GPIO16 = 1; //set GPIO16 low to ground the speaker
            }else{
                EPwm9Regs.TBPRD = (int)(3125000/song2[numTimer0calls]/2);
            }
            //dance
            FwdBkOffset = fwddance2[numTimer0calls];
            turnrate = turndance2[numTimer0calls];
        }   else{
            GPIO_SetupPinMux(16, GPIO_MUX_CPU1, 0); //set GPIO16 back to GPIO (not PWM)
            GpioDataRegs.GPACLEAR.bit.GPIO16 = 1; //set GPIO16 low to ground the speaker
            note2_detected = 0;
            numTimer0calls = 0;
            FwdBkOffset = 0;
            turnrate = 0;
        }

        numTimer0calls++; // only increment after note is detected
    }


    // 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)
{
    if(note_detected == 1){
        // Blink a number of LEDS if the Goertzel value exceeds the threshold
        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;

        GPIO_SetupPinMux(16, GPIO_MUX_CPU1, 5);//set up buzzer

        if (numtimer1calls < songsize){
            //play the song
            if(song[numtimer1calls]==0){
                GpioDataRegs.GPACLEAR.bit.GPIO16 = 1; //set GPIO16 low to ground the speaker
            }else{
                EPwm9Regs.TBPRD = (int)(3125000/song[numtimer1calls]/2);
            }
            //dance
            FwdBkOffset = fwddance[numtimer1calls];
            turnrate = turndance[numtimer1calls];
        }   else{
            GPIO_SetupPinMux(16, GPIO_MUX_CPU1, 0); //set GPIO16 back to GPIO (not PWM)
            GpioDataRegs.GPACLEAR.bit.GPIO16 = 1; //set GPIO16 low to ground the speaker
            note_detected = 0;
            numtimer1calls = 0;
            FwdBkOffset = 0;
            turnrate = 0;
        }

        numtimer1calls++; // only increment after note is detected
    }

    CpuTimer1.InterruptCount++;
}

// cpu_timer2_isr CPU Timer2 ISR
__interrupt void cpu_timer2_isr(void)
{
    if(note3_detected == 1){
        // Blink a number of LEDS if the Goertzel value exceeds the threshold
        GpioDataRegs.GPATOGGLE.bit.GPIO22 = 1;
        GpioDataRegs.GPBTOGGLE.bit.GPIO52 = 1;
        GpioDataRegs.GPCTOGGLE.bit.GPIO67 = 1;

        GPIO_SetupPinMux(16, GPIO_MUX_CPU1, 5);//set up buzzer

        if (numTimer2calls < song3size){
            //play the song
            if(song3[numTimer2calls]==0){
                GpioDataRegs.GPACLEAR.bit.GPIO16 = 1; //set GPIO16 low to ground the speaker
            }else{
                EPwm9Regs.TBPRD = (int)(3125000/song3[numTimer2calls]/2);
            }
            //dance
            FwdBkOffset = fwddance3[numTimer2calls];
            turnrate = turndance3[numTimer2calls];
            //bell
            if((numTimer2calls >= 19 && numTimer2calls <= 26)||(numTimer2calls >= 51 && numTimer2calls <= 58)||(numTimer2calls >= 99 && numTimer2calls <= 106)){
                if(numTimer2calls%2 ==0){
                    EPwm8Regs.CMPA.bit.CMPA = .08*EPwm8Regs.TBPRD; //set RC to 0 degrees
                    EPwm8Regs.CMPB.bit.CMPB = .08*EPwm8Regs.TBPRD; //set RC to 0 degrees
                }else{
                    EPwm8Regs.CMPA.bit.CMPA = .03*EPwm8Regs.TBPRD; //set RC to -90 degrees
                    EPwm8Regs.CMPB.bit.CMPB = .03*EPwm8Regs.TBPRD; //set RC to -90 degrees
                }
            }
        }   else{
            GPIO_SetupPinMux(16, GPIO_MUX_CPU1, 0); //set GPIO16 back to GPIO (not PWM)
            GpioDataRegs.GPACLEAR.bit.GPIO16 = 1; //set GPIO16 low to ground the speaker
            note3_detected = 0;
            numTimer2calls = 0;
            FwdBkOffset = 0;
            turnrate = 0;
        }

        numTimer2calls++; // only increment after note is detected
    }
    CpuTimer2.InterruptCount++;
}

// This function is called each time a char is recieved over UARTA.
void serialRXA(serial_t *s, char data) {
    numRXA ++;
    if (data == 'q') {
        turnrate = turnrate - 0.2;
    } else if (data == 'r') {
        turnrate = turnrate + 0.2;
    } else if (data == '3') {
        FwdBkOffset = FwdBkOffset - 0.2;
    } else if (data == 's') {
        FwdBkOffset = FwdBkOffset + 0.2;
    } else {
        turnrate = 0;
        FwdBkOffset = 0;
    }
}
//ADCA : transmit SPI values
__interrupt void ADCA_ISR (void)
{
    //SPI Read Gyros and Accelerometers

    GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1; // Slave Select Low
    SpibRegs.SPIFFRX.bit.RXFFIL = 8; // Issue the SPIB_RX_INT when two values are in the RX FIFO

    //write to TXBUF
    //set MSB to 1 for READ
    SpibRegs.SPITXBUF = 0x8000|0x3A00; //start address INT_STATUS, write 0x00 to INT_STATUS
    SpibRegs.SPITXBUF = 0x0000; //write 0x00 to read ACCEL_XOUT
    SpibRegs.SPITXBUF = 0x0000; //write 0x00 to read ACCEL_YOUT
    SpibRegs.SPITXBUF = 0x0000; //write 0x00 to read ACCEL_ZOUT
    SpibRegs.SPITXBUF = 0x0000; //write 0x00 to read TEMP_OUT
    SpibRegs.SPITXBUF = 0x0000; //write 0x00 to read GYRO_XOUT
    SpibRegs.SPITXBUF = 0x0000; //write 0x00 to read GYRO_YOUT
    SpibRegs.SPITXBUF = 0x0000; //write 0x00 to read GYRO_ZOUT

    //read from RXBUF in the SPIB interrupt function
    //GPIO set back to high in interrupt to end chip select

    AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1;  //clear interrupt flag
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP1; //clear PIE peripheral so processor waits until next interrupt flag
}
//adcb1 interrupt
__interrupt void ADCB_ISR (void)
{
    adcb0result = AdcbResultRegs.ADCRESULT0; //read value between 0 and 4095 from ADC
    //MAX9814 sensitivity = -44 dBV meaning the signal corresponds to 6.3096mV/Pa of sound pressure
    //ADC values 0 to 4095 correspond to voltages 0-3V, mic produces 0.5 to 2.5 V (values ~682 to ~3412)

    //Ping
    if(PingPong == 0){
        adcb_arrayPing[adcbcount] = adcb0result; // add ADC reading to array
        if(adcbcount == (n_samples-1)){
            adcbcount = -1; //incremented to 0 at end of interrupt
            RunPing = 1; //check in while loop
            PingPong = 1; //switch to Pong buffer
        }
    }
    //Pong
    if(PingPong == 1){
        adcb_arrayPong[adcbcount] = adcb0result;
        if(adcbcount == (n_samples-1)){
            adcbcount = -1; //incremented to 0 at end of interrupt
            RunPong = 1;
            PingPong = 2; //switch to Dong buffer
        }
    }
    //Dong
    if(PingPong == 2){
        adcb_arrayDong[adcbcount] = adcb0result;
        if(adcbcount == (n_samples-1)){
            adcbcount = -1; //incremented to 0 at end of interrupt
            RunDong = 1;
            PingPong = 0; //switch to Ping buffer
        }
    }

    adcbcount++; //increment count
    AdcbRegs.ADCINTFLGCLR.bit.ADCINT1 = 1;  //clear interrupt flag
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP1; //clear PIE peripheral so processor waits until next interrupt flag
}

// DFT with Goertzel Algorithm
float goertzel_mag(int numSamples,int TARGET_FREQUENCY,int SAMPLING_RATE, float* data)
{
    int     k,i;
    float   floatnumSamples;
    float   omega,sine,cosine,coeff,q0,q1,q2,magnitude,real,imag;

    float   scalingFactor = numSamples / 2.0;

    floatnumSamples = (float) numSamples;
    k = (int) (0.5 + ((floatnumSamples * TARGET_FREQUENCY) / SAMPLING_RATE));
    omega = (2.0 * PI * k) / floatnumSamples;
    sine = sin(omega);
    cosine = cos(omega);
    coeff = 2.0 * cosine;
    q0=0;
    q1=0;
    q2=0;

    for(i=0; i<numSamples; i++)
    {
        q0 = coeff * q1 - q2 + data[i];
        q2 = q1;
        q1 = q0;
    }

    // calculate the real and imaginary results
    // scaling appropriately
    real = (q1 - q2 * cosine) / scalingFactor;
    imag = (q2 * sine) / scalingFactor;

    magnitude = sqrtf(real*real + imag*imag);
    return magnitude;
}

//SPIB:Read IMU (acceleromoeter) and encoder (motor) values
__interrupt void SPIB_isr(void){
    //read from RX FIFO
    Gvalue = SpibRegs.SPIRXBUF; //garbage value read
    accelxraw = SpibRegs.SPIRXBUF;
...

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