RecorderBot (UIUC ME 461)

 //#############################################################################
// FILE:   labstarter_main.c
//
// TITLE:  Lab Starter
//#############################################################################

// 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"
// For IPC
#include "F2837xD_Ipc_drivers.h"

#define PI          3.1415926535897932384626433832795
#define TWOPI       6.283185307179586476925286766559
#define HALFPI      1.5707963267948966192313216916398
#define FEETINONEMETER 3.28083989501312
#define MIN(A,B)        (((A) < (B)) ? (A) : (B));

//*****************************************************************************
// 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 ADCB_isr(void); //Define interrupt for ADCB
__interrupt void SPIB_isr(void); //Predefines the SPI interrupt
__interrupt void CPU2toCPU1IPC0(void);


float *cpu2tocpu1; //float pointer for the location CPU2 will give data to CPU1
float *cpu1tocpu2; //float pointer for the location CPU1 will give data to CPU2
char *commandtoCPU2;


void serialRXA(serial_t *s, char data);
void setEPWM2A(float controleffort); //Function definition to pass values EPWM2A (right motor)
void setEPWM2B(float controleffort); //Function definition to pass values EPWM2B (left motor)
float saturate(float input, float saturation_limit); //Function definition for saturate function
void init_eQEPs(void); //Function definition to initialize  eQEP
float readEncLeft(void); //Function definition to read left encoder
float readEncRight(void); //Function definition to read right encoder
void setupSpib(void); //Predefines the SPI setup function

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

uint32_t numCPU2COMs = 0;
float receivefloats[4] = {0,0,0,0};
float sendfloats[4] = {0,0,0,0};
uint32_t timecounter = 0;

uint32_t ADCB_ISR_calls = 0; //Counts number of times ADCB interrupt was called
int16_t pingPong = 1;
int16_t runPong = 0;
int16_t runPing = 0;
int16_t ping_index = 0;
int16_t pong_index = 0;
int16_t powerThreshold = 100;
int16_t G_Low_Threshold = 75;
int16_t G_High_Threshold = 83;
int16_t A_Low_Threshold = 86;
int16_t A_High_Threshold = 95;
int16_t F_Low_Threshold = 69;
int16_t F_High_Threshold = 77;
int16_t C_Low_Threshold = 103;
int16_t C_High_Threshold = 113;
int16_t Stop_Low_Threshold = 118;
int16_t Stop_High_Threshold = 126;


int16_t spivalue1 = 0;
int16_t spivalue2 = 0;
float theta_l = 0; //Stores left encoder value
float theta_r = 0; //Stores right encoder value
float rad_to_feet = 0.19916351; //=1/5.021 is our conversion factor determined for radians to feet based on wheel geometry
float right_dist = 0; //Used to store the distance traveled by the right wheel
float left_dist = 0; //Used to store the distance traveled by the left wheel
float uLeft = 5; //Passed to left motor speed
float uRight = 5; //Passed to right motor speed
float PosLeft_K = 0; //Stores current left position
float PosLeft_K_1 = 0; //Stores past left position
float PosRight_K = 0; //Store current right position
float PosRight_K_1 = 0; //Stores past right position
float vel_right = 0; //Stores right wheel linear velocity
float vel_left = 0; //Stores left wheel linear velocity
float v_ref = 0; //Stores velocity to be passed to robot
float ki = 25; //Integral control constant
float kp = 3; //Proportional control constant
float ek_r = 0; //Right wheel control error
float ek_1_r = 0; //Previous right wheel control error
float ik_r = 0; //Right signal passed through integral
float ik_1_r = 0; //Previous right signal passed through integral
float ek_l = 0;  //Left wheel control error
float ek_1_l = 0; //Previous left wheel control error
float ik_l = 0; //Left signal passed through integral
float ik_1_l = 0; //Previous left signal passed through integral
float theta_l_1 = 0; //Previous left angular position
float theta_r_1 = 0; //Previous right angular position
float x_r_1 = 0; //Previous robot x position
float y_r_1 = 0; //Previous robot y position
float KP_turn = 3; //Proportional turn constant
float e_turn = 0; //Turn error
float turn = 0; //Turn setpoint
float W_r = 0.57743; //Robot width
float R_wh = 0.19583; //Wheel radius
float phi_r = 0; //Robot pose angle
float x_r = 0; //Robot Pose X Coordinate
float y_r = 0; //Robot Pose Y Coordinate
float theta_avg = 0; //Average angular position
float theta_dot_avg = 0; //Average angular velocity
float theta_dot_l = 0; //Left angular velocity
float theta_dot_r = 0; //Right angular velocity
float y_dot_r = 0; //Y linear velocity
float x_dot_r = 0; //X linear velocity
int16_t updown = 1; //Used to decrease/increase PWM signal
int16_t PWM1 = 0; //Used to send values to PWM1 on DAN28027 chip
int16_t PWM2 = 0; //Used to send values to PWM2 on DAN28027 chip
int16_t SPIB_isr_calls = 0; //Counts number of times SPIB isr function is called
int16_t garbage = 0; //Used for useless SPI outputs
int16_t ADC1 = 0; //ADC1 SPI Output
int16_t ADC2 = 0; //ADC2 SPI Output
float ADC1_v = 0; //ADC1 SPI Output in volts
float ADC2_v = 0; //ADC2 SPI Output in volts
int16_t ACCEL_XOUT_RAW = 0; //Used to read the raw X acceleration from the SPI
int16_t ACCEL_YOUT_RAW = 0; //Used to read the raw Y acceleration from the SPI
int16_t ACCEL_ZOUT_RAW = 0; //Used to read the raw Z acceleration from the SPI
int16_t TEMP_OUT_RAW = 0; //Used to read the temperature from the SPI
int16_t GYRO_XOUT_RAW = 0; //Used to read the raw X gyro data from the SPI
int16_t GYRO_YOUT_RAW = 0; //Used to read the raw Y gyro data from the SPI
int16_t GYRO_ZOUT_RAW = 0; //Used to read the raw Z gyro data from the SPI

float ACCEL_XOUT = 0; //Used to store the converted X acceleration data in the range (-4,4) with units of g
float ACCEL_YOUT = 0; //Used to store the converted Y acceleration data in the range (-4,4) with units of g=(-4g,4g)
float ACCEL_ZOUT = 0; //Used to store the converted Z acceleration data in the range (-4,4) with units of g
float GYRO_XOUT = 0; //Used to store the converted X gyro data in the range (-250,250) with units of deg/sec
float GYRO_YOUT = 0; //Used to store the converted Y gyro data in the range  (-250,250) with units of deg/sec
float GYRO_ZOUT = 0; //Used to store the converted Z gyro data in the range  (-250,250) with units of deg/sec



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

    InitIpc();

    commandtoCPU2 = (char*) 0x3FFFC;  // in RAM that CPU1 can R/W but CPU2 can only read.
    //location of cpu2tocpu1 ram
    cpu2tocpu1 = (float*) 0x3F800;
    cpu1tocpu2 = (float*) 0x3FC00;

    commandtoCPU2[0] = 'W';  // W for CPU2 wait

    //     Comment this when use CCS for debugging
//            #ifdef _FLASH
//                // Send boot command to allow the CPU2 application to begin execution



        //         IPCBootCPU2(C1C2_BROM_BOOTMODE_BOOT_FROM_FLASH);






//            #else
//                // Send boot command to allow the CPU2 application to begin execution
//                IPCBootCPU2(C1C2_BROM_BOOTMODE_BOOT_FROM_RAM);
//            #endif
//  Or when you want to run CPU2 from its flash you free run CPU2 and just run IPCBootCPU2 from Flash command.  Actually I do not know when you need boot from RAM ???



    InitGpio();

    // Blue LED on LaunchPad
    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_CPU2, 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);

    EALLOW;
    GpioCtrlRegs.GPDPUD.bit.GPIO122 = 0; // Enable Pull-ups on SPI PINs Recommended by TI for SPI Pins
    GpioCtrlRegs.GPDPUD.bit.GPIO123 = 0;
    GpioCtrlRegs.GPDPUD.bit.GPIO124 = 0;
    GpioCtrlRegs.GPDQSEL2.bit.GPIO122 = 3; // Set prequalifier for SPI PINS
    GpioCtrlRegs.GPDQSEL2.bit.GPIO123 = 3; // The prequalifier eliminates short noise spikes
    GpioCtrlRegs.GPDQSEL2.bit.GPIO124 = 3; // by making sure the serial pin stays low for 3 clock periods.
    EDIS;

    // 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.IPC0_INT = &CPU2toCPU1IPC0;
    PieVectTable.ADCB1_INT = &ADCB_isr; //Enables ADCB interrupt for when it is needed

    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, 4000);
    ConfigCpuTimer(&CpuTimer1, 200, 40000);
    ConfigCpuTimer(&CpuTimer2, 200, 222000);

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

    init_serial(&SerialA,115200,serialRXA);
    init_eQEPs(); //Initialize the eQEP
    setupSpib(); //Setup the SPI

    EALLOW;
    //EPWM 2 Initializations used to pass PWM values to motors
    EPwm2Regs.TBCTL.bit.FREE_SOFT = 3;
    EPwm2Regs.TBCTL.bit.CLKDIV = 0;
    EPwm2Regs.TBCTL.bit.CTRMODE = 0;
    EPwm2Regs.TBCTL.bit.PHSEN = 0;
    EPwm2Regs.TBCTR = 0;
    EPwm2Regs.TBPRD = 2500; //50 MHz/20KHz=2500 50 MHz is frequency of clock source being counted, 20 KHz is desired
    EPwm2Regs.CMPA.bit.CMPA = 0; //Start duty cycle at 0
    EPwm2Regs.AQCTLA.bit.CAU = 1;
    EPwm2Regs.AQCTLA.bit.ZRO = 2;
    EPwm2Regs.TBPHS.bit.TBPHS = 0;
    EPwm2Regs.CMPB.bit.CMPB = 0; //The following three bits are defined again for EPWM2B
    EPwm2Regs.AQCTLB.bit.CBU = 1;
    EPwm2Regs.AQCTLB.bit.ZRO = 2;
    GPIO_SetupPinMux(2, GPIO_MUX_CPU1, 1); //GPIO PinName, CPU, Mux Index
    GPIO_SetupPinMux(3, GPIO_MUX_CPU1, 1); //GPIO PinName, CPU, Mux Index


    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 zero for exercises 1-3 and end of exercise 4
    //EPwm5Regs.TBCTL.bit.CLKDIV = 2; //Divide by 4 to sample at 4 kHz for start of exercise 4
    //EPwm5Regs.TBPRD = 50000;  // Use for all except end of exercise 4. Set Period to 1ms sample (or.25 ms for start of exercise 4.)  Input clock is 50MHz.
    EPwm5Regs.TBPRD = 5000; //Use for end of exercise 4. 5000 instead of 50000 results in 10 KHz frequency of sampling.
    // 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;

    EALLOW;
    //write configurations for all ADCs  ADCA, ADCB, ADCC, ADCD
    AdcbRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
    AdcaRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
    AdcSetMode(ADC_ADCB, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE);  //read calibration settings
    AdcSetMode(ADC_ADCA, 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 you choose Does not have to be A0
                                            //For us we had ADCA SOC0 convert channel 2
    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
                                            //Similarly SOC1 converted channel 3
    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
                                               //Last SOC was SOC1
    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 you choose Does not have to be B0
                                            //Only SOC0 is used, and it will convert channel 4
    AdcbRegs.ADCSOC0CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    AdcbRegs.ADCSOC0CTL.bit.TRIGSEL = 13; // EPWM5 ADCSOCA or another trigger you choose will trigger SOC0
    //AdcbRegs.ADCSOC1CTL.bit.CHSEL = 1;  //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 = 13; // EPWM5 ADCSOCA or another trigger you choose will trigger SOC1
    //AdcbRegs.ADCSOC2CTL.bit.CHSEL = 2;  //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 = 13; // EPWM5 ADCSOCA or another trigger you choose will trigger SOC2
    //AdcbRegs.ADCSOC3CTL.bit.CHSEL = 3;  //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 = 13; // 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
                                               //SOC0 was last SOC
    AdcbRegs.ADCINTSEL1N2.bit.INT1E = 1;   //enable INT1 flag
    AdcbRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //make sure INT1 flag is cleared
    EDIS;

    GPIO_SetupPinMux(9, GPIO_MUX_CPU1, 0);  // Set as GPIO9 and used as DAN28027 SS
    GPIO_SetupPinOptions(9, GPIO_OUTPUT, GPIO_PUSHPULL);  // Make GPIO9 an Output Pin
    GpioDataRegs.GPASET.bit.GPIO9 = 1;  //Initially Set GPIO9/SS High so DAN28027 is not selected

    GPIO_SetupPinMux(66, GPIO_MUX_CPU1, 0);  // Set as GPIO66 and used as MPU-9250 SS
    GPIO_SetupPinOptions(66, GPIO_OUTPUT, GPIO_PUSHPULL);  // Make GPIO66 an Output Pin
    GpioDataRegs.GPCSET.bit.GPIO66 = 1;  //Initially Set GPIO66/SS High so MPU-9250 is not selected

    GPIO_SetupPinMux(63, GPIO_MUX_CPU1, 15);  //Set GPIO63 pin to SPISIMOB
    GPIO_SetupPinMux(64, GPIO_MUX_CPU1, 15);  //Set GPIO64 pin to SPISOMIB
    GPIO_SetupPinMux(65, GPIO_MUX_CPU1, 15);  //Set GPIO65 pin to SPICLKB

    EALLOW;
    GpioCtrlRegs.GPBPUD.bit.GPIO63   = 0;  // Enable Pull-ups on SPI PINs Recommended by TI for SPI Pins
    GpioCtrlRegs.GPCPUD.bit.GPIO64   = 0;
    GpioCtrlRegs.GPCPUD.bit.GPIO65   = 0;
    GpioCtrlRegs.GPBQSEL2.bit.GPIO63 = 3;  // Set I/O pin to asynchronous mode recommended for SPI
    GpioCtrlRegs.GPCQSEL1.bit.GPIO64 = 3;  // Set I/O pin to asynchronous mode recommended for SPI
    GpioCtrlRegs.GPCQSEL1.bit.GPIO65 = 3;  // Set I/O pin to asynchronous mode recommended for SPI
    EDIS;


    SpibRegs.SPICCR.bit.SPISWRESET = 0;  // Put SPI in Reset

    SpibRegs.SPICTL.bit.CLK_PHASE = 1;  //This happens to be the mode for both the DAN28027 and
    SpibRegs.SPICCR.bit.CLKPOLARITY = 0;  //The MPU-9250,  Mode 01.
    SpibRegs.SPICTL.bit.MASTER_SLAVE = 1;  // Set to SPI Master
    SpibRegs.SPICCR.bit.SPICHAR = 15;  // Set to transmit and receive 16 bits each write to SPITXBUF (f in hex)
    SpibRegs.SPICTL.bit.TALK = 1;  // Enable transmission
    SpibRegs.SPIPRI.bit.FREE = 1;  // Free run, continue SPI operation
    SpibRegs.SPICTL.bit.SPIINTENA = 0;  // Disables the SPI interrupt

    SpibRegs.SPIBRR.bit.SPI_BIT_RATE = 50; // Set SCLK bit rate to 1 MHz so 1us period.  SPI base clock is
    // 50MHZ.  And this setting divides that base clock to create SCLK’s period
    SpibRegs.SPISTS.all = 0x0000;  // Clear status flags just in case they are set for some reason

    SpibRegs.SPIFFTX.bit.SPIRST = 1;// Pull SPI FIFO out of reset, SPI FIFO can resume transmit or receive.
    SpibRegs.SPIFFTX.bit.SPIFFENA = 1;    // Enable SPI FIFO enhancements
    SpibRegs.SPIFFTX.bit.TXFIFO =  0;    // Write 0 to reset the FIFO pointer to zero, and hold in reset
    SpibRegs.SPIFFTX.bit.TXFFINTCLR = 1;    // Write 1 to clear SPIFFTX[TXFFINT] flag just in case it is set

    SpibRegs.SPIFFRX.bit.RXFIFORESET = 0;    // Write 0 to reset the FIFO pointer to zero, and hold in reset
    SpibRegs.SPIFFRX.bit.RXFFOVFCLR = 1;    // Write 1 to clear SPIFFRX[RXFFOVF] just in case it is set
    SpibRegs.SPIFFRX.bit.RXFFINTCLR = 1;    // Write 1 to clear SPIFFRX[RXFFINT] flag just in case it is set
    SpibRegs.SPIFFRX.bit.RXFFIENA = 1;   // Enable the RX FIFO Interrupt.  RXFFST >= RXFFIL

    SpibRegs.SPIFFCT.bit.TXDLY = 16; //Set delay between transmits to 16 spi clocks. Needed by DAN28027 chip

    SpibRegs.SPICCR.bit.SPISWRESET = 1;    // Pull the SPI out of reset

    SpibRegs.SPIFFTX.bit.TXFIFO = 1;    // Release transmit FIFO from reset.
    SpibRegs.SPIFFRX.bit.RXFIFORESET = 1;    // Re-enable receive FIFO operation
    SpibRegs.SPICTL.bit.SPIINTENA = 1;    // Enables SPI interrupt.  !! I don’t think this is needed.  Need to Test

    SpibRegs.SPIFFRX.bit.RXFFIL = 10; //Interrupt Level to 16 words or more received into FIFO causes interrupt.  This is just the initial setting for the register.  Will be changed below


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


    //need to acknowledge IPC before use
    IpcRegs.IPCACK.bit.IPC0 = 1;
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;

    // 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;
    //Uncomment the line below to enable ADCB1 in the PIE: Group 1 interrupt 6
    PieCtrlRegs.PIEIER1.bit.INTx2 = 1;
    // Enable SPIB_RX in the PIE: Group 6 interrupt 3
    PieCtrlRegs.PIEIER6.bit.INTx3 = 1;
    //IPC
    PieCtrlRegs.PIEIER1.bit.INTx13 = 1;



    // SCIC setup for CPU2
    GPIO_SetupPinMux(139, GPIO_MUX_CPU1, 6);
    GPIO_SetupPinOptions(139, GPIO_INPUT, GPIO_PULLUP);
    GPIO_SetupPinMux(56, GPIO_MUX_CPU1, 6);
    GPIO_SetupPinOptions(56, GPIO_OUTPUT, GPIO_PUSHPULL);


    uint32_t clk;
    uint32_t baud = 115200;
    ScicRegs.SCICTL1.bit.SWRESET = 0;       // init SCI state machines and opt flags
    ScicRegs.SCICCR.all = 0x0;
    ScicRegs.SCICTL1.all = 0x0;
    ScicRegs.SCICTL2.all = 0x0;
    ScicRegs.SCIPRI.all = 0x0;
    clk = LSPCLK_HZ;                    // set baud rate
    clk /= baud*8;
    clk--;
    ScicRegs.SCILBAUD.all = clk & 0xFF;
    ScicRegs.SCIHBAUD.all = (clk >> 8) & 0xFF;

    ScicRegs.SCICCR.bit.SCICHAR = 0x7;      // (8) 8 bits per character
    ScicRegs.SCICCR.bit.PARITYENA = 0;      // (N) disable party calculation
    ScicRegs.SCICCR.bit.STOPBITS = 0;       // (1) transmit 1 stop bit
    ScicRegs.SCICCR.bit.LOOPBKENA = 0;      // disable loopback test
    ScicRegs.SCICCR.bit.ADDRIDLE_MODE = 0;  // idle-line mode (non-multiprocessor SCI comm)

    ScicRegs.SCIFFCT.bit.FFTXDLY = 0;       // TX: zero-delay

    ScicRegs.SCIFFTX.bit.SCIFFENA = 1;      // enable SCI fifo enhancements
    ScicRegs.SCIFFTX.bit.TXFIFORESET = 0;
    ScicRegs.SCIFFTX.bit.TXFFIL = 0x0;// TX: fifo interrupt at all levels   ???? is this correct
    ScicRegs.SCIFFTX.bit.TXFFINTCLR = 1;  // TX: clear interrupt flag
    ScicRegs.SCIFFTX.bit.TXFFIENA = 0;      // TX: disable fifo interrupt
    ScicRegs.SCIFFTX.bit.TXFIFORESET = 1;

    ScicRegs.SCIFFRX.bit.RXFIFORESET = 0;   // RX: fifo reset
    ScicRegs.SCIFFRX.bit.RXFFINTCLR = 1;    // RX: clear interrupt flag
    ScicRegs.SCIFFRX.bit.RXFFIENA = 1;      // RX: enable fifo interrupt
    ScicRegs.SCIFFRX.bit.RXFFIL = 0x1;      // RX: fifo interrupt
    ScicRegs.SCIFFRX.bit.RXFIFORESET = 1;   // RX: re-enable fifo

    ScicRegs.SCICTL2.bit.RXBKINTENA = 0;    // disable receiver/error interrupt
    ScicRegs.SCICTL2.bit.TXINTENA = 0;      // disable transmitter interrupt

    ScicRegs.SCICTL1.bit.TXWAKE = 0;
    ScicRegs.SCICTL1.bit.SLEEP = 0;         // disable sleep mode
    ScicRegs.SCICTL1.bit.RXENA = 1;         // enable SCI receiver
    ScicRegs.SCICTL1.bit.RXERRINTENA = 0;   // disable receive error interrupt
    ScicRegs.SCICTL1.bit.TXENA = 1;         // enable SCI transmitter
    ScicRegs.SCICTL1.bit.SWRESET = 1;       // re-enable SCI


    // Do this before giving control to CPU2
    EALLOW;
    EPwm5Regs.TBCTL.bit.CTRMODE = 0; //unfreeze, and enter up count mode
    EDIS;

    ScicRegs.SCIFFRX.bit.RXFFINTCLR = 1;
    SciaRegs.SCIFFRX.bit.RXFFINTCLR = 1;
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP8;
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP9;


    //for CPU2 to use SCIC and or SPIC
    EALLOW;
    //            DevCfgRegs.CPUSEL6.bit.SPI_C = 1;  //SPI C connected to CPU2
    DevCfgRegs.CPUSEL5.bit.SCI_C = 1;  //SCI C connected to CPU2


    EDIS;

    commandtoCPU2[0] = 'G';  // G for CPU2 Go

    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
    }


    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

    setEPWM2A(0); //Pass PWM to right motor
    setEPWM2B(0); //Pass negative PWM to left motor
    // IDLE loop. Just sit and loop forever (optional):
    while(1)
    {
        if (UARTPrint == 1 ) {
            serial_printf(&SerialA,"Number CPU2 Timer interrupt Calls %ld \r\n",CpuTimer2.InterruptCount);
            UARTPrint = 0;
        }
        if ((runPing == 1) || (runPong == 1)) {
            if (runPing == 1){
                hnd_rfft->InBuf     = &ping_input[0];  //Input buffer
                runPing = 0;
            }
            if (runPong == 1){
                hnd_rfft->InBuf     = &pong_input[0];  //Input buffer
                runPong = 0;
            }
            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=4;i<(RFFT_SIZE/2);i++) {
                if (pwrSpec[i]>maxpwr && pwrSpec[i]>powerThreshold) {
                    maxpwr = pwrSpec[i];
                    maxpwrindex = i; //512 is nyquist which is 10 Khz/2 = 5 KHz
                }
            }
            //UARTPrint = 1;
            //G Note
            if (maxpwrindex > G_Low_Threshold && maxpwrindex < G_High_Threshold){ //80 Expected
                serial_printf(&SerialA,"G Note, Index: %ld, Power: %.3f \r\n",maxpwrindex,maxpwr);
                turn = turn + 0.5; //turn right in place
                v_ref = 0;
                cpu1tocpu2[0] = 7.0;
                IpcRegs.IPCSET.bit.IPC0 = 1;
                DELAY_US(1000000);
            }
            //F Note
            if (maxpwrindex > F_Low_Threshold && maxpwrindex < F_High_Threshold){ //73 expected
                serial_printf(&SerialA,"F Note, Index: %ld, Power: %.3f \r\n",maxpwrindex,maxpwr);
                turn = turn - 0.5; //turn left in place
                v_ref = 0;
                cpu1tocpu2[0] = 6.0;
                IpcRegs.IPCSET.bit.IPC0 = 1;
                DELAY_US(1000000);
            }
            //A Note
            if (maxpwrindex > A_Low_Threshold && maxpwrindex < A_High_Threshold){ //90 expected
                serial_printf(&SerialA,"A Note, Index: %ld, Power: %.3f \r\n",maxpwrindex,maxpwr);
                v_ref = v_ref - 0.5; //Go backwards
                turn = 0;
                cpu1tocpu2[0] = 1.0;
                IpcRegs.IPCSET.bit.IPC0 = 1;
                DELAY_US(1000000);
            }
            //C Note
            if (maxpwrindex > C_Low_Threshold && maxpwrindex < C_High_Threshold){ //108 expected
                serial_printf(&SerialA,"C Note, Index: %ld, Power: %.3f \r\n",maxpwrindex,maxpwr);
                v_ref = v_ref + 0.5; //Go forwards
                turn = 0;
                cpu1tocpu2[0] = 3.0;
                IpcRegs.IPCSET.bit.IPC0 = 1;
                DELAY_US(1000000);
            }
            //STOP
            if (maxpwrindex > Stop_Low_Threshold && maxpwrindex < Stop_High_Threshold){ //108 expected
                serial_printf(&SerialA,"STOP, Index: %ld, Power: %.3f \r\n",maxpwrindex,maxpwr);
                v_ref = 0; //Go forwards
                turn = 0;
                cpu1tocpu2[0] = 9.0;
                IpcRegs.IPCSET.bit.IPC0 = 1;
                DELAY_US(1000000);
            }
        }



    }
}


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



    // Insert SWI ISR Code here.......


    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;

}

//IPC
__interrupt void CPU2toCPU1IPC0(void){
    GpioDataRegs.GPBTOGGLE.bit.GPIO52 = 1;
    GpioDataRegs.GPETOGGLE.bit.GPIO131 = 1;//led7
    //put data from cpu2 into cpu2tocpu1 array
    int i;
    for (i=0;i<4;i++) {
        receivefloats[i] = cpu2tocpu1[i];
    }
    numCPU2COMs++;
    //UARTPrint = 1;
    IpcRegs.IPCACK.bit.IPC0 = 1;
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
}

__interrupt void ADCB_isr (void)  //ADCB Interrupt
{
    ADCB_ISR_calls++;
    if (pingPong == 1){
        ping_input[ping_index] = (float)AdcbResultRegs.ADCRESULT0/1365.0; //Adds microphone reading to array
        ping_index++;
        if (ping_index >= 1024){
            pingPong = 0;
            runPing = 1;
            ping_index = 0;
            pong_index = 0;
        }
    }
    else if (pingPong == 0){
        pong_input[pong_index] = (float)AdcbResultRegs.ADCRESULT0/1365.0; //Adds microphone reading to array
        pong_index++;
        if (pong_index >= 1024){
            pingPong = 1;
            runPong = 1;
            pong_index = 0;
            ping_index = 0;
        }
    }

    //pingpong=1 ping input
    //pingpong=0 pong input
    //when index hits 1024 switch pingpong and set runping/runpong
    AdcbRegs.ADCINTFLGCLR.bit.ADCINT1 = 1;  //clear interrupt flag
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
}


// cpu_timer0_isr - CPU Timer0 ISR
__interrupt void cpu_timer0_isr(void)
{
    CpuTimer0.InterruptCount++;
    numTimer0calls++;
    // We know that passing 5 to setEPWM2? results in 2.085 ft/sec. Therefore, passing 2.398 will result  in 1 m/s. Therefore, multiply v_ref by 2.398


    //Old States are Being Saved Here
    PosLeft_K_1 = left_dist;
    PosRight_K_1 = right_dist;
    ik_1_r = ik_r;
    ik_1_l = ik_l;
    ek_1_r = ek_r;
    ek_1_l = ek_l;
    theta_l_1 = theta_l;
    theta_r_1 = theta_r;
    x_r_1 = x_r;
    y_r_1 = y_r;

    //Current states are defined here
    theta_l = -readEncLeft(); //Get left angular position from encoder
    theta_r = readEncRight(); //Get right angular position from encoder
    phi_r = R_wh*(theta_r-theta_l)/W_r; //Calculates pose angle
    theta_avg = 0.5*(theta_r+theta_l); //Calculates average position
    theta_dot_l = (theta_l - theta_l_1) / (0.004); //Calculates left wheel angular velocity
    theta_dot_r = (theta_r - theta_r_1) / (0.004); //Calculates right wheel angular velocity
    theta_dot_avg = 0.5*(theta_dot_r+theta_dot_l); //Calculates average angular velocity
    x_dot_r = R_wh*theta_dot_avg*cos(phi_r); //Calculates x linear velocity of robot
    y_dot_r = R_wh*theta_dot_avg*sin(phi_r); //Calculates y linear velocity of robot
    x_r = x_r_1 + 0.004*x_dot_r; //Updates x pose based on previous pose and linear velocity
    y_r = y_r_1 + 0.004*y_dot_r; //Updates y pose based on previous pose and linear velocity
    right_dist = theta_r * rad_to_feet; //converts radians to feet
    left_dist = theta_l * rad_to_feet; //converts radians to feet
    PosLeft_K = left_dist; //Stores current left position
    PosRight_K = right_dist; //Stores current right position
    vel_left = (PosLeft_K - PosLeft_K_1) / (0.004); //Uses two most recent left positions to find left velocity
    vel_right = (PosRight_K - PosRight_K_1) / (0.004); //Uses two most recent right positions to find right velocity
    e_turn = turn + (vel_left - vel_right); //Sets turn error
    ek_r = v_ref - vel_right + KP_turn*e_turn; //Finds total right error
    ek_l = v_ref - vel_left - KP_turn*e_turn; //Find total left error

    //Avoid integral windup
    if (fabs(uRight) < 9.9){
        ik_r = ik_1_r + 0.004*(ek_r + ek_1_r)/2;
    }
    else {
        ik_r = ik_1_r; //If wheel is stopped do not increment ik
    }

    if (fabs(uLeft) < 9.9){
        ik_l = ik_1_l + 0.004*(ek_l + ek_1_l)/2;
    }
    else {
        ik_l = ik_1_l; //If wheel is stopped do not increment ik
    }

    uRight = ek_r*kp + ik_r*ki; //Set PWM to ek*Kp + ik*Ki for the right motor
    uLeft = ek_l*kp + ik_l*ki; //Set PWM to ek*Kp + ik*Ki for the left motor
    setEPWM2A(uRight); //Pass PWM to right motor
    setEPWM2B(-uLeft); //Pass negative PWM to left motor
//    if (numTimer0calls % 75 == 0) {
//        UARTPrint = 1; //Print every 75 iterations
//    }

//    if ((numTimer0calls%50) == 0) {
//        PieCtrlRegs.PIEIFR12.bit.INTx9 = 1;  // Manually cause the interrupt for the SWI
//    }
    //Clear GPIO9 Low to act as a Slave Select. Right now, just to scope. Later to select DAN28027 chip
    //GpioDataRegs.GPACLEAR.bit.GPIO9 = 1; //For DAN Chip
    GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1; //For MPU chip
    SpibRegs.SPIFFRX.bit.RXFFIL = 8; // Issue the SPIB_RX_INT when two values are in the RX FIFO (2 initially, 3 for ex. 3, 8 for ex. 4)
    //SpibRegs.SPITXBUF = 0x4A3B; // 0x4A3B and 0xB517 have no special meaning. Wanted to send (Ex.2)
    //SpibRegs.SPITXBUF = 0xB517; // something so you can see the pattern on the Oscilloscope (Ex.2)



...

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