Published May 20, 2021

Waltzing robot

Segbot that dances to music and recognizes different notes from a song and reacts to their notes.

BeginnerShowcase (no instructions)165

Things used in this project

Hardware components

Texas Instruments LAUNCHXL-F28379D C2000 Delfino LaunchPad

Breakout Board - UIUC SE 423

SparkFun Electret Microphone Breakout

SparkFun Triple Axis Accelerometer and Gyro Breakout - MPU-6050

Hand tools and fabrication machines

3D Printer (generic)

Story

The goal of this project was to create a robot that balances on two wheels and can dance to the beat of the music. This Sedbot consists of a circuit board that was provided by SE 423 class at UIUC, a microcontroller, TMS320f28379D, that was donated by Texas instruments, Gyroscope and accelerometer, microphone, and a set of LED lights.

The overall goal of this project was to create a robot that can remember a dance routine when a song is played by reacting to the different tones of the song. In addition to the robot moving, LED lights were added to recognize the dance move it was performing, and the joystick was used as an emergency stop.

Balance

In order to get the robot balancing, the code from one of our prior labs was used. This used ePWM5 and ADCA interrupt function to transmit and the SPI to receive the gyro and accelerometer readings from the MPU-9250 Motion Tracking sensor. Once the reading was read, the SPIB interrupt and a Kalman filter were used to read the feedback signals. The filter was used to combine the gyro and accelerometer to make the tilt measurement that does not drift and more responsive.

Steering Controls

Once the balance was controlled, the steering controls were controlled using a steering control law.

Beat Detection

The beat was picked up and was converted into digital signals by the microphone. This microphone was connected to the ePWM 4 in order to trigger ADCA interrupt every 0.1ms. There was a build- in FFT the Texas Instruments library that uses a ping pong buffer that allows the signal to overlap causing the to be read a lot quicker.

Then using the frequency, maxpwrindex, which is scaled between 0-512, and the intensity of the music, maxpwr, which was used to drive the segbot. A combination of both will help eliminate any background noise.

EALLOW;
    EPwm4Regs.ETSEL.bit.SOCAEN = 0; // Disable SOC on A group
    EPwm4Regs.TBCTL.bit.CTRMODE = 3; // freeze counter
    EPwm4Regs.ETSEL.bit.SOCASEL = 2; // Select Event when counter equal to PRD
    EPwm4Regs.ETPS.bit.SOCAPRD = 1; // Generate pulse on 1st event (“pulse” is the same as “trigger”)
    EPwm4Regs.TBCTR = 0x0; // Clear counter
    EPwm4Regs.TBPHS.bit.TBPHS = 0x0000; // Phase is 0
    EPwm4Regs.TBCTL.bit.PHSEN = 0; // Disable phase loading
    EPwm4Regs.TBCTL.bit.CLKDIV = 0; // divide by 1 50Mhz Clock
    //EPwm4Regs.TBPRD = 12500; // Set Period to .25ms sample. Input clock is 50MHz.
    EPwm4Regs.TBPRD = 5000; // Set Period to .10ms 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;

    //2nd step from Ex1
    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);

    //ADCB  code from lab 4
    AdcbRegs.ADCSOC0CTL.bit.CHSEL = 4; //SOC0 will convert Channel you choose Does not have to be B0
    AdcbRegs.ADCSOC0CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    AdcbRegs.ADCSOC0CTL.bit.TRIGSEL = 11;// EPWM4 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 = 11;// EPWM4 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 = 12;// EPWM4 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 = 12;// EPWM4 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


    //3rd step from Ex1
    // 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 referene 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;
int16_t i = 0;
    float samplePeriod = 0.0002;

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

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

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

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

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

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

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

    for(i=0; i<512;i++){
        histogram[i] = 0;
    }

Dancing Moves

Using the steering controls that were set up earlier, turn rate and Forward/Backwards offset were set to sine equations C+A*sin(f*Pi*t). Taking the C, A, f variables where changes in multiple if statements when a specific frequency was played. Combining this with time delays, so the right move was preformed at the right part of the song. The segbot had 9 dance moves that could be triggered with specific range frequencies and sound intensity. In addition, each dance move had a set of LED linked so that would show which move the robot was doing. This is further explained in the senor video.

https://drive.google.com/file/d/1_u22lvwFOPzmmquoDZxQ1qQX7VOp1MAt/view?usp=drivesdk

Schematics

Code

Code

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

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



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

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

void setupSpib(void);
void setDACA(float dacouta0);
void init_eQEPs(void);

float readEncRight(void);
float readEncLeft(void);

void serialRXA(serial_t *s, char data);
void setDACA(float dacouta0);
void setDACB(float dacouta1);


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

//Code for Project
int16_t runpong = 0;
int16_t runping = 0;
int16_t pingpong = 0; //fill ping when pingpong = 0, fill pong when pingpong = 1
int16_t countpingpong = 0;
int32_t histogram[512];

// code from lab 4
int16_t  adcb0result = 0;
int16_t  adcb1result = 0;
float ADCB0volt = 0;
int32_t countADCB = 0;
int32_t countFIRB = 0;

//Code from lab 7
//Lab 7 Ex 1(Code from lab 4)
float ADCA1volt = 0;
int32_t countADCA = 0;
int32_t countFIRA = 0;

float ADCA0volt = 0;
int16_t  adca0result = 0;
int16_t  adca1result = 0;


//Lab 7 Ex 1(Code from lab 5)
uint16_t updown = 0;
float spivaluevolt1 =0;
float spivaluevolt2 =0;
uint16_t RCSERVO1 =0;
uint16_t RCSERVO2 =0;
int16_t dummy= 0;
int16_t accelXraw = 0;
int16_t accelYraw = 0;
int16_t accelZraw = 0;
float accelx = 0;
float accely = 0;
float accelz = 0;
int16_t gyroXraw = 0;
int16_t gyroYraw = 0;
int16_t gyroZraw = 0;
float gyrox = 0;
float gyroy = 0;
float gyroz = 0;
int16_t Tempraw = 0;
float Tempreading = 0;
//


// Lab 7 Ex 1(lab 6 code)
float LeftWheel = 0;
float RightWheel = 0;

float Leftdistance = 0;
float Rightdistance = 0;

float uLeft = 5.0;
float uRight = 5.0;

float PosRight_K_1 = 0;
float PosLeft_K_1 = 0;

float VLeftK = 0;
float VRightK = 0;

float ErrorLeft = 0;
float ErrorRight = 0;

float Vref = 0;
float VelRight =0;
float VelLeft =0;

float ILeftk = 0;
float IRightk = 0;
float ILeftk_1 = 0;
float IRightk_1 = 0;

float Kp = 3;
float Ki = 25;

float ErrorLeftk_1 = 0;
float ErrorRightk_1 = 0;
float KRightk = 0;
float KLeftk = 0;
float turn = 0;
float eturn = 0;
float kturn = 3;

float Psi_R = 0;
float R_Wh = 0.10625;
float W_R = 0.62;
float O_r = 0;
float O_l =0;
float O_avg = 0;
float O_avgD = 0;
float O_rD = 0;
float O_lD = 0;
float X_RD = 0;
float Y_RD = 0;
float IX_RD = 0;
float IX_RD_1 = 0;
float IY_RD = 0;
float IY_RD_1 = 0;
float O_avgD_1 = 0;
float X_RD_1 =0;
float Y_RD_1 =0;
float LeftWheel_1 = 0;

float RightWheel_1 =0;
float rawright = 0;
float Vleftrad =0;
float Vrightrad =0;
float O_avgthea = 0;
float Vrawleft = 0;
float O_avgvelo = 0;
//end

float LeftAngle = 0;
float RightAngle = 0;
int32_t CountTimerVar = 0;

//Lab 7 Ex 2
// 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 = -.76;
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 LeftWheel1 = 0;
float RightWheel1 = 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;
//end code

//lab 7 ex3
float Ubal = 0.0;
float VelR_K1 = 0.0;
float VelL_K1 = 0.0;
float VelR_k = 0.0;
float VelL_k = 0.0;

float ThetaR_K1 =0.0;
float ThetaL_K1 =0.0;
float ThetaL =0.0;
float ThetaR =0.0;
float K1 = -60;
float K2 = -4.5;
float K3 = -1.1;

float ULeft = 0;
float URight = 0;
//end

//lab 7 ex 4
float WhlDiff = 0.0;
float WhlDiff_1 = 0.0;
float velWhlDiff = 0.0;
float velWhlDiff_1 = 0.0;
float turnref = 0.0;
float turnrate = 0.0;
float errorDiff = 0.0;
float errorDiff_1 = 0.0;
float intDiff = 0.0;
float intDiff_1 = 0.0;
float turnref_1 = 0.0;
float turnrate_1 = 0.0;
float FwdBackOffset = 0.0;
float Kd = 0.0;

float t = 0.0;
float C1 = 0;
float A1 =0;
float f1 =3;

float C2 = 0;
float A2 =0;
float f2 =1;
float state21count = 0;
float  state21delaytime =0;
uint16_t State = 1;
uint16_t countA = 0;
uint16_t state = 10;
int16_t TimeCount = 0;
int16_t TimeDely = 0;
//end
//End code from lab 7


//code from lab 4
void setDACA(float dacouta0) {
    if (dacouta0 > 3.0) dacouta0 = 3.0;
    if (dacouta0 < 0.0) dacouta0 = 0.0;
    DacaRegs.DACVALS.bit.DACVALS = dacouta0*1365.0; // 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 = dacouta1*1365.0; // perform scaling of 0-3 to 0-4095
}


//Code from lab 7
//Lab 7 Ex 1(Code from lab 4)- Setting up the joystick
__interrupt void ADCA_isr (void)
{
    countADCA ++;
    adca0result = AdcaResultRegs.ADCRESULT0;
    adca1result = AdcaResultRegs.ADCRESULT1;

    // Here covert ADCIND0, ADCIND1 to volts
    ADCA0volt= adca0result/1365.0; // perform scaling of 0-3 to 0-4095
    ADCA1volt= adca1result/1365.0; // perform scaling of 0-3 to 0-4095

    // Print ADCIND0 and ADCIND1’s voltage value to TeraTerm every 100ms
    //    if ((countFIRA % 100) == 0) {
    //        UARTPrint = 1;
    //    }

    //Code from Lab 5
    GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;
    SpibRegs.SPIFFRX.bit.RXFFIL = 8;
    SpibRegs.SPITXBUF = ((0x8000) | (0x3A00));
    SpibRegs.SPITXBUF = 0;
    SpibRegs.SPITXBUF = 0;
    SpibRegs.SPITXBUF = 0;
    SpibRegs.SPITXBUF = 0;
    SpibRegs.SPITXBUF = 0;
    SpibRegs.SPITXBUF = 0;
    SpibRegs.SPITXBUF = 0;

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

    /// end of code
}

//Lab 7 Ex 1(Code from lab 6)- Setting up the accel/Gyro X,Y,Z
__interrupt void SPIB_isr(void){

    //    GpioDataRegs.GPCSET.bit.GPIO66 = 1;
    dummy = SpibRegs.SPIRXBUF;
    accelXraw = SpibRegs.SPIRXBUF;
    accelYraw = SpibRegs.SPIRXBUF;
    accelZraw = SpibRegs.SPIRXBUF;
    Tempraw = SpibRegs.SPIRXBUF;
    gyroXraw = SpibRegs.SPIRXBUF;
    gyroYraw = SpibRegs.SPIRXBUF;
    gyroZraw = SpibRegs.SPIRXBUF;

    GpioDataRegs.GPCSET.bit.GPIO66 = 1;

    accelx = accelXraw*4.0/32767.0;
    accely = accelYraw*4.0/32767.0;
    accelz = accelZraw*4.0/32767.0;
    Tempreading = Tempraw;
    gyrox = gyroXraw*250.0/32767.0;
    gyroy = gyroYraw*250.0/32767.0;
    gyroz = gyroZraw*250.0/32767.0;


    LeftAngle = -readEncLeft();
    RightAngle = readEncRight();

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

    if (uLeft > 10 || uLeft < -10){
        ILeftk = ILeftk_1;
    }

    if (uRight > 10 || uRight < -10){
        IRightk = IRightk_1;
    }

    CountTimerVar++;
    if ((CountTimerVar %200) == 0) {
        UARTPrint = 1;
    }

    ////Lab 7 Ex 2- setting up the Kalman Filter, This code was given to us.
    // This could allows the tilt and Gyro value read zero at any angle the
    //Code to be copied into SPIB_ISR interrupt function after the IMU measurements have been collected.
    // Need to change some of the variables so they could match the the Gyros and Acclc we calcuated
    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
    }
    SpibRegs.SPIFFRX.bit.RXFFOVFCLR=1; // Clear Overflow flag
    SpibRegs.SPIFFRX.bit.RXFFINTCLR=1; // Clear Interrupt flag
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP6;
}
//end of Lab 7 Ex 2
//End of code from lab 7!


__interrupt void ADCB_isr (void)
{
    countADCB ++;
    adcb0result = AdcbResultRegs.ADCRESULT0;
    // Here covert ADCIND0, ADCIND1 to volts
    ADCB0volt= adcb0result/1365.0; // perform scaling of 0-3 to 0-4095

    if (pingpong == 0){
        ping_input[countpingpong] = ADCB0volt;
        countpingpong ++;
        if(countpingpong >= 1024){
            countpingpong = 0;
            pingpong = 1;
            runping = 1;
        }
    }
    else{
        pong_input[countpingpong] = ADCB0volt;
        countpingpong ++;
        if(countpingpong >= 1024){
            countpingpong = 0;
            pingpong = 0;
            runpong = 1;
        }
    }


    // Here write values to DAC channels
    setDACA(ADCB0volt);

    // Print ADCIND0 and ADCIND1’s voltage value to TeraTerm every 100ms
    if ((countADCB % 1000) == 0) {
        UARTPrint = 1;
    }

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


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.SPIB_RX_INT = &SPIB_isr; // Lab 7 ex 1 (Lab 6)
    PieVectTable.ADCB1_INT = &ADCB_isr;
    PieVectTable.ADCA1_INT = &ADCA_isr;// Lab 7 ex 1 (Lab 4)
    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.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, 1000000);
    ConfigCpuTimer(&CpuTimer1, 200, 20000);
    ConfigCpuTimer(&CpuTimer2, 200, 10000);

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


    setupSpib();
    // Lab 7 ex 1 (Lab 4 for the joystick)
    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 = 12500; // Set Period to .25ms sample. Input clock is 50MHz.
    EPwm5Regs.TBPRD = 50000; // Set Period to .10ms 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;

    //2nd step from Ex1
    EALLOW;
    //write configurations for all ADCs ADCA, ADCB, ADCC, ADCD
    AdcaRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
    AdcSetMode(ADC_ADCA, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE); //read calibration settings
    //Set pulse positions to late
    AdcaRegs.ADCCTL1.bit.INTPULSEPOS = 1;

    //power up the ADCs
    AdcaRegs.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 = 13;// EPWM5 ADCSOCA or another trigger you choose will trigger SOC0
    AdcaRegs.ADCSOC1CTL.bit.CHSEL = 3; //SOC1 will convert Channel you choose Does not have to be A1
    AdcaRegs.ADCSOC1CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    AdcaRegs.ADCSOC1CTL.bit.TRIGSEL = 13;// EPWM5 ADCSOCA or another trigger you choose will trigger SOC1
    AdcaRegs.ADCINTSEL1N2.bit.INT1SEL = 1; //set to last SOC that is converted and it will set INT1 flag ADCA1
    AdcaRegs.ADCINTSEL1N2.bit.INT1E = 1; //enable INT1 flag
    AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //make sure INT1 flag is cleared

    //3rd step from Ex1
    // 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;

    // Use this code from lab 4 and change the EPwm5Regs to EPWm4Regs, This code was give by el Profe.
    // This Regs will be asign to ADCB and will be at 10 kH.
    EALLOW;
    EPwm4Regs.ETSEL.bit.SOCAEN = 0; // Disable SOC on A group
    EPwm4Regs.TBCTL.bit.CTRMODE = 3; // freeze counter
    EPwm4Regs.ETSEL.bit.SOCASEL = 2; // Select Event when counter equal to PRD
    EPwm4Regs.ETPS.bit.SOCAPRD = 1; // Generate pulse on 1st event (“pulse” is the same as “trigger”)
    EPwm4Regs.TBCTR = 0x0; // Clear counter
    EPwm4Regs.TBPHS.bit.TBPHS = 0x0000; // Phase is 0
    EPwm4Regs.TBCTL.bit.PHSEN = 0; // Disable phase loading
    EPwm4Regs.TBCTL.bit.CLKDIV = 0; // divide by 1 50Mhz Clock
    //EPwm4Regs.TBPRD = 12500; // Set Period to .25ms sample. Input clock is 50MHz.
    EPwm4Regs.TBPRD = 5000; // Set Period to .10ms 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;

    //2nd step from Ex1
    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);

    //ADCB  code from lab 4
    AdcbRegs.ADCSOC0CTL.bit.CHSEL = 4; //SOC0 will convert Channel you choose Does not have to be B0
    AdcbRegs.ADCSOC0CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    AdcbRegs.ADCSOC0CTL.bit.TRIGSEL = 11;// EPWM4 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 = 11;// EPWM4 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 = 12;// EPWM4 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 = 12;// EPWM4 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


    //3rd step from Ex1
    // 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 referene 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;

    //Lab 7 ex 1 (Lab 6 for motors)
    //this is coding the motors
    // A is the right motor, and B is the left motor
    EPwm2Regs.TBCTL.bit.CTRMODE = 0; // inituilzation go here towards the end of main function
    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 = 0;
    EPwm2Regs.AQCTLA.bit.CAU = 1;
    EPwm2Regs.AQCTLA.bit.ZRO = 2;

    EPwm2Regs.CMPB.bit.CMPB = 0;
    EPwm2Regs.AQCTLB.bit.CBU = 1;
    EPwm2Regs.AQCTLB.bit.ZRO = 2;

    EPwm2Regs.TBPHS.bit.TBPHS = 0;

    // right motor
    GPIO_SetupPinMux(2, GPIO_MUX_CPU1, 1);
    //    GPIO_SetupPinOptions(22, GPIO_OUTPUT, GPIO_PUSHPULL);
    //    GpioDataRegs.GPACLEAR.bit.GPIO22 = 1


    // left motor
    GPIO_SetupPinMux(3, GPIO_MUX_CPU1, 1);
    //    GPIO_SetupPinOptions(22, GPIO_OUTPUT, GPIO_PUSHPULL);
    //    GpioDataRegs.GPACLEAR.bit.GPIO22 = 1

    // RC Servo Motor 1 PWM
    GPIO_SetupPinMux(14, GPIO_MUX_CPU1, 1);

    // RC Servo Motor 2 PWM
    GPIO_SetupPinMux(15, GPIO_MUX_CPU1, 1);

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

    init_eQEPs();
    //end of Lab 7 ex 1 (Lab 6 for motors)


    // 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;// from lab 5

    // 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;
    // Enable TINT0 in the PIE: Group 1 interrupt 2
    PieCtrlRegs.PIEIER1.bit.INTx2 = 1;//ADCB1 - Lab 4
    // Enable SWI in the PIE: Group 6 interrupt 3
    PieCtrlRegs.PIEIER6.bit.INTx3 = 1;
    // Enable TINT0 in the PIE: Group 1 interrupt 1
    PieCtrlRegs.PIEIER1.bit.INTx1 = 1;//ADCA1 - Lab 4


    int16_t i = 0;
    float samplePeriod = 0.0002;

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

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

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

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

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

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

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

    for(i=0; i<512;i++){
...

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

Credits

Paloma Alcala

1 project • 0 followers

Waltzing robot

Things used in this project

Hardware components

Hand tools and fabrication machines

Story

Balance

Steering Controls

Beat Detection

Dancing Moves

Custom parts and enclosures

Motor mounts

supporting legs

Schematics

Balance and Steering Controls

Code

Code

Credits

Paloma Alcala

Comments

Embed the widget on your own site

Waltzing robot

Waltzing robot

Things used in this project

Hardware components

Hand tools and fabrication machines

Story

Balance

Steering Controls

Beat Detection

Dancing Moves

Custom parts and enclosures

Motor mounts

supporting legs

Schematics

Balance and Steering Controls

Code

Code

Credits

Paloma Alcala

Comments

Related channels and tags