Marilyn Sedlak
Published

Dancing Segway Robot

This balancing "Segbot" sings and dances upon the recognition of different notes (UIUC ME 461)

BeginnerShowcase (no instructions)327
Dancing Segway Robot

Things used in this project

Hardware components

LAUNCHXL-F28379D C2000 Delfino LaunchPad
Texas Instruments LAUNCHXL-F28379D C2000 Delfino LaunchPad
×1
SparkFun IMU Breakout - MPU-9250
SparkFun IMU Breakout - MPU-9250
×1
PCB - Dan Block UIUC
×1
Microphone
×1
Buzzer
×1
DC Motor
×2

Software apps and online services

Code Composer Studio
Texas Instruments Code Composer Studio

Story

Read more

Schematics

Circuit Board Schematic

The circuit board, designed at University of Illinois by Dan Block, is used in conjunction with the TI TMS320F28379D microcontroller

Code

SongBot

C/C++
When flashed onto the segbot, this code uses the microphone to listen to specified notes. When one of these notes is detected, the segbot sings a song and does a dance.
//#############################################################################
// 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.

Workspace

C/C++
.zip folder that contains a CCS project for ME 461 labs over the course of the semester as well as the FinalProject folder that has different versions of the final code for this project
No preview (download only).

Credits

Marilyn Sedlak
2 projects • 2 followers
Mechanical engineering student at UIUC
Contact
Thanks to Dan Block.

Comments

Please log in or sign up to comment.