This project was created in SE 423 - Mechatronics at the University of Illinois at Urbana-Champaign. This course uses a robot featuring the TI C2000 family of microprocessors with the F28379D Launchpad board. This project's main sensor is a microphone, which it uses to pick up signals that are then processed by a fast Fourier Transform (FFT). Different frequencies can be used to control the robot in unique ways. The program also features beat detection software that dances in response to the beat of a song. Movement is based on detection of the eQEP peripheral of the TMS320F28379D processor. Coupled and decoupled controllers are used that control speed and allow for forward, backward, and rotational movement. The project is described in more detail below.
The TI FFT library finds the peak of a frequency such that one specific note can be easily identified. For example, a frequency of 392 is a G4 note. If the microphone picks up this frequency, the FFT is able to identify the note and the program tells the velocity of the wheel to increase. The program also uses the notes F4, E4, D4, and C4 to control the robot's motors. These notes are required to be played in order for the dance sequence to begin. More information on the FFT can be found here: https://www.dataq.com/data-acquisition/general-education-tutorials/fft-fast-fourier-transform-waveform-analysis.html
The dance sequence detects strong amplitudes of the song based around the FFT reading. The dance reads specific amplitudes over 170 and between frequencies 16 and 19 (inclusive). This may have to be tuned for the specific song and music volume. Due to the FFT reading several different amplitudes at a time, a slight delay is used so that the dancing does not occur in a rapid sequence. While the dance sequence is not in delay, the robot will sway back and forth for each beat.
//#############################################################################
// FILE: labstarter_main.c
//
// TITLE: Lab Starter
//#############################################################################
// Included Files
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <string.h>
#include <math.h>
#include <limits.h>
#include "F28x_Project.h"
#include "driverlib.h"
#include "device.h"
#include "f28379dSerial.h"
#include "dsp.h"
#include "fpu32/fpu_rfft.h"
#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(pong_arr, "FFT_buffer_1")
#endif
float pong_arr[RFFT_SIZE];
#ifdef __cplusplus
#pragma DATA_SECTION("FFT_buffer_1")
#else
#pragma DATA_SECTION(ping_arr, "FFT_buffer_1")
#endif
float ping_arr[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);
void serialRXA(serial_t *s, char data);
void setEPWM2A(float controleffort);
void setEPWM2B(float controleffort);
void init_eQEPs(void);
float readEncLeft(void);
float readEncRight(void);
// Count variables
uint32_t numTimer0calls = 0;
uint32_t numSWIcalls = 0;
uint32_t numRXA = 0;
uint16_t UARTPrint = 0;
uint16_t adcb0result = 0;
float adcb0result_volts = 0;
int32_t count = 0;
int32_t runping = 0;
int32_t runpong = 0;
int ping = 1;
int pong = 0;
int16_t freq = 0;
int note = 1;
float sequence1 = 0;
float sequence2 = 0;
float sequence3 = 0;
float sequence4 = 0;
float sequence5 = 0;
int beat = 0;
float song_cutoff = 170;
int cutoff = 0;
int fftcount = 0;
int32_t histogram[200];
int32_t timestamp[200];
int delayCount = 0;
int inDelay = 0;
int delayTime = 0;
int countDelay = 0;
int danceCount = 0;
int timer = 0;
int song = 0;
//variables for lab 6
float rightWheel = 0;
float leftWheel = 0;
float rightDist = 0;
float leftDist = 0;
float uLeft = 0;
float uRight = 0;
float velLeft = 0;
float leftDistK_1 = 0;
float velRight = 0;
float rightDistK_1 = 0;
float Kp = 3;
float Ki = 25;
float Vref = 0;
float errorLeft = 0;
float errorRightK_1 = 0;
float errorRight = 0;
float errorLeftK_1 = 0;
float iLeftK_1 = 0;
float iRightK_1 = 0;
float turn = 0;
float errorTurn = 0;
float Kturn = 3;
float robotWidth = 0.61;
float wheelRadius = .10625;
float poseAngle = 0;
float poseX = 0;
float poseY = 0;
float avgAngle = 0;
float rightWheelK_1 = 0;
float leftWheelK_1 = 0;
float avgAngle_dot = 0;
float rightWheel_dot = 0;
float leftWheel_dot = 0;
float xr_dot = 0;
float yr_dot = 0;
float xr = 0;
float yr = 0;
float xrK_1 = 0;
float yrK_1 = 0;
float xr_dotK_1;
float yr_dotK_1;
void main(void)
{
// PLL, WatchDog, enable Peripheral Clocks
// This example function is found in the F2837xD_SysCtrl.c file.
InitSysCtrl();
InitGpio();
GPIO_SetupPinMux(2, GPIO_MUX_CPU1, 1);
GPIO_SetupPinOptions(2, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPASET.bit.GPIO2 = 1;
GPIO_SetupPinMux(3, GPIO_MUX_CPU1, 1);
GPIO_SetupPinOptions(3, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPASET.bit.GPIO3 = 1;
// 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.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, 4000);
ConfigCpuTimer(&CpuTimer2, 200, 40000);
// 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);
int j = 0;
for (j = 0; j < 200; j++) {
histogram[j] = 0;
timestamp[j] = 0;
}
EPwm2Regs.TBCTL.bit.FREE_SOFT = 0x3;
EPwm2Regs.TBCTL.bit.CTRMODE = 0x0;
EPwm2Regs.TBCTL.bit.CLKDIV = 0x0;
EPwm2Regs.TBCTL.bit.PHSEN = 0;
EPwm2Regs.TBCTR = 0x00;
EPwm2Regs.TBPRD = 2500;
EPwm2Regs.CMPA.bit.CMPA = 0;
EPwm2Regs.AQCTLA.bit.CAU = 0x1;
EPwm2Regs.AQCTLA.bit.ZRO = 0x2;
EPwm2Regs.TBPHS.bit.TBPHS = 0;
EPwm2Regs.CMPB.bit.CMPB = 0;
EPwm2Regs.AQCTLB.bit.CBU = 0x1;
EPwm2Regs.AQCTLB.bit.ZRO = 0x2;
EALLOW;
EPwm5Regs.ETSEL.bit.SOCAEN = 0; // Disable SOC on A group
EPwm5Regs.TBCTL.bit.CTRMODE = 3; // freeze counter
EPwm5Regs.ETSEL.bit.SOCASEL = 0x2; // Select Event when counter equal to PRD
EPwm5Regs.ETPS.bit.SOCAPRD = 0x1; // 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 = 5000; // Set Period to 1ms sample. Input clock is 50MHz. 50000 hz for ex 1 2 3 12500 for ex 4 and then 5000
// Notice here that we are not setting CMPA or CMPB because we are not using the PWM signal
EPwm5Regs.ETSEL.bit.SOCAEN = 1; //enable SOCA
EPwm5Regs.TBCTL.bit.CTRMODE = 0; //unfreeze, and enter up count mode
EDIS;
EALLOW;
//write configurations for all ADCs ADCA, ADCB, ADCC, ADCD
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);
// SE423, Lab 4 Page 4 of 11
// 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 = 0xD;// EPWM5 ADCSOCA or another trigger you choose will trigger SOC0
// AdcbRegs.ADCSOC1CTL.bit.CHSEL = 1; //SOC1 will convert Channel you choose Does not have to be B1
// AdcbRegs.ADCSOC1CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
// AdcbRegs.ADCSOC1CTL.bit.TRIGSEL = 0xD;// EPWM5 ADCSOCA or another trigger you choose will trigger SOC1
// AdcbRegs.ADCSOC2CTL.bit.CHSEL = 2; //SOC2 will convert Channel you choose Does not have to be B2
// AdcbRegs.ADCSOC2CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
// AdcbRegs.ADCSOC2CTL.bit.TRIGSEL = 0xD;// EPWM5 ADCSOCA or another trigger you choose will trigger SOC2
// AdcbRegs.ADCSOC3CTL.bit.CHSEL = 3; //SOC3 will convert Channel you choose Does not have to be B3
// AdcbRegs.ADCSOC3CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
// AdcbRegs.ADCSOC3CTL.bit.TRIGSEL = 0xD;// 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 = 2; //set SOC2 to convert pin D2
// AdcdRegs.ADCSOC2CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
// AdcdRegs.ADCSOC2CTL.bit.TRIGSEL = 0xD; // EPWM5 ADCSOCA will trigger SOC2
// AdcdRegs.ADCSOC3CTL.bit.CHSEL = 3; //set SOC3 to convert pin D3
// AdcdRegs.ADCSOC3CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
// AdcdRegs.ADCSOC3CTL.bit.TRIGSEL = 0xD; // 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;
init_eQEPs();
// 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;
// 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.PIEIER1.bit.INTx2 = 1;
int16_t i = 0;
float samplePeriod = 0.0002;
// Clear input buffers:
for(i=0; i < RFFT_SIZE; i++){
ping_arr[i] = 0.0f;
}
for (i=0;i<RFFT_SIZE;i++) {
ping_arr[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_arr[0]; //Input buffer
hnd_rfft->OutBuf = &test_output[0]; //Output buffer
hnd_rfft->MagBuf = &pwrSpec[0]; //Magnitude buffer
hnd_rfft->CosSinBuf = &RFFTF32Coef[0]; //Twiddle factor buffer
RFFT_f32_sincostable(hnd_rfft); //Calculate twiddle factor
for (i=0; i < RFFT_SIZE; i++){
test_output[i] = 0; //Clean up output buffer
}
for (i=0; i <= RFFT_SIZE/2; i++){
pwrSpec[i] = 0; //Clean up magnitude buffer
}
int16_t tries = 0;
while(tries < 10) {
hnd_rfft->InBuf = &ping_arr[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_arr[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
// IDLE loop. Just sit and loop forever (optional):
// IDLE loop. Just sit and loop forever (optional):
//Within the while loop is where the FFT runs and the frequency/beat detection occurs.
//maxpwr is the identification of the amplitude of the FFT. This is used for beat detection to determine the stronger
//sounds that should be read. maxpwrindex is the index of that amplitude and is directly proportional to frequency
//using the Nyquist frequency. The program alternates between variables "ping" and "pong" as one is read and
//the other performs the FFT. More comments can be found before the frequency and beat detection software.
while(1)
{
if (UARTPrint == 1 ) {
// serial_printf(&SerialA,"Number CPU2 Timer interrupt Calls %ld \r\n",CpuTimer2.InterruptCount);
serial_printf(&SerialA,"frequency: %d index: %d maxpwr: %.3f \r\n",freq, maxpwrindex, maxpwr);
if (note == 1) {
serial_printf(&SerialA,"This note is a G4! \r\n");
}
if (note == 2) {
serial_printf(&SerialA,"This note is an F4! \r\n");
}
if (note == 3) {
serial_printf(&SerialA,"This note is a E4! \r\n");
}
if (note == 4) {
serial_printf(&SerialA,"This note is a D4! \r\n");
}
if (note == 5) {
serial_printf(&SerialA,"This note is a C4! \r\n");
}
note = 0;
UARTPrint = 0;
}
if (runping == 1) {
hnd_rfft -> InBuf = &ping_arr[0]; //Input buffer
}
if (runpong == 1) {
hnd_rfft -> InBuf = &pong_arr[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 = 5; i < (RFFT_SIZE/2); i++) {
for (i = 3; i < 40; i++) {
if (pwrSpec[i] > maxpwr) {
maxpwr = pwrSpec[i];
maxpwrindex = i;
}
}
//This is the frequency detection software, in which five specific notes call the control algorithm
//cputimer1. Each note corresponds to a certain frequency and the detected note will be printed in the
//above print statement. If each note is detected, the dance sequence activates. See below comments for more.
freq = 5000.0/511.0*maxpwrindex; //scale index to frequency using Nyquist frequency
if ((maxpwr > cutoff) && (song == 0)) {
//note found
if ((freq > 380) && (freq < 400)) { //note 392
//this note is a G4
note = 1;
Vref = Vref + 0.05;
sequence1 = 1;
}
if ((freq > 345) && (freq < 355)) { //note 349
//this note is an F4
note = 2;
Vref = Vref - 0.05;
sequence2 = 1;
}
if ((freq > 325) && (freq < 335)) { //note 329
//this note is an E4
note = 3; //7th scale
turn = turn + 0.05;
Vref = 0;
sequence3 = 1;
}
if ((freq > 290) && (freq < 295)) { //note 293
//this note is a D4
turn = turn - 0.05;
Vref = 0;
note = 4; //7th scale
sequence4 = 1;
}
if ((freq > 255) && (freq < 270)) { //note 261 - C4
//this will cancel the robot's movement
Vref = 0;
turn = 0;
sequence5 = 1;
note = 5;
}
}
UARTPrint = 1;
//This is the beat detection sequence. For the beat to be detected, a cutoff of the maxpwr is used
//so that only the stronger beats can be detected. This must be tuned for different songs and volumes.
//A histogram was used to understand where the beats were showing up along the maxpwrindex axis. They were
//determined to be within the range from 16 to 19 (inclusive). If a beat is detected in this range and not in
//delay, a dance rotation will occur. The dancing rotates back and force every other count. A delay is used
//because many different readings occur at once. The delay time may have to be tuned for each individual
//song depending on how long the beats occur.
if ((sequence1 == 1) && (sequence2 == 1) && (sequence3 == 1) && (sequence4 == 1) && (sequence5 == 1)) {
//activate song sequence and turn off note checking
song = 1;
if ((maxpwr > song_cutoff)) {
histogram[maxpwrindex]++;
timestamp[fftcount % 100] = fftcount;
fftcount++;
if ((maxpwrindex >= 16) && (maxpwrindex <= 19) && (inDelay == 0)) {
inDelay = 1;
countDelay = 0;
delayTime = 10;
//activate dance sequence
if ((danceCount % 2) == 0) {
Vref = .5;
turn = 1;
}
else if ((danceCount % 2) == 1) {
Vref = -.5;
turn = 1;
}
danceCount++;
}
}
if (inDelay == 1) {
countDelay++;
if (countDelay == delayTime) {
inDelay = 0;
}
}
}
}
}
}
// SWI_isr, Using this interrupt as a Software started interrupt
__interrupt void SWI_isr(void) {
// These three lines of code allow SWI_isr, to be interrupted by other interrupt functions
// making it lower priority than all other Hardware interrupts.
PieCtrlRegs.PIEACK.all = PIEACK_GROUP12;
asm(" NOP"); // Wait one cycle
EINT; // Clear INTM to enable interrupts
// Insert SWI ISR Code here.......
numSWIcalls++;
// Blink a number of LEDS
// GpioDataRegs.GPATOGGLE.bit.GPIO27 = 1;
// GpioDataRegs.GPBTOGGLE.bit.GPIO60 = 1;
// GpioDataRegs.GPBTOGGLE.bit.GPIO61 = 1;
// GpioDataRegs.GPETOGGLE.bit.GPIO157 = 1;
// GpioDataRegs.GPETOGGLE.bit.GPIO158 = 1;
DINT;
}
// 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
}
// Blink LaunchPad Red LED
GpioDataRegs.GPBTOGGLE.bit.GPIO34 = 1;
// Blink a number of LEDS
// GpioDataRegs.GPATOGGLE.bit.GPIO7 = 1;
// GpioDataRegs.GPATOGGLE.bit.GPIO8 = 1;
// GpioDataRegs.GPATOGGLE.bit.GPIO9 = 1;
// GpioDataRegs.GPATOGGLE.bit.GPIO24 = 1;
// GpioDataRegs.GPATOGGLE.bit.GPIO25 = 1;
// GpioDataRegs.GPATOGGLE.bit.GPIO26 = 1;
// Acknowledge this interrupt to receive more interrupts from group 1
PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
}
//This function accesses the eQEP peripheral of the TMS320F28379D processor
//to calculate robot car velocity and implement coupled and decoupled
//controllers that control speed and allow for forward, backward, and rotational movement.
//Using the block diagrams from the lab documentation, pose bearing, x coordinates, and y coordinates
//can be found using the trapezoidal rule. This function follows the steps of a control system closely,
//including the reading of the sensor data, the error calculation, the control effort, and the saving of states.
// cpu_timer1_isr - CPU Timer1 ISR
__interrupt void cpu_timer1_isr(void)
{
//read feedback - quadrature encoder
rightWheel = readEncRight(); //diameter of wheel is 2.5 and 12 inches in 1 foot
leftWheel = -readEncLeft();
rightDist = (1.0/9.7) * rightWheel;
leftDist = (1.0/9.7) * leftWheel;
errorTurn = turn + (velLeft - velRight);
velLeft = (leftDist - leftDistK_1)/.004;
errorLeft = Vref - velLeft - Kturn*errorTurn;
// errorLeft = Vref - velLeft;
float iLeftK = iLeftK_1 + ((errorLeft + errorLeftK_1)/2.0*.004);
velRight = (rightDist - rightDistK_1)/.004;
// errorRight = Vref - velRight;
errorRight = Vref - velRight + Kturn*errorTurn;
float iRightK = iRightK_1 + ((errorRight + errorRightK_1)/2.0*.004);
//Code for Exercise 5
poseAngle = (wheelRadius/robotWidth)*(rightWheel - leftWheel);
avgAngle = .50*(rightWheel + leftWheel);
rightWheel_dot = (rightWheel - rightWheelK_1)/.004;
leftWheel_dot = (leftWheel - leftWheelK_1)/.004;
avgAngle_dot = .50*(rightWheel_dot + leftWheel_dot);
xr_dot = wheelRadius*avgAngle_dot*cos(poseAngle);
yr_dot = wheelRadius*avgAngle_dot*sin(poseAngle);
xr = xrK_1 + ((xr_dot + xr_dotK_1)/2.0*.004);
yr = yrK_1 + ((yr_dot + yr_dotK_1)/2.0*.004);
// calculate control effort
uLeft = Kp * errorLeft + Ki*iLeftK;
uRight = Kp * errorRight + Ki*iRightK;
if (fabs(uLeft) >= 10) {
iLeftK = iLeftK_1;
}
if (fabs(uRight) >= 10) {
iRightK = iRightK_1;
}
setEPWM2B(-uLeft);
setEPWM2A(uRight); //left or right
//save past states
leftDistK_1 = leftDist;
errorLeftK_1 = errorLeft;
iLeftK_1 = iLeftK;
rightDistK_1 = rightDist;
errorRightK_1 = errorRight;
iRightK_1 = iRightK;
rightWheelK_1 = rightWheel;
leftWheelK_1 = leftWheel;
xrK_1 = xr;
yrK_1 = yr;
xr_dotK_1 = xr_dot;
yr_dotK_1 = yr_dot;
// Blink a number of LEDS
// 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;
CpuTimer1.InterruptCount++;
}
// cpu_timer2_isr CPU Timer2 ISR
__interrupt void cpu_timer2_isr(void)
{
// Blink LaunchPad Blue LED
GpioDataRegs.GPATOGGLE.bit.GPIO31 = 1;
// Blink a number of LEDS
// GpioDataRegs.GPDTOGGLE.bit.GPIO111 = 1;
// GpioDataRegs.GPETOGGLE.bit.GPIO130 = 1;
// GpioDataRegs.GPETOGGLE.bit.GPIO131 = 1;
// GpioDataRegs.GPATOGGLE.bit.GPIO4 = 1;
// GpioDataRegs.GPATOGGLE.bit.GPIO5 = 1;
// GpioDataRegs.GPATOGGLE.bit.GPIO6 = 1;
CpuTimer2.InterruptCount++;
// if ((CpuTimer2.InterruptCount % 50) == 0) {
// UARTPrint = 1;
// }
}
// This function is called each time a char is recieved over UARTA.
void serialRXA(serial_t *s, char data) {
numRXA ++;
}
//This function works through ADCB to sample the microphone's signal. There are two switch variables
//ping and pong that fill up an array of 1024 each and switch to the other while the FFT runs in the
//main function main loop.
__interrupt void ADCB_ISR (void) {
// GpioDataRegs.GPBSET.bit.GPIO52 = 1;
adcb0result = AdcbResultRegs.ADCRESULT0;
adcb0result_volts = adcb0result*3.0/4095.0;
if (ping == 1) {
ping_arr[count]= adcb0result_volts;
count++;
if (count >= 1024){
ping = 0;
pong = 1;
runping = 1;
count = 0;
}
}
else if (pong == 1) {
pong_arr[count]= adcb0result_volts;
count++;
if (count >= 1024){
pong = 0;
ping = 1;
runpong = 1;
count = 0;
}
}
// if ((count % 100) == 0) {
// UARTPrint = 1;
// }
// GpioDataRegs.GPBCLEAR.bit.GPIO52 = 1;
AdcbRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //clear interrupt flag
PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
}
//Function that saturates parameter controleffort between -10 and 10 and then
//sets the CMPA to determine the duty cycle. The equation converts input between
//-10 and 10 to the range 0 to 2500.
void setEPWM2A(float controleffort) {
if (controleffort > 10){
controleffort = 10;
}
if (controleffort < -10){
controleffort = -10;
}
EPwm2Regs.CMPA.bit.CMPA = controleffort*2500.0/20.0 + 1250.0;
}
//Function that saturates parameter controleffort between -10 and 10 and then
//sets the CMPA to determine the duty cycle. The equation converts input between
//-10 and 10 to the range 0 to 2500.
void setEPWM2B(float controleffort) {
//Function that saturates parameter controleffort between -10 and 10 and then
//sets the CMPB to determine the duty cycle. The equation converts input between
//-10 and 10 to the range 0 to 2500.
if (controleffort > 10){
controleffort = 10;
}
if (controleffort < -10){
controleffort = -10;
}
EPwm2Regs.CMPB.bit.CMPB = controleffort*2500.0/20.0 + 1250.0;
}
//This function sets up SPIB and includes the GPIO initializations.
//The function goes through all the different necessary registers and also transfers the 16-bit values to initialize the MPU-9250 registers.
void init_eQEPs(void) {
// setup eQEP1 pins for input
EALLOW;
//Disable internal pull-up for the selected output pins for reduced power consumption
GpioCtrlRegs.GPAPUD.bit.GPIO20 = 1; // Disable pull-up on GPIO20 (EQEP1A)
GpioCtrlRegs.GPAPUD.bit.GPIO21 = 1; // Disable pull-up on GPIO21 (EQEP1B)
GpioCtrlRegs.GPAQSEL2.bit.GPIO20 = 2; // Qual every 6 samples
GpioCtrlRegs.GPAQSEL2.bit.GPIO21 = 2; // Qual every 6 samples
EDIS;
// This specifies which of the possible GPIO pins will be EQEP1 functional pins.
// Comment out other unwanted lines.
GPIO_SetupPinMux(20, GPIO_MUX_CPU1, 1);
GPIO_SetupPinMux(21, GPIO_MUX_CPU1, 1);
EQep1Regs.QEPCTL.bit.QPEN = 0; // make sure eqep in reset
EQep1Regs.QDECCTL.bit.QSRC = 0; // Quadrature count mode
EQep1Regs.QPOSCTL.all = 0x0; // Disable eQep Position Compare
EQep1Regs.QCAPCTL.all = 0x0; // Disable eQep Capture
EQep1Regs.QEINT.all = 0x0; // Disable all eQep interrupts
EQep1Regs.QPOSMAX = 0xFFFFFFFF; // use full range of the 32 bit count
EQep1Regs.QEPCTL.bit.FREE_SOFT = 2; // EQep uneffected by emulation suspend in Code Composer
EQep1Regs.QPOSCNT = 0;
EQep1Regs.QEPCTL.bit.QPEN = 1; // Enable EQep
// setup QEP2 pins for input
EALLOW;
//Disable internal pull-up for the selected output pinsfor reduced power consumption
...
This file has been truncated, please download it to see its full contents.
Comments
Please log in or sign up to comment.