Published May 14, 2021

Rhythm Robot (UIUC SE 423)

This dancing Segbot will start dancing to the tune of a song when it hears music.

IntermediateShowcase (no instructions)231

Things used in this project

Hardware components

Texas Instruments LAUNCHXL-F28379D C2000 Delfino LaunchPad

Software apps and online services

Texas Instruments Code Composer Studio

Story

The "Segbot" was built during the Spring 2021 semester at the University of Illinois at Urbana-Champaign by engineering students in SE 423: Mechatronics. It is a balancing, two-wheeled robot that stays upright using a Kalman filtered control system. The Segbot was designed by Professor Dan Block at the University of Illinois and uses the Texas Instruments F28379D Launchpad board as a controller. The Segbot possesses a gyroscope/accelerometer sensor as well as a microphone. For my final project, I wanted to make the Segbot dance to the tune of a real song. Below is a video of the Segbot dancing to "Fireworks" by Purple Disco Machine. The Segbot will stop dancing if the music stops playing, and will go back and forth (bop) when it hears significant increases in pitch such as during the chorus, then return to its default "twist dance".

FFT Library

The Segbot detects sound using the microphone which is converted to ADC signals. The Texas Instruments FFT (Fast Fourier Transform) library, which uses a ping-pong buffer, then computes the magnitude and pitch of the sound waves, which are stored as the variables maxpwr and maxpwrindex, respectively.

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

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


    // IDLE loop. Just sit and loop forever (optional):
    while(1)
    {
        if (UARTPrint == 1 ) {
            serial_printf(&SerialA,"maxpwr: %f\tmaxpwrindex: %u\r\n",maxpwr, maxpwrindex);
            UARTPrint = 0;
        }
        if (runping == 1) {
            hnd_rfft->InBuf     = &ping_input[0];  //Input buffer
        }
        if (runpong == 1) {
            hnd_rfft->InBuf     = &pong_input[0];  //Input buffer
        }
        if (runping == 1 || runpong == 1) {
            runping = 0;
            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=3;i<(RFFT_SIZE/2);i++) {
                if (pwrSpec[i]>maxpwr) {
                    maxpwr = pwrSpec[i];
                    maxpwrindex = i;
                }
            }
            UARTPrint = 1;
        }
    }
}

Algorithms

The Segbot dances with "rhythm" by changing its dance move from side-to-side to forward and backward whenever it hears significant spikes in frequency that people would notice, such as during a chorus. Since real songs contain a huge spectrum of notes at different magnitudes, the main challenge was figuring out how to make the Segbot dance such that it does not simply do a dance whenever it hears a note high enough. This is because there are many spikes in frequency throughout a song and the Segbot would just switch back and forth between dance moves or be stuck on one move. Instead, many parameters were defined that could be tweaked by changing their value in real-time using Code Composer Studio while testing the Segbot to determine the best values. Many of these parameters were stored as global variables.

float spinspeed;
float latspeed;
bool turning = false;
bool start = false;
uint16_t beatcount;
uint16_t totalpitch;
uint16_t totalmag;
uint16_t minmag = 120;
uint16_t minpitch = 15;
uint16_t changepitch = 5;
float pitchsamplecount = 50.0;
float magsamplecount = 500.0;
float spinlimit = 9;
float latlimit = 2.5;
float backinc = .1;
float fwdinc = .1;
float anginc = .2;
float lastchange;
uint16_t highnotecount;
float percenthigh = 0.02;
uint16_t highnote = 100;
uint16_t minmovecount = 500;
uint16_t countbopping;
uint16_t mincountbopping = 300;

The most important parameters that enable the Segbot to dance with rhythm are the percenthigh, highnote, and mincountbopping variables. The F28379D possesses multiple CPUs which can be used to crunch numbers in real-time. The Cpu2 interrupt function, which was configured to run every 4 milliseconds, was used to calculate the measurements needed to make the Segbot dance properly. The pitch would be sampled for a certain amount of time, and the high notes counted. If in the last period of time, say, 50 counts (0.2 seconds), a significant amount of notes are high, then the robot would do the "bop" dance for a minimum of 300 counts (1.2 seconds). Through testing with numerous songs, I determined that the best parameter configuration to make the Segbot appear to have the most rhythm was that if 2% of notes in the last sample are above a 100 maxpwrindex, the Segbot will do its bop dance for 1.2 seconds, and then return to its base "twist" dance. The high note, percent of high notes, sample time, and minimum time bopping can all be adjusted.

To make the dance moves smooth, the variables that determine the spin rate and movement rate (turnrate and FwdBackOffset), should not be changed dramatically, otherwise, the Segbot would lose its balance and zoom off in one direction. For this reason, these values are incremented a small amount each iteration of the interrupt function until it reaches a limit and then reversed. The robot will also stop dancing if it is not picking up sound magnitudes of at least a 100 maxpwr.

__interrupt void cpu_timer2_isr(void)
{
    totalpitch += maxpwrindex;
    float avgpitch = totalpitch / pitchsamplecount;
    totalmag += maxpwr;
    float avgmag = totalmag / magsamplecount;
    if (maxpwrindex > highnote) {
        highnotecount++;
    }
    if (!turning) {
        countbopping++;
    }
    if (CpuTimer2.InterruptCount % (int)(pitchsamplecount) == 0) {
        if (turning || countbopping > mincountbopping) {
            if (highnotecount / pitchsamplecount > percenthigh) {
                turning = false;
                latspeed = 0;
                spinspeed = 0;
            }
            else {
                turning = true;
                latspeed = 0;
                spinspeed = 0;
            }
            countbopping = 0;
        }
        highnotecount = 0;
        totalpitch = 0;
    }
    if (CpuTimer2.InterruptCount % (int)(magsamplecount) == 0) {
        if (avgmag > minmag && avgpitch > minpitch) {
            start = true;
        }
        else {
            start = false;
            FwdBackOffset = 0;
            turnrate = 0;
        }
        totalmag = 0;
    }

    if (start) {
        if ((turning && spinspeed > spinlimit) || (!turning && latspeed > latlimit)) {
            increasing = false;
        }
        if ((turning && spinspeed < -spinlimit) || (!turning && latspeed < -latlimit)) {
            increasing = true;
        }
        if (increasing) {
            latspeed += fwdinc;
            spinspeed += anginc;
        }
        else {
            latspeed -= backinc;
            spinspeed -= anginc;
        }
        if (turning) {
            turnrate = spinspeed;
            FwdBackOffset = 0;
        }
        else {
            turnrate = 0;
            FwdBackOffset = latspeed;
        }
    }
    CpuTimer2.InterruptCount++;
}

Thanks for reading.

// 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"
#include "LEDPatterns.h"

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

//*****************************************************************************
// 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 SPIB_isr(void);
__interrupt void ADCA_ISR (void);
__interrupt void ADCB_ISR (void);

void serialRXA(serial_t *s, char data);
void setupSpib(void);
void init_eQEPs(void);
float readEncLeft(void);
float readEncRight(void);
void setEPWM2A(float controleffort);
void setEPWM2B(float controleffort);
void setDACA(float dacouta0);
void setDACB(float dacouta1);
void changeMovementSegBot();

// Count variables
uint32_t numTimer0calls = 0;
uint32_t numSWIcalls = 0;
uint32_t numRXA = 0;
uint16_t UARTPrint = 0;
uint16_t LEDdisplaynum = 0;
int16_t aspivalue0 = 0;
int16_t aspivalue1 = 0;
int16_t aspivalue2 = 0;
int16_t gspivalue0 = 0;
int16_t gspivalue1 = 0;
int16_t gspivalue2 = 0;
uint16_t pwm = 1500;
float accelx = 0;
float accely = 0;
float accelz = 0;
float gyrox = 0;
float gyroy = 0;
float gyroz = 0;
bool increasing = true;
float adc1voltage;
float adc2voltage;
float LeftWheel;
float RightWheel;
float LeftDistance;
float RightDistance;
float LeftDistance_1 = 0;
float RightDistance_1 = 0;
float uLeft = 5.0;
float uRight = -5.0;
float vLeft;
float vRight;
float ErrorLeft;
float ErrorRight;
float ErrorLeftK_1;
float ErrorRightK_1;
float ILeftK;
float IRightK;
float ILeftK_1 = 0;
float IRightK_1 = 0;
//float Kp = 3;
//float Ki = 15;
float Vref = 0;
float turn;
float ErrorTurn;
float KPturn = 3;
float VXr_1 = 0;
float VYr_1 = 0;
float VXr = 0;
float VYr = 0;
float Xr = 0;
float Yr = 0;
float bearing = 0;
float radius = 0.10625;
float width = 0.61;
float VangLeft = 0;
float VangRight = 0;
float avgangle = 0;
float Vangavg = 0;

//lab 7
uint16_t adca0result = 0;
uint16_t adca1result = 0;
float xk;
float xk2;
float yk;
int i;
float array[22] = {0};
float b[22]={ -2.3890045153263611e-03, -3.3150057635348224e-03, -4.6136191242627002e-03, -4.1659855521681268e-03, 1.4477422497795286e-03, 1.5489414225159667e-02, 3.9247886844071371e-02, 7.0723964095458614e-02, 1.0453473887246176e-01, 1.3325672639406205e-01, 1.4978314227429904e-01, 1.4978314227429904e-01, 1.3325672639406205e-01, 1.0453473887246176e-01, 7.0723964095458614e-02, 3.9247886844071371e-02, 1.5489414225159667e-02, 1.4477422497795286e-03, -4.1659855521681268e-03, -4.6136191242627002e-03, -3.3150057635348224e-03, -2.3890045153263611e-03};
uint32_t ADCDcount = 0;
uint32_t spib_isr;

//Exercise 2 global variables
// 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 = -.745;
int16 IMU_data[9];
uint16_t temp=0;
int16_t doneCal = 0;
float tilt_value = 0;
float tilt_array[4] = {0, 0, 0, 0};
float gyro_value = 0;
float gyro_array[4] = {0, 0, 0, 0};
float LeftWheelArray[4] = {0,0,0,0};
float RightWheelArray[4] = {0,0,0,0};
// Kalman Filter vars
float T = 0.001; //sample rate, 1ms
float Q = 0.01; // made global to enable changing in runtime
float R = 25000;//50000;
float kalman_tilt = 0;
float kalman_P = 22.365;
int16_t SpibNumCalls = -1;
float pred_P = 0;
float kalman_K = 0;
int32_t timecount = 0;
int16_t calibration_state = 0;
int32_t calibration_count = 0;

//Exercise 3
float velLeft = 0;
float velRight = 0;
float velLeft_1 = 0;
float velRight_1 = 0;
float LeftWheel_1 = 0;
float RightWheel_1 = 0;
float ubal = 0;
float k1 = -60.0;
float k2 = -4.5;
float k3 = -1.1;
float uLeftBal = 0;
float uRightBal = 0;

//exercise 4
float WhlDiff;
float WhlDiff_1;
float vel_WhlDiff;
float vel_WhlDiff_1;
float turnref;
float errorDiff;
float errorDiff_1;
float intDiff;
float intDiff_1;
float FwdBackOffset;
float Kp = 3.0;
float Ki = 20.0;
float Kd = 0.08;
float turnrate;
float turnrate_1;
float turnref_1;

//final project
float runping;
float runpong;
uint16_t index = 0;
bool ping = true;
float adcb0result;
float adcb1result;

float spinspeed;
float latspeed;
bool turning = false;
bool start = false;
uint16_t beatcount;
uint16_t totalpitch;
uint16_t totalmag;
uint16_t minmag = 120;
uint16_t minpitch = 15;
uint16_t changepitch = 5;
float pitchsamplecount = 50.0;
float magsamplecount = 500.0;
float spinlimit = 9;
float latlimit = 2.5;
float backinc = .1;
float fwdinc = .1;
float anginc = .2;
float lastchange;
uint16_t highnotecount;
float percenthigh = 0.02;
uint16_t highnote = 100;
uint16_t minmovecount = 500;
uint16_t countbopping;
uint16_t mincountbopping = 300;


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

    InitGpio();

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    // Interrupts that are used in this example are re-mapped to
    // ISR functions found within this project
    EALLOW;  // This is needed to write to EALLOW protected registers
    PieVectTable.TIMER0_INT = &cpu_timer0_isr;
    PieVectTable.TIMER1_INT = &cpu_timer1_isr;
    PieVectTable.TIMER2_INT = &cpu_timer2_isr;
    PieVectTable.SCIA_RX_INT = &RXAINT_recv_ready;
    PieVectTable.SCIC_RX_INT = &RXCINT_recv_ready;
    PieVectTable.SCID_RX_INT = &RXDINT_recv_ready;
    PieVectTable.SCIA_TX_INT = &TXAINT_data_sent;
    PieVectTable.SCIC_TX_INT = &TXCINT_data_sent;
    PieVectTable.SCID_TX_INT = &TXDINT_data_sent;
    PieVectTable.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, 10000);
    ConfigCpuTimer(&CpuTimer1, 200, 20000);
    ConfigCpuTimer(&CpuTimer2, 200, 4000);

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

    init_eQEPs();
    setupSpib();

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

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

    // Exercise 1.2
    EALLOW;
    //write configurations for all ADCs  ADCA, ADCB, ADCC, ADCD
    AdcaRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
    AdcbRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
    AdccRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
    AdcdRegs.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
    AdcSetMode(ADC_ADCC, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE);  //read calibration settings
    AdcSetMode(ADC_ADCD, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE);  //read calibration settings
    //Set pulse positions to late
    AdcaRegs.ADCCTL1.bit.INTPULSEPOS = 1;
    AdcbRegs.ADCCTL1.bit.INTPULSEPOS = 1;
    AdccRegs.ADCCTL1.bit.INTPULSEPOS = 1;
    AdcdRegs.ADCCTL1.bit.INTPULSEPOS = 1;
    //power up the ADCs
    AdcaRegs.ADCCTL1.bit.ADCPWDNZ = 1;
    AdcbRegs.ADCCTL1.bit.ADCPWDNZ = 1;
    AdccRegs.ADCCTL1.bit.ADCPWDNZ = 1;
    AdcdRegs.ADCCTL1.bit.ADCPWDNZ = 1;
    //delay for 1ms to allow ADC time to power up
    DELAY_US(1000);
    //Select the channels to convert and end of conversion flag
    //Many statements commented out,  To be used when using ADCA or ADCB
    //ADCA
    AdcaRegs.ADCSOC0CTL.bit.CHSEL = 2;  //SOC0 will convert Channel you choose Does not have to be A0
    AdcaRegs.ADCSOC0CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    AdcaRegs.ADCSOC0CTL.bit.TRIGSEL = 0xd;// EPWM5 ADCSOCA or another trigger you choose will trigger SOC0
    AdcaRegs.ADCSOC1CTL.bit.CHSEL = 3;  //SOC1 will convert Channel you choose Does not have to be A1
    AdcaRegs.ADCSOC1CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    AdcaRegs.ADCSOC1CTL.bit.TRIGSEL = 0xd;// EPWM5 ADCSOCA or another trigger you choose will trigger SOC1
    AdcaRegs.ADCINTSEL1N2.bit.INT1SEL = 1; //set to last SOC that is converted and it will set INT1 flag ADCA1
    AdcaRegs.ADCINTSEL1N2.bit.INT1E = 1;   //enable INT1 flag
    AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //make sure INT1 flag is cleared

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

    //    //ADCD
    //    AdcdRegs.ADCSOC0CTL.bit.CHSEL = 0;  // set SOC0 to convert pin D0
    //    AdcdRegs.ADCSOC0CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    //    AdcdRegs.ADCSOC0CTL.bit.TRIGSEL = 0xd; // EPWM5 ADCSOCA will trigger SOC0
    //    AdcdRegs.ADCSOC1CTL.bit.CHSEL = 1;  //set SOC1 to convert pin D1
    //    AdcdRegs.ADCSOC1CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    //    AdcdRegs.ADCSOC1CTL.bit.TRIGSEL = 0xd; // EPWM5 ADCSOCA will trigger SOC1
    //    //AdcdRegs.ADCSOC2CTL.bit.CHSEL = ???;  //set SOC2 to convert pin D2
    //    //AdcdRegs.ADCSOC2CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    //    //AdcdRegs.ADCSOC2CTL.bit.TRIGSEL = ???; // EPWM5 ADCSOCA will trigger SOC2
    //    //AdcdRegs.ADCSOC3CTL.bit.CHSEL = ???;  //set SOC3 to convert pin D3
    //    //AdcdRegs.ADCSOC3CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    //    //AdcdRegs.ADCSOC3CTL.bit.TRIGSEL = ???; // EPWM5 ADCSOCA will trigger SOC3
    //    AdcdRegs.ADCINTSEL1N2.bit.INT1SEL = 1; //set to SOC1, the last converted, and it will set INT1 flag ADCD1
    //    AdcdRegs.ADCINTSEL1N2.bit.INT1E = 1;   //enable INT1 flag
    //    AdcdRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //make sure INT1 flag is cleared
    EDIS;

    //    // Exercise 1.3
    //    // Enable DACA and DACB outputs
    EALLOW;
    DacaRegs.DACOUTEN.bit.DACOUTEN = 1;//enable dacA output-->uses ADCINA0
    DacaRegs.DACCTL.bit.LOADMODE = 0;//load on next sysclk
    DacaRegs.DACCTL.bit.DACREFSEL = 1;//use ADC VREF as reference voltage
    DacbRegs.DACOUTEN.bit.DACOUTEN = 1;//enable dacB output-->uses ADCINA1
    DacbRegs.DACCTL.bit.LOADMODE = 0;//load on next sysclk
    DacbRegs.DACCTL.bit.DACREFSEL = 1;//use ADC VREF as reference voltage
    EDIS;
    ///////////////////////////////////////////////////////////////////////////////////////////

    //Configuration for EPWM2A and EPWM2B, the two motors
    EPwm2Regs.TBCTL.bit.CTRMODE = 0;
    EPwm2Regs.TBCTL.bit.FREE_SOFT = 2;
    EPwm2Regs.TBCTL.bit.PHSEN = 0;
    EPwm2Regs.TBCTL.bit.CLKDIV = 0;
    EPwm2Regs.TBCTR = 0;
    EPwm2Regs.TBPRD = 2500;
    EPwm2Regs.CMPA.bit.CMPA = 1250;
    EPwm2Regs.CMPB.bit.CMPB = 1250;
    EPwm2Regs.AQCTLA.bit.CAU = 1;
    EPwm2Regs.AQCTLA.bit.ZRO = 2;
    EPwm2Regs.AQCTLB.bit.CBU = 1;
    EPwm2Regs.AQCTLB.bit.ZRO = 2;
    EPwm2Regs.TBPHS.bit.TBPHS = 0;

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

    //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;
    IER |= M_INT6; //SPIB

    // Enable TINT0 in the PIE: Group 1 interrupt 7
    PieCtrlRegs.PIEIER1.bit.INTx7 = 1;
    // Enable SWI in the PIE: Group 12 interrupt 9
    PieCtrlRegs.PIEIER12.bit.INTx9 = 1;
    PieCtrlRegs.PIEIER6.bit.INTx3 = 1;
    // Enable ADCA1
    PieCtrlRegs.PIEIER1.bit.INTx1 = 1;
    //Enable ADCB1
    PieCtrlRegs.PIEIER1.bit.INTx2 = 1;

    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
    }

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


    // IDLE loop. Just sit and loop forever (optional):
    while(1)
    {
        if (UARTPrint == 1 ) {
            serial_printf(&SerialA,"maxpwr: %f\tmaxpwrindex: %u\r\n",maxpwr, maxpwrindex);
            UARTPrint = 0;
        }
        if (runping == 1) {
            hnd_rfft->InBuf     = &ping_input[0];  //Input buffer
        }
        if (runpong == 1) {
            hnd_rfft->InBuf     = &pong_input[0];  //Input buffer
        }
        if (runping == 1 || runpong == 1) {
            runping = 0;
            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=3;i<(RFFT_SIZE/2);i++) {
                if (pwrSpec[i]>maxpwr) {
                    maxpwr = pwrSpec[i];
                    maxpwrindex = i;
                }
            }
            UARTPrint = 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



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

    //exercise 3
    velLeft = 0.6*velLeft_1+100*(LeftWheel - LeftWheel_1);
    velRight = 0.6*velRight_1+100*(RightWheel - RightWheel_1);

    ubal = -k1*tilt_value-k2*gyro_value-k3*(velLeft+velRight)/2.0;
    //uLeftBal = ubal / 2.0;
    //uRightBal = ubal / 2.0;

    //exercise 4
    WhlDiff = LeftWheel - RightWheel;
    vel_WhlDiff = 0.333*vel_WhlDiff_1+166.667*(WhlDiff - WhlDiff_1);
    errorDiff = turnref - WhlDiff;
    intDiff = intDiff_1 + ((errorDiff + errorDiff_1)/2.0)*.004;
    turn = Kp*errorDiff + Ki*intDiff - Kd*vel_WhlDiff;
    if (fabs(turn) > 3.0) {
        intDiff = intDiff_1;
    }
    if (turn > 4.0) {
        turn = 4.0;
    }
    if (turn < -4.0) {
        turn = -4.0;
    }
    uLeftBal = ubal / 2.0 + turn + FwdBackOffset;
    uRightBal = ubal / 2.0 - turn + FwdBackOffset;
    turnref = turnref_1 + ((turnrate + turnrate_1)/2.0)*.004;

    setEPWM2B(uLeftBal);
    setEPWM2A(uRightBal);

    //save states
    LeftWheel_1 = LeftWheel;
    RightWheel_1 = RightWheel;
    velLeft_1 = velLeft;
    velRight_1 = velRight;

    vel_WhlDiff_1 = vel_WhlDiff;
    WhlDiff_1 = WhlDiff;
    errorDiff_1 = errorDiff;
    intDiff_1 = intDiff;
    turnrate_1 = turnrate;
    turnref_1 = turnref;

    numSWIcalls++;

    DINT;

}

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

    numTimer0calls++;
    /*
    if ((numTimer0calls%50) == 0) {
        PieCtrlRegs.PIEIFR12.bit.INTx9 = 1;  // Manually cause the interrupt for the SWI  
    }
     */
    if ((numTimer0calls%250) == 0) {
        displayLEDletter(LEDdisplaynum);
        LEDdisplaynum++;
        if (LEDdisplaynum == 0xFFFF) {  // prevent roll over exception
            LEDdisplaynum = 0;
        }
    }

    // Blink LaunchPad Red LED
    //GpioDataRegs.GPBTOGGLE.bit.GPIO34 = 1;
    // 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)
{

    CpuTimer1.InterruptCount++;
}

// cpu_timer2_isr CPU Timer2 ISR
__interrupt void cpu_timer2_isr(void)
{
    totalpitch += maxpwrindex;
    float avgpitch = totalpitch / pitchsamplecount;
    totalmag += maxpwr;
    float avgmag = totalmag / magsamplecount;
    if (maxpwrindex > highnote) {
        highnotecount++;
    }
    if (!turning) {
        countbopping++;
    }
    if (CpuTimer2.InterruptCount % (int)(pitchsamplecount) == 0) {
        if (turning || countbopping > mincountbopping) {
            if (highnotecount / pitchsamplecount > percenthigh) {
                turning = false;
                latspeed = 0;
                spinspeed = 0;
            }
            else {
                turning = true;
                latspeed = 0;
                spinspeed = 0;
            }
            countbopping = 0;
        }
        highnotecount = 0;
        totalpitch = 0;
    }
    if (CpuTimer2.InterruptCount % (int)(magsamplecount) == 0) {
        if (avgmag > minmag && avgpitch > minpitch) {
            start = true;
        }
        else {
            start = false;
            FwdBackOffset = 0;
            turnrate = 0;
        }
        totalmag = 0;
    }

    if (start) {
        if ((turning && spinspeed > spinlimit) || (!turning && latspeed > latlimit)) {
            increasing = false;
        }
        if ((turning && spinspeed < -spinlimit) || (!turning && latspeed < -latlimit)) {
            increasing = true;
        }
        if (increasing) {
            latspeed += fwdinc;
            spinspeed += anginc;
        }
        else {
            latspeed -= backinc;
            spinspeed -= anginc;
        }
        if (turning) {
            turnrate = spinspeed;
            FwdBackOffset = 0;
        }
        else {
            turnrate = 0;
            FwdBackOffset = latspeed;
        }
    }
    CpuTimer2.InterruptCount++;
}


// This function is called each time a char is received over UARTA.

__interrupt void SPIB_isr(void){
    //Exercise 1
    //GpioDataRegs.GPASET.bit.GPIO9  =  1;  //  Set  GPIO  9 high to  end  Slave  Select.    Now  to  Scope.
    //exercise 3
    //adc1voltage = (float)(spivalue1)*(3.3/4500.0);
    //adc2voltage = (float)(spivalue2)*(3.3/4500.0);
    //exercise 4
    GpioDataRegs.GPCSET.bit.GPIO66  =  1;

    uint16_t discard = SpibRegs.SPIRXBUF;
    aspivalue0 = SpibRegs.SPIRXBUF;
    aspivalue1 = SpibRegs.SPIRXBUF;
    aspivalue2 = SpibRegs.SPIRXBUF;

    accelx = aspivalue0*4.0/32767.0;
    accely = aspivalue1*4.0/32767.0;
    accelz = aspivalue2*4.0/32767.0;

    discard = SpibRegs.SPIRXBUF;
    gspivalue0 = SpibRegs.SPIRXBUF;
    gspivalue1 = SpibRegs.SPIRXBUF;
    gspivalue2 = SpibRegs.SPIRXBUF;

    gyrox = gspivalue0*250.0/32767.0;
    gyroy = gspivalue1*250.0/32767.0;
    gyroz = gspivalue2*250.0/32767.0;

    //Lab 7
    //Read radians traveled by wheels
    //RightWheel = readEncRight();
    //LeftWheel = readEncLeft();

    //setEPWM2B(uLeft);
    //setEPWM2A(uRight);


    //Exercise 2
    //Code to be copied into SPIB_ISR interrupt function after the IMU measurements have been collected.
    if(calibration_state == 0)  {
        calibration_count++;
        if (calibration_count == 2000)  {
            calibration_state = 1;
            calibration_count = 0;
        }
    }
    else if(calibration_state == 1) {
        accelx_offset+=accelx;
        accely_offset+=accely;
        accelz_offset+=accelz;
        gyrox_offset+=gyrox;
        gyroy_offset+=gyroy;
        gyroz_offset+=gyroz;
        calibration_count++;
        if (calibration_count == 2000) {
            calibration_state = 2;
            accelx_offset/=2000.0;
            accely_offset/=2000.0;
            accelz_offset/=2000.0;
            gyrox_offset/=2000.0;
            gyroy_offset/=2000.0;
            gyroz_offset/=2000.0;
            calibration_count = 0;
            doneCal = 1;
        }
    }
    else if(calibration_state == 2) {
        accelx -=(accelx_offset);
        accely -=(accely_offset);
        accelz -=(accelz_offset-accelzBalancePoint);
        gyrox -= gyrox_offset;
        gyroy -= gyroy_offset;
        gyroz -= gyroz_offset;
        /*--------------Kalman Filtering code start---------------------------------------------------------------------*/
        float tiltrate = (gyrox*PI)/180.0; // rad/s
        float pred_tilt, z, y, S;
        // Prediction Step
        pred_tilt = kalman_tilt + T*tiltrate;
        pred_P = kalman_P + Q;
        // Update Step
        z = -accelz; // Note the negative here due to the polarity of AccelZ
        y = z - pred_tilt;
        S = pred_P + R;
        kalman_K = pred_P/S;
        kalman_tilt = pred_tilt + kalman_K*y;
        kalman_P = (1 - kalman_K)*pred_P;
        SpibNumCalls++;
        // Kalman Filter used
        tilt_array[SpibNumCalls] = kalman_tilt;
        gyro_array[SpibNumCalls] = tiltrate;
        LeftWheelArray[SpibNumCalls] = -readEncLeft();
        RightWheelArray[SpibNumCalls] = readEncRight();
        if (SpibNumCalls >= 3) { // should never be greater than 3
            tilt_value = (tilt_array[0] + tilt_array[1] + tilt_array[2] + tilt_array[3])/4.0;
            gyro_value = (gyro_array[0] + gyro_array[1] + gyro_array[2] + gyro_array[3])/4.0;
            LeftWheel=(LeftWheelArray[0]+LeftWheelArray[1]+LeftWheelArray[2]+LeftWheelArray[3])/4.0;
            RightWheel=(RightWheelArray[0]+RightWheelArray[1]+RightWheelArray[2]+RightWheelArray[3])/4.0;
            SpibNumCalls = -1;
            PieCtrlRegs.PIEIFR12.bit.INTx9 = 1; // Manually cause the interrupt for the SWI
        }
    }
    timecount++;
    if  ((timecount%200) == 0)    {
        if(doneCal == 0) {
            GpioDataRegs.GPATOGGLE.bit.GPIO31 = 1; // Blink Blue LED while calibrating
        }
        GpioDataRegs.GPBTOGGLE.bit.GPIO34 = 1; // Always Block Red LED
        UARTPrint = 1; // Tell While loop to print
    }

    //Later  to  deselect  DAN28027
    // Later when actually communicating with the DAN28027 do something with the data.  Now do nothing.
    SpibRegs.SPIFFRX.bit.RXFFOVFCLR = 1;      // Clear Overflow flag just in case of an overflow
    SpibRegs.SPIFFRX.bit.RXFFINTCLR = 1;      // Clear RX FIFO Interrupt flag so next interrupt will happen
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP6;  // Acknowledge INT6 PIE interrupt}
}

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') {
        FwdBackOffset = FwdBackOffset -  0.2;
    }
    else if (data == 's') {
        FwdBackOffset = FwdBackOffset + 0.2;
    }
...

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

Credits

Benjamin Kofman

1 project • 0 followers

Thanks to Dan Block.

Rhythm Robot (UIUC SE 423)

Things used in this project

Hardware components

Software apps and online services

Story

FFT Library

Algorithms

Code

C main file

Project file zip

Credits

Benjamin Kofman

Comments

Embed the widget on your own site

Rhythm Robot (UIUC SE 423)

Rhythm Robot (UIUC SE 423)

Things used in this project

Hardware components

Software apps and online services

Story

FFT Library

Algorithms

Code

C main file

Project file zip

Credits

Benjamin Kofman

Comments

Related channels and tags