//#############################################################################
// FILE: FinalProject_main.c
//
// TITLE: Balancing Segbot
//#############################################################################
// Included Files
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <string.h>
#include <math.h>
#include <limits.h>
#include "F28x_Project.h"
#include "driverlib.h"
#include "device.h"
#include "f28379dSerial.h"
#define PI 3.1415926535897932384626433832795
#define TWOPI 6.283185307179586476925286766559
#define HALFPI 1.5707963267948966192313216916398
#define n_samples 1000 // with a sampling frequency of 10kHz, this runs Goertzl 10x per second with 1000 ADC samples
#define sampling_rate 10000 // 10 kHz
#define NOTE 261.6 // Hz C4
#define NOTE2 466 // Hz Bb4
#define NOTE3 294 // Hz D4
//NOTES FOR SONG
#define C4 262
#define G4 392
#define A3 220
#define G3 196
#define BF3 233 // FLAT
#define F4 349
#define E4 330
#define D4 294
#define F3 174.6
#define E3 165
#define REST 0
#define BF4 466 //FLAT
#define AF4 415 //FLAT
#define F4 349
#define EF4 311 //FLAT
#define DF4 277 //FLAT
#define FS4 370 //SHARP
#define A4 440
#define B4 494
#define C5 523
#define D5 587
#define B3 247
#define songsize 124
#define song2size 28
#define song3size 120
// Interrupt Service Routines predefinition
__interrupt void cpu_timer0_isr(void);
__interrupt void cpu_timer1_isr(void);
__interrupt void cpu_timer2_isr(void);
__interrupt void SWI_isr(void);
__interrupt void ADCA_ISR (void);
__interrupt void ADCB_ISR (void);
__interrupt void SPIB_isr(void);
void serialRXA(serial_t *s, char data);
void setupSpib(void); //lab4 initialize SPIB setup
float readEncLeft(void);//lab5 initialize functions
float readEncRight(void);
void init_eQEPs(void);
void setEPWM6A(float controleffort);
void setEPWM6B(float controleffort);
float goertzel_mag(int numSamples,int TARGET_FREQUENCY,int SAMPLING_RATE, float* data);
//Song
float song[songsize] = {C4,C4,C4,C4, A3,A3,A3,A3, A3,A3,A3,A3, REST,REST,A3,A3, C4,C4,D4,D4, G3,G3,G3,G3,
G3,G3,G3,G3, REST,REST,REST,REST, G3,G3,A3,A3, BF3,BF3,BF3,BF3, F4,F4,F4,F4, F4,F4,F4,F4,
E4,E4,C4,C4, D4,D4,C4,BF3, A3,A3,A3,A3, A3,A3,A3,A3, REST,REST,C4,C4, D4,D4,D4,D4,
REST,REST,D4,D4, G4,F4,F4,E4, E4,F4,D4,D4, C4,C4,C4,C4, REST,REST,REST,REST, F3,F3,G3,G3,
A3,A3,D4,D4, C4,C4,C4,C4, REST,REST,C4,C4, BF3,BF3,A3,A3, E3,E3,E3,E3, F3,F3,F3,F3, F3,F3,F3,F3};
//Dance
float turndance[songsize] = {0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,6,6, -6,-6,6,6, -4,-4,-4,-4,
-4,-4,-4,-4, -4,-4,-4,-4, 6,6,-6,-6, 0,0,0,0, 0,0,0,0, 0,0,6,6,
-6,-6,6,6, -10,-10,-10,-10, -10,-10,-10,-10, -10,-10,-10,-10, -10,-10,6,6, -6,-6,6,6,
-6,-6,10,10, 10,10,10,10, 10,10,10,10, 10,10,10,10, 10,10,10,10, 0,0,0,0,
0,0,-10,-10, -10,-10,-10,-10, -10,-10,6,6, -6,-6,6,6, -6,-6,6,6, -10,-10,-10,-10, -10,-10,-10,-10};
float fwddance[songsize] = {1.5,1.5,1.5,1.5, -0.75,-0.75,-0.75,-0.75, -0.75,-0.75,-0.75,-0.75, -0.75,-0.75,0,0, 0,0,0,0, 0,0,0,0,
0,0,0,0, 0,0,0,0, 0,0,0,0, 1,1,1,1, -0.75,-0.75,-0.75,-0.75, -0.75,-0.75,0,0,
0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0,
0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, .75,.75,-1,-1,
.75,.75,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0};
//Song2
float song2[song2size] = {REST,BF4,BF4,BF4, AF4,AF4,BF4,AF4, AF4,AF4,AF4,AF4, F4,F4,AF4,AF4, F4,F4,F4,F4, EF4,EF4,F4,EF4, EF4,DF4,BF3,BF3};
//Dance2
float turndance2[song2size] = {0,0,0,0, 10,10,10,10, 10,10,10,10, -6,-6,6,6, -6,-6,-6,6, -6,6,-6,6, -6,6,-6,6};
float fwddance2[song2size] = {1,1,1,1, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0};
//Song3
float song3[song3size] ={D4,D4,C4, D4,D4,C4,C4, C4,C4,A3,E4, D4,D4,C4,A3, A3,E4,E4,D4, E4,D4,D4,D4,
D4,D4,B3,G4, E4,D4,D4,B3, B3,D4,D4,B3, D4,D4,C4,C4, C4,C4,A3,E4, D4,C4,C4,A3,
A3,E4,E4,D4, E4,D4,D4,B3, D4,D4,B3,G4, E4,E4,D4,B3, B3,D4,D4,C4, D4,C4,C4,C4,
C4,C4,G4,G4, A4,A4,G4,G4, G4,E4,E4,D4, E4,D4,D4,D4, D4,D4,G4,G4, A4,A4,G4,G4,
A4,A4,B4,B4, C5,C5,C5,C5, C5,C5,C5,C5, B4,B4,D5,D5, B4,A4,A4,A4, A4,A4,G4,G4, G4,G4};
//Dance3
float turndance3[song3size] = {4,4,4 -4,-4,-4,-4, 4,4,4,4, -4,-4,-4,-4, -4,0,0,0, 0,0,0,0,
0,0,0,0, 0,0,0,0, 7,7,7,7, -7,-7,-7,-7, 7,7,7,7, -7,-7,-7,-7,
-7,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,6,-6, 6,-6,6,-6,
6,-10,-10,-10, -10,-10,-10,-10, -10,6,-6,6, -6,6,-6,6, -6,6,-10,-10, -10,-10,-10,-10,
20,20,-20,-20, 6,6,6,6, 6,6,6,6, 0,0,0,0, 0,0,0,0, -6,-6,-6,-6,-6,-6};
float fwddance3[song3size] = {0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,.75,.75,.75, -.75,-.75,-.75,-.75,
.75,.75,.75,.75, -.75,-.75,-.75,-.75, -.75,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0,
0,.75,.75,.75, -.75,-.75,-.75,-.75, .75,.75,.75,.75, -.75,-.75,-.75,-.75, 0,0,0,0, 0,0,0,0,
0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0,
0,0,0,0, 0,0,0,0, 0,0,0,0, -1,-1,-1,-1, 1,1,1,1, 1,-1,-1,-1, 0,0};
// Count variables
uint32_t numTimer0calls = 0;
uint32_t numtimer1calls = 0;
uint32_t numTimer2calls = 0;
uint32_t numSWIcalls = 0;
uint32_t numRXA = 0;
uint16_t UARTPrint = 0;
//lab3 ADCA
int16_t adca0result = 0;
int16_t adca1result = 0;
float adca0Volts = 0;
float adca1Volts = 0;
float adca0Filtered = 0;
float adca1Filtered = 0;
int32_t adcacount = 0;
//lab3 variables for interrupt function
int16_t adcb0result = 0;
int32_t adcbcount = 0;
float adcb_arrayPing[n_samples]= {0};
float adcb_arrayPong[n_samples]= {0};
float adcb_arrayDong[n_samples]= {0};
int16_t PingPong = 0;
float goer_result = 0;
float goer_result2 = 0;
float goer_result3 = 0;
int16_t pwm12prd = 50000000/sampling_rate;
int16_t thresh = 300; //tune this value to detect target frequency
int16_t note_detected = 0;
int16_t note2_detected = 0;
int16_t note3_detected = 0;
int16_t RunPing = 0;
int16_t RunPong = 0;
int16_t RunDong = 0;
//lab3
//This function sets DACA to the voltage between 0V and 3V passed to this function.
//If outside 0V to 3V the output is saturated at 0V to 3V
//Example code
//float myu = 2.25;
//setDACA(myu); // DACA will now output 2.25 Volts
void setDACA(float dacouta0) {
if (dacouta0 > 3.0) dacouta0 = 3.0;
if (dacouta0 < 0.0) dacouta0 = 0.0;
DacaRegs.DACVALS.bit.DACVALS = (int)(dacouta0*4095/3); // perform scaling of 0-3 to 0-4095
}
void setDACB(float dacouta1) {
if (dacouta1 > 3.0) dacouta1 = 3.0;
if (dacouta1 < 0.0) dacouta1 = 0.0;
DacbRegs.DACVALS.bit.DACVALS = (int)(dacouta1*4095/3); // perform scaling of 0-3 to 0-4095
}
//lab4
int16_t updown = 0;
int16_t Gvalue = 0;
float accelx = 0;
float accely = 0;
float accelz = 0;
float gyrox = 0;
float gyroy = 0;
float gyroz = 0;
int16_t accelxraw = 0;
int16_t accelyraw = 0;
int16_t accelzraw = 0;
int16_t gyroxraw = 0;
int16_t gyroyraw = 0;
int16_t gyrozraw = 0;
//lab5
float LeftWheel = 0;
float RightWheel = 0;
float RadFt = 9.45;
float xRight = 0;
float xLeft = 0;
float uLeft = 0;
float uRight = 0;
float xLeft_1 = 0;
float xRight_1 = 0;
float vLeft = 0;
float vRight = 0;
float vRef = .5; //target velocity ft/s
float eR = 0; //error
float eL = 0;
float eR_1 = 0;
float eL_1 = 0;
float I_R = 0;
float I_L = 0;
float I_L1 = 0;
float I_R1 = 0;
float eturn = 0;
float turn = 0;
int16_t SPIcnt = 0;
//lab6
// Needed global Variables
float accelx_offset = 0;
float accely_offset = 0;
float accelz_offset = 0;
float gyrox_offset = 0;
float gyroy_offset = 0;
float gyroz_offset = 0;
//float accelzBalancePoint = -.825; //needs to be fine tuned originally -.76
float accelzBalancePoint = -.8;
int16 IMU_data[9];
uint16_t temp=0;
int16_t doneCal = 0;
float tilt_value = 0;
float tilt_array[4] = {0, 0, 0, 0};
float gyro_value = 0;
float gyro_array[4] = {0, 0, 0, 0};
float LeftWheelArray[4] = {0,0,0,0};
float RightWheelArray[4] = {0,0,0,0};
float WheelDif = 0;
float WheelDif_1 = 0;
float velWheelDif = 0;
float velWheelDif_1 = 0;
float turnref = 0;
float eDif = 0;
float eDif_1 = 0;
float IntDif = 0;
float IntDif_1;
float FwdBkOffset = 0;
float Kp = 3.0; //proportional gain
float Ki = 20.0; //integral gain
float Kd = .08; //derivative gain
float turnrate = 0;
float turnrate_1 = 0;
float turnref_1 = 0;
// Kalman Filter vars
float T = 0.001; //sample rate, 1ms
float Q = 0.01; // made global to enable changing in runtime
float R = 25000;//50000;
float kalman_tilt = 0;
float kalman_P = 22.365;
int16_t SpibNumCalls = -1;
float pred_P = 0;
float kalman_K = 0;
int32_t timecount = 0;
int16_t calibration_state = 0;
int32_t calibration_count = 0;
float vLeft_1 = 0;
float vRight_1 = 0;
float LeftWheel_1 = 0;
float RightWheel_1 = 0;
//simulation gains
float K1 = -25;
float K2 = -2.8;
float K3 = -1.0;
float uBal = 0;
void main(void)
{
// PLL, WatchDog, enable Peripheral Clocks
// This example function is found in the F2837xD_SysCtrl.c file.
InitSysCtrl();
InitGpio();
// Blue LED on LuanchPad
GPIO_SetupPinMux(31, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(31, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPASET.bit.GPIO31 = 1;
// Red LED on LaunchPad
GPIO_SetupPinMux(34, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(34, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPBSET.bit.GPIO34 = 1;
// LED1 and PWM Pin
GPIO_SetupPinMux(22, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(22, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO22 = 1;
// LED2
GPIO_SetupPinMux(52, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(52, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPBCLEAR.bit.GPIO52 = 1;
// LED3
GPIO_SetupPinMux(67, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(67, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPCCLEAR.bit.GPIO67 = 1;
// LED4
GPIO_SetupPinMux(94, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(94, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPCCLEAR.bit.GPIO94 = 1;
// LED5
GPIO_SetupPinMux(95, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(95, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPCCLEAR.bit.GPIO95 = 1;
// LED6
GPIO_SetupPinMux(97, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(97, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPDCLEAR.bit.GPIO97 = 1;
// LED7
GPIO_SetupPinMux(111, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(111, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPDCLEAR.bit.GPIO111 = 1;
// LED8
GPIO_SetupPinMux(130, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(130, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPECLEAR.bit.GPIO130 = 1;
// LED9
GPIO_SetupPinMux(131, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(131, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPECLEAR.bit.GPIO131 = 1;
// LED10
GPIO_SetupPinMux(4, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(4, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO4 = 1;
// LED11
GPIO_SetupPinMux(5, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(5, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO5 = 1;
// LED12
GPIO_SetupPinMux(6, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(6, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO6 = 1;
// LED13
GPIO_SetupPinMux(7, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(7, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO7 = 1;
// LED14
GPIO_SetupPinMux(8, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(8, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO8 = 1;
// LED15
GPIO_SetupPinMux(9, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(9, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO9 = 1;
// LED16
GPIO_SetupPinMux(24, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(24, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO24 = 1;
// LED17
GPIO_SetupPinMux(25, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(25, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO25 = 1;
// LED18
GPIO_SetupPinMux(26, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(26, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO26 = 1;
// LED19
GPIO_SetupPinMux(27, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(27, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO27 = 1;
// LED20
GPIO_SetupPinMux(60, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(60, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPBCLEAR.bit.GPIO60 = 1;
// LED21
GPIO_SetupPinMux(61, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(61, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPBCLEAR.bit.GPIO61 = 1;
// LED22
GPIO_SetupPinMux(157, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(157, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPECLEAR.bit.GPIO157 = 1;
// LED23
GPIO_SetupPinMux(158, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(158, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPECLEAR.bit.GPIO158 = 1;
//DRV8874 #1 DIR Direction
GPIO_SetupPinMux(29, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(29, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPASET.bit.GPIO29 = 1;
//DRV8874 #2 DIR Direction
GPIO_SetupPinMux(32, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(32, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPBSET.bit.GPIO32 = 1;
//MPU9250 CS Chip Select
GPIO_SetupPinMux(66, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(66, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPCSET.bit.GPIO66 = 1;
//PushButton 1
GPIO_SetupPinMux(122, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(122, GPIO_INPUT, GPIO_PULLUP);
//PushButton 2
GPIO_SetupPinMux(123, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(123, GPIO_INPUT, GPIO_PULLUP);
//PushButton 3
GPIO_SetupPinMux(124, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(124, GPIO_INPUT, GPIO_PULLUP);
//PushButton 4
GPIO_SetupPinMux(125, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(125, GPIO_INPUT, GPIO_PULLUP);
//Right Motor
GPIO_SetupPinMux(10, GPIO_MUX_CPU1, 1); // lab2 mux
//Left Motor
GPIO_SetupPinMux(11, GPIO_MUX_CPU1, 1); // lab2 mux
// Clear all interrupts and initialize PIE vector table:
// Disable CPU interrupts
DINT;
EALLOW;
// Motors
EPwm6Regs.TBCTL.bit.CTRMODE = 0; // set to up mode
EPwm6Regs.TBCTL.bit.FREE_SOFT = 2; // free run
EPwm6Regs.TBCTL.bit.PHSEN = 0; // disable phase loading
EPwm6Regs.TBCTL.bit.CLKDIV = 0; // divide by 1
EPwm6Regs.TBCTR = 0; // start timers at 0
EPwm6Regs.TBPRD = 2500; // set period to 50 us (20 kHz)
EPwm6Regs.CMPA.bit.CMPA = 0; // start duty cycle at 0
EPwm6Regs.CMPB.bit.CMPB = 0; // start duty cycle at 0
EPwm6Regs.AQCTLA.bit.CAU = 1; // clear signal when CMPA is reached
EPwm6Regs.AQCTLB.bit.CBU = 1; // clear signal when CMPB is reached
EPwm6Regs.AQCTLA.bit.ZRO = 2; // set pin when TBCTR is zero
EPwm6Regs.AQCTLB.bit.ZRO = 2; // set pin when TBCTR is zero
EPwm6Regs.TBPHS.bit.TBPHS = 0; // set phase to zero
//lab3 ePWM5 timer with no duty cycle output for ADCA
EPwm5Regs.ETSEL.bit.SOCAEN = 0; // Disable SOC on A group
EPwm5Regs.TBCTL.bit.CTRMODE = 3; // freeze counter
EPwm5Regs.ETSEL.bit.SOCASEL = 2; // Select Event when counter equal to PRD
EPwm5Regs.ETPS.bit.SOCAPRD = 1; // Generate pulse on 1st event (“pulse” is the same as “trigger”)
EPwm5Regs.TBCTR = 0x0; // Clear counter
EPwm5Regs.TBPHS.bit.TBPHS = 0x0000; // Phase is 0
EPwm5Regs.TBCTL.bit.PHSEN = 0; // Disable phase loading
EPwm5Regs.TBCTL.bit.CLKDIV = 0; // divide by 1 50Mhz Clock
EPwm5Regs.TBPRD = 50000; // Set Period to 1 ms sample. fs = 1000 Hz Input clock is 50MHz.
// Notice here that we are not setting CMPA or CMPB because we are not using the PWM signal
EPwm5Regs.ETSEL.bit.SOCAEN = 1; //enable SOCA
EPwm5Regs.TBCTL.bit.CTRMODE = 0; //unfreeze, and enter up count mode
//use as timer for ADCB
EPwm12Regs.ETSEL.bit.SOCAEN = 0; // Disable SOC on A group
EPwm12Regs.TBCTL.bit.CTRMODE = 3; // freeze counter
EPwm12Regs.ETSEL.bit.SOCASEL = 2; // Select Event when counter equal to PRD
EPwm12Regs.ETPS.bit.SOCAPRD = 1; // Generate pulse on 1st event (“pulse” is the same as “trigger”)
EPwm12Regs.TBCTR = 0x0; // Clear counter
EPwm12Regs.TBPHS.bit.TBPHS = 0x0000; // Phase is 0
EPwm12Regs.TBCTL.bit.PHSEN = 0; // Disable phase loading
EPwm12Regs.TBCTL.bit.CLKDIV = 0; // divide by 1 50Mhz Clock
EPwm12Regs.TBPRD = pwm12prd;
// Notice here that we are not setting CMPA or CMPB because we are not using the PWM signal
EPwm12Regs.ETSEL.bit.SOCAEN = 1; //enable SOCA
EPwm12Regs.TBCTL.bit.CTRMODE = 0; //unfreeze, and enter up count mode
//ePWM9 used for song
//GPIO_SetupPinMux(16, GPIO_MUX_CPU1, 5);//set up buzzer
GPIO_SetupPinMux(16, GPIO_MUX_CPU1, 0); //set GPIO16 back to GPIO (not PWM)
// set up ePWM9A, 50 MHz clock
EPwm9Regs.TBCTL.bit.CTRMODE = 0; // set to up mode
EPwm9Regs.TBCTL.bit.FREE_SOFT = 2; // free run
EPwm9Regs.TBCTL.bit.PHSEN = 0; // disable phase loading
EPwm9Regs.TBCTL.bit.CLKDIV = 4; // divide by 16 - effective clock 3125000
EPwm9Regs.TBCTR = 0; // start timers at 0, counts up until equal to TBPRD
EPwm9Regs.TBPRD = 65535; //startval
//not using CMPA
EPwm9Regs.AQCTLA.bit.PRD = 3; //toggle output pin when TBCTR = TBPRD
EPwm9Regs.AQCTLA.bit.CAU = 0;
EPwm9Regs.TBPHS.bit.TBPHS = 0; // set phase to zero
// write the initialization code for the two EPWM channels that are by
// default wired though jumpers to the RC servo 3 pin connector
GPIO_SetupPinMux(14, GPIO_MUX_CPU1, 1);
GPIO_SetupPinMux(15, GPIO_MUX_CPU1, 1);
EPwm8Regs.TBCTL.bit.CTRMODE = 0; // set to up mode
EPwm8Regs.TBCTL.bit.FREE_SOFT = 2; // free run
EPwm8Regs.TBCTL.bit.PHSEN = 0; // disable phase loading
EPwm8Regs.TBCTL.bit.CLKDIV = 100; // divide by 16
EPwm8Regs.TBCTR = 0; // start timers at 0
//Problem - TBPRD is 16-bits and can only have a value up to 65,535 --> change CLKDIV
EPwm8Regs.TBPRD = 62500; // 50Hz carrier frequency = 20 ms period
EPwm8Regs.AQCTLA.bit.CAU = 1; // clear signal when CMPA is reached
EPwm8Regs.AQCTLB.bit.CBU = 1; // clear signal when CMPB is reached
EPwm8Regs.AQCTLA.bit.ZRO = 2; // set pin when TBCTR is zero
EPwm8Regs.AQCTLB.bit.ZRO = 2; // set pin when TBCTR is zero
EPwm8Regs.TBPHS.bit.TBPHS = 0; // set phase to zero
// start CMPA’s value such that it is commanding the RC Servo to
// 0 degrees (8% duty cycle)
EPwm8Regs.CMPA.bit.CMPA = .08*EPwm8Regs.TBPRD; // start at 0 deg
EPwm8Regs.CMPB.bit.CMPB = .08*EPwm8Regs.TBPRD; // start at 0 deg
EDIS;
//lab3 ADCA and ADCB
EALLOW;
AdcaRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
AdcbRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
AdcSetMode(ADC_ADCA, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE); //read calibration settings
AdcSetMode(ADC_ADCB, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE); //read calibration settings
//Set pulse positions to late
AdcaRegs.ADCCTL1.bit.INTPULSEPOS = 1;
AdcbRegs.ADCCTL1.bit.INTPULSEPOS = 1;
//power up the ADCs
AdcaRegs.ADCCTL1.bit.ADCPWDNZ = 1;
AdcbRegs.ADCCTL1.bit.ADCPWDNZ = 1;
//delay for 1ms to allow ADC time to power up
DELAY_US(1000);
//ADCA
AdcaRegs.ADCSOC0CTL.bit.CHSEL = 2; //SOC0 will convert Channel A2
AdcaRegs.ADCSOC0CTL.bit.ACQPS = 14; //sample window is acqps + 1 SYSCLK cycles = 75ns
AdcaRegs.ADCSOC0CTL.bit.TRIGSEL = 0x0D;// EPWM5 ADCSOCA will trigger SOC0
AdcaRegs.ADCSOC1CTL.bit.CHSEL = 3; //SOC1 will convert Channel A3
AdcaRegs.ADCSOC1CTL.bit.ACQPS = 14; //sample window is acqps + 1 SYSCLK cycles = 75ns
AdcaRegs.ADCSOC1CTL.bit.TRIGSEL = 0x0D;// EPWM5 ADCSOCA will trigger SOC1
AdcaRegs.ADCINTSEL1N2.bit.INT1SEL = 1; //set to last SOC that is converted and it will set INT1 flag ADCA1
AdcaRegs.ADCINTSEL1N2.bit.INT1E = 1; //enable INT1 flag
AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //make sure INT1 flag is cleared
//ADCB
AdcbRegs.ADCSOC0CTL.bit.CHSEL = 4; //SOC0 will convert Channel B4
AdcbRegs.ADCSOC0CTL.bit.ACQPS = 14; //sample window is acqps + 1 SYSCLK cycles = 75ns
AdcbRegs.ADCSOC0CTL.bit.TRIGSEL = 0x1B; // EPWM12 ADCSOCA will trigger SOC0
AdcbRegs.ADCINTSEL1N2.bit.INT1SEL = 0; //set to last SOC that is converted and it will set INT1 flag ADCB1
AdcbRegs.ADCINTSEL1N2.bit.INT1E = 1; //enable INT1 flag
AdcbRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //make sure INT1 flag is cleared
EDIS;
//lab5 initialize encoders
init_eQEPs();
//lab4 run SPI setup
setupSpib();
// Initialize the PIE control registers to their default state.
// The default state is all PIE interrupts disabled and flags
// are cleared.
// This function is found in the F2837xD_PieCtrl.c file.
InitPieCtrl();
// Disable CPU interrupts and clear all CPU interrupt flags:
IER = 0x0000;
IFR = 0x0000;
// Initialize the PIE vector table with pointers to the shell Interrupt
// Service Routines (ISR).
// This will populate the entire table, even if the interrupt
// is not used in this example. This is useful for debug purposes.
// The shell ISR routines are found in F2837xD_DefaultIsr.c.
// This function is found in F2837xD_PieVect.c.
InitPieVectTable();
// Interrupts that are used in this example are re-mapped to
// ISR functions found within this project
EALLOW; // This is needed to write to EALLOW protected registers
PieVectTable.TIMER0_INT = &cpu_timer0_isr;
PieVectTable.TIMER1_INT = &cpu_timer1_isr;
PieVectTable.TIMER2_INT = &cpu_timer2_isr;
PieVectTable.SCIA_RX_INT = &RXAINT_recv_ready;
PieVectTable.SCIC_RX_INT = &RXCINT_recv_ready;
PieVectTable.SCID_RX_INT = &RXDINT_recv_ready;
PieVectTable.SCIA_TX_INT = &TXAINT_data_sent;
PieVectTable.SCIC_TX_INT = &TXCINT_data_sent;
PieVectTable.SCID_TX_INT = &TXDINT_data_sent;
PieVectTable.SPIB_RX_INT = &SPIB_isr;
PieVectTable.ADCA1_INT = &ADCA_ISR;
PieVectTable.ADCB1_INT = &ADCB_ISR;
PieVectTable.EMIF_ERROR_INT = &SWI_isr;
EDIS; // This is needed to disable write to EALLOW protected registers
// Initialize the CpuTimers Device Peripheral. This function can be
// found in F2837xD_CpuTimers.c
InitCpuTimers();
// Configure CPU-Timer 0, 1, and 2 to interrupt every second:
// 200MHz CPU Freq, 1 second Period (in uSeconds)
ConfigCpuTimer(&CpuTimer0, 200, 150001); //tempo2
ConfigCpuTimer(&CpuTimer1, 200, 197628); //tempo
ConfigCpuTimer(&CpuTimer2, 200, 230769); //tempo3
// Enable CpuTimer Interrupt bit TIE
CpuTimer0Regs.TCR.all = 0x4000;
CpuTimer1Regs.TCR.all = 0x4000;
CpuTimer2Regs.TCR.all = 0x4000;
init_serial(&SerialA,115200,serialRXA);
// init_serial(&SerialC,115200,serialRXC);
// init_serial(&SerialD,115200,serialRXD);
// Enable CPU int1 which is connected to CPU-Timer 0, CPU int13
// which is connected to CPU-Timer 1, and CPU int 14, which is connected
// to CPU-Timer 2: int 12 is for the SWI.
IER |= M_INT1;
IER |= M_INT8; // SCIC SCID
IER |= M_INT9; // SCIA
IER |= M_INT12;
IER |= M_INT13;
IER |= M_INT14;
IER |= M_INT6; //lab4 enable interrupt 6
// Enable TINT0 in the PIE: Group 1 interrupt 7
PieCtrlRegs.PIEIER1.bit.INTx7 = 1;
// Enable SWI in the PIE: Group 12 interrupt 9
PieCtrlRegs.PIEIER12.bit.INTx9 = 1;
//group 6 interrupt 3
PieCtrlRegs.PIEIER6.bit.INTx3 = 1; //lab4 SPI
// lab3 Enable PIE: Group 1 interrupt 1, ADCA1
PieCtrlRegs.PIEIER1.bit.INTx1 = 1;
// Enable PIE: Group 1 interrupt 2, ADCB1
PieCtrlRegs.PIEIER1.bit.INTx2 = 1;
AdcbRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //clear interrupt flag
PieCtrlRegs.PIEACK.all = PIEACK_GROUP1; //clear PIE peripheral so processor waits until next interrupt flag
// Enable global Interrupts and higher priority real-time debug events
EINT; // Enable Global interrupt INTM
ERTM; // Enable Global realtime interrupt DBGM
// IDLE loop. Just sit and loop forever:
while(1)
{
if (UARTPrint == 1 ) {
//serial_printf(&SerialA,"Tilt: %f Gyro: %f Left: %f Right: %f\r\n", tilt_value, gyro_value, LeftWheel, RightWheel);
UARTPrint = 0;
}
//after n samples, pass data through Goertzel fxn
// use Ping Pong buffer
if(RunPing == 1){
goer_result = goertzel_mag(n_samples,NOTE,sampling_rate, adcb_arrayPing);
RunPing = 0;
if(goer_result > thresh){
note_detected = 1;
}
}
if(RunPong == 1){
goer_result2 = goertzel_mag(n_samples,NOTE2,sampling_rate, adcb_arrayPong);
RunPong = 0;
if(goer_result2 > thresh){
note2_detected = 1;
}
}
if(RunDong == 1){
goer_result3 = goertzel_mag(n_samples,NOTE3,sampling_rate, adcb_arrayDong);
RunDong = 0;
if(goer_result3 > thresh){
note3_detected = 1;
}
}
}
}
// SWI_isr, Using this interrupt as a Software started interrupt
__interrupt void SWI_isr(void) {
// These three lines of code allow SWI_isr, to be interrupted by other interrupt functions
// making it lower priority than all other Hardware interrupts.
PieCtrlRegs.PIEACK.all = PIEACK_GROUP12;
asm(" NOP"); // Wait one cycle
EINT; // Clear INTM to enable interrupts
//lab6 control with turnrate
turnref = turnref_1 + (turnrate + turnrate_1)*.004/2.0;
//lab6 steering
WheelDif = LeftWheel - RightWheel; //difference between wheel positions, radians
velWheelDif = .33*velWheelDif_1 + .67*(WheelDif-WheelDif_1)/.004; //raw velocity higher weight than old velocity, less lag
eDif = turnref - WheelDif;
IntDif = IntDif_1 + (eDif + eDif_1)*.004/2.0;
turn = Kp*eDif + Ki*IntDif - Kd*velWheelDif;
//integral windup guard
if(fabs(turn)>3){
IntDif = IntDif_1;
}
//saturate turning so the segbot balance dominates
if(turn>4){
turn=4;
}
if(turn<-4){
turn=-4;
}
//lab6 Balance
vLeft = .6*vLeft_1 + .4*(LeftWheel - LeftWheel_1)/.004; //rad/s
vRight = .6*vRight_1 + .4*(RightWheel - RightWheel_1)/.004; //rad/s
uBal = -K1*tilt_value -K2*gyro_value -K3*(vLeft+vRight)/2;
//lab6 control
uLeft = uBal/2 + turn + FwdBkOffset;
uRight = uBal/2 -turn + FwdBkOffset;
//pass controleffort through PWM
setEPWM6A(uLeft);
setEPWM6B(-uRight);
//update variables
vLeft_1 = vLeft;
vRight_1 = vRight;
LeftWheel_1 = LeftWheel;
RightWheel_1 = RightWheel;
WheelDif_1 = WheelDif;
velWheelDif_1 = velWheelDif;
eDif_1 = eDif;
IntDif_1 = IntDif;
turnrate_1 = turnrate;
turnref_1 = turnref;
numSWIcalls++;
DINT;
}
// cpu_timer0_isr - CPU Timer0 ISR
__interrupt void cpu_timer0_isr(void)
{
CpuTimer0.InterruptCount++;
if(note2_detected == 1){
// Blink a number of LEDS if the Goertzel value exceeds the threshold
GpioDataRegs.GPBTOGGLE.bit.GPIO52 = 1;
GpioDataRegs.GPCTOGGLE.bit.GPIO94 = 1;
GpioDataRegs.GPDTOGGLE.bit.GPIO97 = 1;
GPIO_SetupPinMux(16, GPIO_MUX_CPU1, 5);//set up buzzer
if (numTimer0calls < song2size){
//play the song
if(song2[numTimer0calls]==0){
GpioDataRegs.GPACLEAR.bit.GPIO16 = 1; //set GPIO16 low to ground the speaker
}else{
EPwm9Regs.TBPRD = (int)(3125000/song2[numTimer0calls]/2);
}
//dance
FwdBkOffset = fwddance2[numTimer0calls];
turnrate = turndance2[numTimer0calls];
} else{
GPIO_SetupPinMux(16, GPIO_MUX_CPU1, 0); //set GPIO16 back to GPIO (not PWM)
GpioDataRegs.GPACLEAR.bit.GPIO16 = 1; //set GPIO16 low to ground the speaker
note2_detected = 0;
numTimer0calls = 0;
FwdBkOffset = 0;
turnrate = 0;
}
numTimer0calls++; // only increment after note is detected
}
// Acknowledge this interrupt to receive more interrupts from group 1
PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
}
// cpu_timer1_isr - CPU Timer1 ISR
__interrupt void cpu_timer1_isr(void)
{
if(note_detected == 1){
// Blink a number of LEDS if the Goertzel value exceeds the threshold
GpioDataRegs.GPATOGGLE.bit.GPIO22 = 1;
GpioDataRegs.GPBTOGGLE.bit.GPIO52 = 1;
GpioDataRegs.GPCTOGGLE.bit.GPIO67 = 1;
GpioDataRegs.GPCTOGGLE.bit.GPIO94 = 1;
GpioDataRegs.GPCTOGGLE.bit.GPIO95 = 1;
GpioDataRegs.GPDTOGGLE.bit.GPIO97 = 1;
GPIO_SetupPinMux(16, GPIO_MUX_CPU1, 5);//set up buzzer
if (numtimer1calls < songsize){
//play the song
if(song[numtimer1calls]==0){
GpioDataRegs.GPACLEAR.bit.GPIO16 = 1; //set GPIO16 low to ground the speaker
}else{
EPwm9Regs.TBPRD = (int)(3125000/song[numtimer1calls]/2);
}
//dance
FwdBkOffset = fwddance[numtimer1calls];
turnrate = turndance[numtimer1calls];
} else{
GPIO_SetupPinMux(16, GPIO_MUX_CPU1, 0); //set GPIO16 back to GPIO (not PWM)
GpioDataRegs.GPACLEAR.bit.GPIO16 = 1; //set GPIO16 low to ground the speaker
note_detected = 0;
numtimer1calls = 0;
FwdBkOffset = 0;
turnrate = 0;
}
numtimer1calls++; // only increment after note is detected
}
CpuTimer1.InterruptCount++;
}
// cpu_timer2_isr CPU Timer2 ISR
__interrupt void cpu_timer2_isr(void)
{
if(note3_detected == 1){
// Blink a number of LEDS if the Goertzel value exceeds the threshold
GpioDataRegs.GPATOGGLE.bit.GPIO22 = 1;
GpioDataRegs.GPBTOGGLE.bit.GPIO52 = 1;
GpioDataRegs.GPCTOGGLE.bit.GPIO67 = 1;
GPIO_SetupPinMux(16, GPIO_MUX_CPU1, 5);//set up buzzer
if (numTimer2calls < song3size){
//play the song
if(song3[numTimer2calls]==0){
GpioDataRegs.GPACLEAR.bit.GPIO16 = 1; //set GPIO16 low to ground the speaker
}else{
EPwm9Regs.TBPRD = (int)(3125000/song3[numTimer2calls]/2);
}
//dance
FwdBkOffset = fwddance3[numTimer2calls];
turnrate = turndance3[numTimer2calls];
//bell
if((numTimer2calls >= 19 && numTimer2calls <= 26)||(numTimer2calls >= 51 && numTimer2calls <= 58)||(numTimer2calls >= 99 && numTimer2calls <= 106)){
if(numTimer2calls%2 ==0){
EPwm8Regs.CMPA.bit.CMPA = .08*EPwm8Regs.TBPRD; //set RC to 0 degrees
EPwm8Regs.CMPB.bit.CMPB = .08*EPwm8Regs.TBPRD; //set RC to 0 degrees
}else{
EPwm8Regs.CMPA.bit.CMPA = .03*EPwm8Regs.TBPRD; //set RC to -90 degrees
EPwm8Regs.CMPB.bit.CMPB = .03*EPwm8Regs.TBPRD; //set RC to -90 degrees
}
}
} else{
GPIO_SetupPinMux(16, GPIO_MUX_CPU1, 0); //set GPIO16 back to GPIO (not PWM)
GpioDataRegs.GPACLEAR.bit.GPIO16 = 1; //set GPIO16 low to ground the speaker
note3_detected = 0;
numTimer2calls = 0;
FwdBkOffset = 0;
turnrate = 0;
}
numTimer2calls++; // only increment after note is detected
}
CpuTimer2.InterruptCount++;
}
// This function is called each time a char is recieved over UARTA.
void serialRXA(serial_t *s, char data) {
numRXA ++;
if (data == 'q') {
turnrate = turnrate - 0.2;
} else if (data == 'r') {
turnrate = turnrate + 0.2;
} else if (data == '3') {
FwdBkOffset = FwdBkOffset - 0.2;
} else if (data == 's') {
FwdBkOffset = FwdBkOffset + 0.2;
} else {
turnrate = 0;
FwdBkOffset = 0;
}
}
//ADCA : transmit SPI values
__interrupt void ADCA_ISR (void)
{
//SPI Read Gyros and Accelerometers
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1; // Slave Select Low
SpibRegs.SPIFFRX.bit.RXFFIL = 8; // Issue the SPIB_RX_INT when two values are in the RX FIFO
//write to TXBUF
//set MSB to 1 for READ
SpibRegs.SPITXBUF = 0x8000|0x3A00; //start address INT_STATUS, write 0x00 to INT_STATUS
SpibRegs.SPITXBUF = 0x0000; //write 0x00 to read ACCEL_XOUT
SpibRegs.SPITXBUF = 0x0000; //write 0x00 to read ACCEL_YOUT
SpibRegs.SPITXBUF = 0x0000; //write 0x00 to read ACCEL_ZOUT
SpibRegs.SPITXBUF = 0x0000; //write 0x00 to read TEMP_OUT
SpibRegs.SPITXBUF = 0x0000; //write 0x00 to read GYRO_XOUT
SpibRegs.SPITXBUF = 0x0000; //write 0x00 to read GYRO_YOUT
SpibRegs.SPITXBUF = 0x0000; //write 0x00 to read GYRO_ZOUT
//read from RXBUF in the SPIB interrupt function
//GPIO set back to high in interrupt to end chip select
AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //clear interrupt flag
PieCtrlRegs.PIEACK.all = PIEACK_GROUP1; //clear PIE peripheral so processor waits until next interrupt flag
}
//adcb1 interrupt
__interrupt void ADCB_ISR (void)
{
adcb0result = AdcbResultRegs.ADCRESULT0; //read value between 0 and 4095 from ADC
//MAX9814 sensitivity = -44 dBV meaning the signal corresponds to 6.3096mV/Pa of sound pressure
//ADC values 0 to 4095 correspond to voltages 0-3V, mic produces 0.5 to 2.5 V (values ~682 to ~3412)
//Ping
if(PingPong == 0){
adcb_arrayPing[adcbcount] = adcb0result; // add ADC reading to array
if(adcbcount == (n_samples-1)){
adcbcount = -1; //incremented to 0 at end of interrupt
RunPing = 1; //check in while loop
PingPong = 1; //switch to Pong buffer
}
}
//Pong
if(PingPong == 1){
adcb_arrayPong[adcbcount] = adcb0result;
if(adcbcount == (n_samples-1)){
adcbcount = -1; //incremented to 0 at end of interrupt
RunPong = 1;
PingPong = 2; //switch to Dong buffer
}
}
//Dong
if(PingPong == 2){
adcb_arrayDong[adcbcount] = adcb0result;
if(adcbcount == (n_samples-1)){
adcbcount = -1; //incremented to 0 at end of interrupt
RunDong = 1;
PingPong = 0; //switch to Ping buffer
}
}
adcbcount++; //increment count
AdcbRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //clear interrupt flag
PieCtrlRegs.PIEACK.all = PIEACK_GROUP1; //clear PIE peripheral so processor waits until next interrupt flag
}
// DFT with Goertzel Algorithm
float goertzel_mag(int numSamples,int TARGET_FREQUENCY,int SAMPLING_RATE, float* data)
{
int k,i;
float floatnumSamples;
float omega,sine,cosine,coeff,q0,q1,q2,magnitude,real,imag;
float scalingFactor = numSamples / 2.0;
floatnumSamples = (float) numSamples;
k = (int) (0.5 + ((floatnumSamples * TARGET_FREQUENCY) / SAMPLING_RATE));
omega = (2.0 * PI * k) / floatnumSamples;
sine = sin(omega);
cosine = cos(omega);
coeff = 2.0 * cosine;
q0=0;
q1=0;
q2=0;
for(i=0; i<numSamples; i++)
{
q0 = coeff * q1 - q2 + data[i];
q2 = q1;
q1 = q0;
}
// calculate the real and imaginary results
// scaling appropriately
real = (q1 - q2 * cosine) / scalingFactor;
imag = (q2 * sine) / scalingFactor;
magnitude = sqrtf(real*real + imag*imag);
return magnitude;
}
//SPIB:Read IMU (acceleromoeter) and encoder (motor) values
__interrupt void SPIB_isr(void){
//read from RX FIFO
Gvalue = SpibRegs.SPIRXBUF; //garbage value read
accelxraw = SpibRegs.SPIRXBUF;
...
This file has been truncated, please download it to see its full contents.
Comments
Please log in or sign up to comment.