//#############################################################################
// 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"
#define PI 3.1415926535897932384626433832795
#define TWOPI 6.283185307179586476925286766559
#define HALFPI 1.5707963267948966192313216916398
// 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);
//Lab4-2
__interrupt void SPIB_isr(void);
//****************Lab3-3 microphone reading*************
__interrupt void ADCB_ISR(void);
//Varaibles for Lab3-3
int16_t adcb0result = 0;
int32_t ADCB_count = 0;
float acbc0scaled = 0;
float acbc0scaled_1 = 0; //prev value
//xb is the current ADC reading, xb_1 is the ADC reading one millisecond ago, xb_2 two milliseconds ago, etc
//yb is the filtered value
float yb = 0.0;
float yb_1 = 0.0; //previous voltage value of
int32_t period_timer = 0;
float period = 0.0;
float frequency = 0.0;
#define ARRAY_SIZE 51 //50th order filter
float xb[ARRAY_SIZE] = {0.0}; //create an xb array of all Zeroes
float bb[51]={ 4.4386708142154924e-03, //750-1110Hz bandpass filter
4.0819401687493375e-03,
3.6964789700683698e-03,
3.0229624550739509e-03,
1.7499261361542152e-03,
-4.2138486396470226e-04,
-3.7068603319891274e-03,
-8.1753026543628423e-03,
-1.3701117711035941e-02,
-1.9942972701613437e-02,
-2.6354103401927439e-02,
-3.2225577850126579e-02,
-3.6759044454338259e-02,
-3.9160987761299504e-02,
-3.8746943281146543e-02,
-3.5042027971949885e-02,
-2.7863874034645172e-02,
-1.7375709634282227e-02,
-4.1007408522239731e-03,
1.1106266307477984e-02,
2.7128843663354728e-02,
4.2693309670939303e-02,
5.6496387016975951e-02,
6.7341158712959037e-02,
7.4265157640474172e-02,
7.6645060400053699e-02,
7.4265157640474172e-02,
6.7341158712959037e-02,
5.6496387016975951e-02,
4.2693309670939303e-02,
2.7128843663354728e-02,
1.1106266307477984e-02,
-4.1007408522239731e-03,
-1.7375709634282227e-02,
-2.7863874034645172e-02,
-3.5042027971949885e-02,
-3.8746943281146543e-02,
-3.9160987761299504e-02,
-3.6759044454338259e-02,
-3.2225577850126579e-02,
-2.6354103401927439e-02,
-1.9942972701613437e-02,
-1.3701117711035941e-02,
-8.1753026543628423e-03,
-3.7068603319891274e-03,
-4.2138486396470226e-04,
1.7499261361542152e-03,
3.0229624550739509e-03,
3.6964789700683698e-03,
4.0819401687493375e-03,
4.4386708142154924e-03};
//************ END Mic Initialization *************8
//Lab4-4
void setupSpib(void);
//*********Lab5-1************
void init_eQEPs(void);
float readEncLeft(void);
float readEncRight(void);
//Lab5-2
void setEPWM6A(float controleffort);
void setEPWM6B(float controleffort);
void serialRXA(serial_t *s, char data);
//*********END***************
//********* Lab6-1 **********
//Lab3-takehome Joystick
__interrupt void ADCA_ISR(void);
//Lab4-4 MPU-9250
int16_t dummy= 0;
int16_t accelXraw = 0;
int16_t accelYraw = 0;
int16_t accelZraw = 0;
int16_t tempraw = 0;
int16_t gyroXraw = 0;
int16_t gyroYraw = 0;
int16_t gyroZraw = 0;
float accelx = 0;
float accely = 0;
float accelz = 0;
float gyrox = 0;
float gyroy = 0;
float gyroz = 0;
//counter for number of times SPIB function is called
int32_t numSPIB_ISRcalls = 0;
//*************END****************
// Count variables
uint32_t numTimer0calls = 0;
uint32_t numSWIcalls = 0;
uint32_t numRXA = 0;
uint16_t UARTPrint = 0;
uint16_t updown = 1;
float volt1 = 0.0;
float volt2 = 0.0;
//Lab5-1
float LeftWheel = 0.0;
float RightWheel = 0.0;
float radperft = 9.3305;
float distleft = 0.0;
float distright = 0.0;
//******Lab6-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 = -.83;
int16 IMU_data[9];
uint16_t temp=0;
int16_t doneCal = 0;
float tilt_value = 0;
float tilt_array[4] = {0, 0, 0, 0};
float gyro_value = 0;
float gyro_array[4] = {0, 0, 0, 0};
float LeftWheelArray[4] = {0,0,0,0};
float RightWheelArray[4] = {0,0,0,0};
// Kalman Filter vars
float T = 0.001; //sample rate, 1ms
float Q = 0.01; // made global to enable changing in runtime
float R = 25000;//50000;
float kalman_tilt = 0;
float kalman_P = 22.365;
int16_t SpibNumCalls = -1;
float pred_P = 0;
float kalman_K = 0;
int32_t timecount = 0;
int16_t calibration_state = 0;
int32_t calibration_count = 0;
//**********End Lab6-2****************
//*********Start Lab6-3******************
float uLeft = 0.0; // control effort for left wheel
float uRight = 0.0; //control effort for right wheel
float XLeftK = 0.0; //current left wheel pos.
float XLeftK_1 = 0.0; //Prev left wheel pos.
float XRightK = 0.0; //current Right wheel pos.
float XRightK_1 = 0.0; //prevouis rightt wheel pos.
float VLeftK = 0.0; //Left velocity
float VLeftK_1 = 0.0; //Prev Left velocity
float VRightK = 0.0; //Right Velocity
float VRightK_1 = 0.0; //prev Right Velocity
//***Initial Gains****
float K1 = -32.0; //Tilt Gain
float K2 = -3.0; //Gyrorate Gain
float K3 = -1.0; //Velocity Gain
//***Simul. Gains****
//float K1 = -42.902; //Tilt Gain
//float K2 = -3.216; //Gyrorate Gain
//float K3 = -1.063; //Velocity Gain
float Ubal = 0.0; //Balancing Control law
//************End Lab6-3*****************
//******** Start Lab6-4 ***********
float WhlDiff = 0.0; //dif between wheel angle in radians
float WhlDiff_1 = 0.0;
float vel_WhlDiff = 0.0; //dif between wheel velocitys in rad/s
float vel_WhlDiff_1 = 0.0;
float eDiff = 0.0;
float IntDiff = 0.0;
float VelDiff = 0.0;
float eDiff_1 = 0.0;
float IntDiff_1 = 0.0;
float VelDiff_1 = 0.0;
float turn = 0.0; //angle to turn to
float FwdBckOffset = 0.0; //make segbot go fwd or bckwrd
float turnref = 0.0; //Cmd segbot to turn to this angle and stop
float turnref_1 = 0.0;
float errorDiff = 0.0; //error between turnref and whldiff
float errorDiff_1 = 0.0;
float Kp_turn = 3.0; //Porportianal Gain
float Ki_turn = 20.0; //Integral Gain
float Kd_turn = 0.08; //Derivative Gain
float turnrate = 0.0; //rate at which segbot turns
float turnrate_1 = 0.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);
// Clear all interrupts and initialize PIE vector table:
// Disable CPU interrupts
DINT;
setupSpib();
init_eQEPs();
EALLOW; //Lab3-takehome
EPwm5Regs.ETSEL.bit.SOCAEN = 0; // Disable SOC on A group
EPwm5Regs.TBCTL.bit.CTRMODE = 3; // freeze counter
EPwm5Regs.ETSEL.bit.SOCASEL = 2; // Select Event when counter equal to PRD
EPwm5Regs.ETPS.bit.SOCAPRD = 1; // Generate pulse on 1st event (“pulse” is the same as “trigger”)
EPwm5Regs.TBCTR = 0x0; // Clear counter
EPwm5Regs.TBPHS.bit.TBPHS = 0x0000; // Phase is 0
EPwm5Regs.TBCTL.bit.PHSEN = 0; // Disable phase loading
EPwm5Regs.TBCTL.bit.CLKDIV = 0; // divide by 1 50Mhz Clock
EPwm5Regs.TBPRD = 50000; // Set Period to 1ms sample. Input clock is 50MHz.
// Notice here that we are not setting CMPA or CMPB because we are not using the PWM signal
EPwm5Regs.ETSEL.bit.SOCAEN = 1; //enable SOCA
EPwm5Regs.TBCTL.bit.CTRMODE = 0; //unfreeze, and enter up count mode
EDIS;
//Lab5-2 settings copied from Lab2-takehome
EPwm6Regs.TBCTL.bit.CTRMODE = 0;
EPwm6Regs.TBCTL.bit.FREE_SOFT = 2;
EPwm6Regs.TBCTL.bit.PHSEN = 0;
EPwm6Regs.TBCTL.bit.CLKDIV = 0;
EPwm6Regs.TBCTR = 0;
EPwm6Regs.TBPRD = 2500; //20KHz carrier freq
EPwm6Regs.CMPA.bit.CMPA = 0;
EPwm6Regs.AQCTLA.bit.CAU = 1;
EPwm6Regs.AQCTLA.bit.ZRO = 2;
EPwm6Regs.TBPHS.bit.TBPHS = 0;
EPwm6Regs.CMPB.bit.CMPB = 0;
EPwm6Regs.AQCTLB.bit.CBU = 1;
EPwm6Regs.AQCTLB.bit.ZRO = 2;
//************EPWM8 for Mic**************
EALLOW;
EPwm8Regs.ETSEL.bit.SOCAEN = 0; // Disable SOC on A group
EPwm8Regs.TBCTL.bit.CTRMODE = 3; // freeze counter
EPwm8Regs.ETSEL.bit.SOCASEL = 2; // Select Event when counter equal to PRD
EPwm8Regs.ETPS.bit.SOCAPRD = 1; // Generate pulse on 1st event (“pulse” is the same as “trigger”)
EPwm8Regs.TBCTR = 0x0; // Clear counter
EPwm8Regs.TBPHS.bit.TBPHS = 0x0000; // Phase is 0
EPwm8Regs.TBCTL.bit.PHSEN = 0; // Disable phase loading
EPwm8Regs.TBCTL.bit.CLKDIV = 0; // divide by 1 50Mhz Clock
EPwm8Regs.TBPRD = 2000; //EX) Set Period to 0.25ms (4,000Hz) TBPRD=12500. Input clock is 50MHz.
// 10KHz = Period of 0.10ms. TBPRD = 5000
// 25KHz (TBPRD = 2000)
// Notice here that we are not setting CMPA or CMPB because we are not using the PWM signal
EPwm8Regs.ETSEL.bit.SOCAEN = 1; //enable SOCA
EPwm8Regs.TBCTL.bit.CTRMODE = 0; //unfreeze, and enter up count mode
EDIS;
//EPWM6A
GPIO_SetupPinMux(10, GPIO_MUX_CPU1, 1); //GPIO PinName, CPU, Mux Index
//EPWM6B
GPIO_SetupPinMux(11, GPIO_MUX_CPU1, 1); //GPIO PinName, CPU, Mux Index
EALLOW; // Below are protected registers
GpioCtrlRegs.GPAPUD.bit.GPIO10 = 1; //motor
GpioCtrlRegs.GPAPUD.bit.GPIO11 = 1; //motor
EDIS;
EALLOW; //Lab3-takehome
//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
//************ADCB for Mic**********
AdcbRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
AdcSetMode(ADC_ADCB, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE); //read calibration settings
AdcbRegs.ADCCTL1.bit.INTPULSEPOS = 1;//Set pulse positions to late
AdcbRegs.ADCCTL1.bit.ADCPWDNZ = 1; //power up the ADCs
//delay for 1ms to allow ADC time to power up
DELAY_US(1000);
//ADCA //Lab3-Takehome
AdcaRegs.ADCSOC0CTL.bit.CHSEL = 0x2; //SOC0 will convert Channel 2
AdcaRegs.ADCSOC0CTL.bit.ACQPS = 14; //sample window is acqps + 1 SYSCLK cycles = 75ns
AdcaRegs.ADCSOC0CTL.bit.TRIGSEL = 0xD;// EPWM5 ADCSOCA or another trigger you choose will trigger SOC0
AdcaRegs.ADCSOC1CTL.bit.CHSEL = 0x3; //SOC1 will convert Channel 3
AdcaRegs.ADCSOC1CTL.bit.ACQPS = 14; //sample window is acqps + 1 SYSCLK cycles = 75ns
AdcaRegs.ADCSOC1CTL.bit.TRIGSEL = 0xD;// EPWM5 ADCSOCA or another trigger you choose will trigger SOC1
AdcaRegs.ADCINTSEL1N2.bit.INT1SEL = 0x0; //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 for MicroPhone
AdcbRegs.ADCSOC0CTL.bit.CHSEL = 0x4; //SOC0 will convert Channel you choose Does not have to be B0
AdcbRegs.ADCSOC0CTL.bit.ACQPS = 14; //sample window is acqps + 1 SYSCLK cycles = 75ns
AdcbRegs.ADCSOC0CTL.bit.TRIGSEL = 0x13; // EPWM8A ADCSOCA will trigger SOC0
AdcbRegs.ADCINTSEL1N2.bit.INT1SEL = 0x0; //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;
// Enable DACA and DACB outputs
//Copied from Lab3-takehome
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;
// 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.EMIF_ERROR_INT = &SWI_isr;
//Lab4-2
PieVectTable.SPIB_RX_INT = &SPIB_isr;
//Lab3-takehome for Joystick
PieVectTable.ADCA1_INT = &ADCA_ISR;
//Lab3-3 for Microphone
PieVectTable.ADCB1_INT = &ADCB_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, 20000); //Time out 4ms to call SWI interrupt
ConfigCpuTimer(&CpuTimer1, 200, 4000); //Time out 4ms for init_eQEPs()
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);
// 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_INT6; //SPIB_TX SPIB_RX
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.PIEIER6.bit.INTx3 = 1; //SPIB_RX
//lab3-1 & 3-2: enable interrupt group 1 interrupt 6
PieCtrlRegs.PIEIER1.bit.INTx6 = 1;
//Lab3-3: Enable interrupt Group 1 interrupt 2 for micrphone
PieCtrlRegs.PIEIER1.bit.INTx2 = 1;
//lab3-takehome: enable interrupt group 1 interrupt 1
PieCtrlRegs.PIEIER1.bit.INTx1 = 1;
// 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):tilt_value, gyro_value, LeftWheel,
while(1)
{
if (UARTPrint == 1 ) {
//serial_printf(&SerialA,"tilt_value, gyro_value, LeftWheel, RightWheel = %.3f,%.3f, %.3f, %.3f \r\n", tilt_value, gyro_value, LeftWheel, RightWheel);
serial_printf(&SerialA,"Freq_filt, yb = %f %f\r\n",frequency, yb);
UARTPrint = 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.......
//Find Velocity of Wheels (Should be in rad/s)
XLeftK = LeftWheel;
XRightK = RightWheel;
VLeftK = 0.6*VLeftK_1+100*XLeftK-100*XLeftK_1; //calc velocity of Left wheel w/ V(s)/P(s) = 125s/(s+125)
VRightK = 0.6*VRightK_1+100*XRightK-100*XRightK_1; //calc velocity of Right wheel w/ V(s)/P(s) = 125s/(s+125)
//Calc Balancing Control
Ubal = -K1*tilt_value-K2*gyro_value-K3*((VLeftK+VRightK)/2.0);
//Turning Control
WhlDiff = LeftWheel-RightWheel;
//TF = V/W = (166.6667z+166.6667)/(z+0.3333)
vel_WhlDiff = 166.6667*WhlDiff-166.6667*WhlDiff_1+vel_WhlDiff_1*0.3333;
turnref = turnref_1 + ((turnrate+turnrate_1)/2.0)*0.004;
errorDiff = turnref-WhlDiff;
IntDiff = IntDiff_1 + ((errorDiff+errorDiff_1)/2.0)*0.004; //Trapezoid Rule (0.004s)
turn = Kp_turn*errorDiff+Ki_turn*IntDiff-Kd_turn*vel_WhlDiff; //Angle of turn
if (fabs(turn)>3.0){
IntDiff = IntDiff_1; //Eliminating integral windup
}
if (turn>4.0){ //Saturating Turn
turn = 4.0;
}else if (turn<-4.0){
turn = -4.0;
}
//Set velocities of motors
uLeft = Ubal/2.0+turn+FwdBckOffset;
uRight = Ubal/2.0-turn+FwdBckOffset;
setEPWM6A(uLeft); //Left Motor
setEPWM6B(-uRight); //Right Motor
//Set Prev States for next iteration
XLeftK_1 = XLeftK;
XRightK_1 = XRightK;
VLeftK_1 = VLeftK;
VRightK_1 = VRightK;
IntDiff_1 = IntDiff;
errorDiff_1 = errorDiff;
vel_WhlDiff_1 = vel_WhlDiff;
turnrate_1 = turnrate;
turnref_1 = turnref;
numSWIcalls++;
DINT;
}
// cpu_timer0_isr - CPU Timer0 ISR
__interrupt void cpu_timer0_isr(void)
{
CpuTimer0.InterruptCount++;
// Acknowledge this interrupt to receive more interrupts from group 1
PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
}
// cpu_timer1_isr - CPU Timer1 ISR
__interrupt void cpu_timer1_isr(void)
{
CpuTimer1.InterruptCount++;
}
// cpu_timer2_isr CPU Timer2 ISR
__interrupt void cpu_timer2_isr(void)
{
}
// 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') {
FwdBckOffset = FwdBckOffset - 0.2;
} else if (data == 's') {
FwdBckOffset = FwdBckOffset + 0.2;
} else {
turnrate = 0;
FwdBckOffset = 0;
}
}
__interrupt void ADCA_ISR(void)
{
//Transmitting to MPU-9250
//Copied from lab4-4
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;
SpibRegs.SPIFFRX.bit.RXFFIL = 8;
SpibRegs.SPITXBUF = ((0x8000) | (0x3A00)); //16bits
SpibRegs.SPITXBUF = 0; //Acc_X_H/L (16bits)
SpibRegs.SPITXBUF = 0; //Acc_Y_H/L (16bits)
SpibRegs.SPITXBUF = 0; //Acc_Z_H/L (16bits)
SpibRegs.SPITXBUF = 0; //Temp_H/L (16bits)
SpibRegs.SPITXBUF = 0; //Gyro_X_H/L (16bits)
SpibRegs.SPITXBUF = 0; //Gyro_Y_H/L (16bits)
SpibRegs.SPITXBUF = 0; //Gyro_Z_H/L (16bits)
// Blink LaunchPad Red LED
GpioDataRegs.GPBTOGGLE.bit.GPIO34 = 1;
AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //clear interrupt flag
PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
}
__interrupt void ADCB_ISR(void)
{
adcb0result = AdcbResultRegs.ADCRESULT0;
//Iterate period timer
period_timer++;
// period_timer2++;
// Here covert ADCINB0 to volts
acbc0scaled = adcb0result/1365.0;
xb[0] = acbc0scaled;
//have to zero out yb inside interupt cause
//yb is being iterated in the for loop and constantly added onto
yb = 0;
//for loop to solve a nth order filter
//size of filter defined by ARRAY_SIZE
int i;
for ( i=0; i<ARRAY_SIZE; i=i+1 ) {
yb = yb + bb[i]*xb[i];
}
//for loop to iterate past states based on ARRAY_SIZE
//Save past states before exiting from the function so
//that next sample they are the older state
for (i=ARRAY_SIZE-1; i>0; i=i-1){
xb[i]=xb[i-1];
}
//If statement to figure out the frequency of the signal
if (yb_1 < 0.35 && yb >= 0.35){ //triggers at peak of sine wave (sensitivity)
period = period_timer; //sets period to # of times ADCB has been called
frequency = 1/(period*(.00004)); //calc freq based on 25KHz sampling freq
period_timer = 0; //reset # of times ADCB was called
}
else {
//will set frequency to 0 if 2 waves have passed and..
//..none have been above the require threshold..
//(ie no sound is being passed by the filter)
if (period_timer > 2*period){
frequency = 0;
}
}
if (frequency>650 && frequency<800){ //750Hz input
// Column 1 ON
GpioDataRegs.GPASET.bit.GPIO22 = 1; // LED1 On
// Column 2 OFF
GpioDataRegs.GPDCLEAR.bit.GPIO111 = 1; // LED7 Off
// Column 3 OFF
GpioDataRegs.GPACLEAR.bit.GPIO7 = 1; // LED13 Off
FwdBckOffset = -1.5;
turnrate = 0;
}
else if (frequency>830 && frequency<1000){ //950Hz input
// Column 2 ON
GpioDataRegs.GPDSET.bit.GPIO111 = 1; // LED7 On
// Column 1 OFF
GpioDataRegs.GPACLEAR.bit.GPIO22 = 1; // LED1 Off
// Column 3 OFF
GpioDataRegs.GPACLEAR.bit.GPIO7 = 1; // LED13 Off
FwdBckOffset = 1.5;
turnrate = 0;
}
else if (frequency>1030 && frequency<1185){ //1110Hz input
// Column 3 ON
GpioDataRegs.GPASET.bit.GPIO7 = 1; // LED13 On
// Column 1 OFF
GpioDataRegs.GPACLEAR.bit.GPIO22 = 1; // LED1 Off
// Column 2 OFF
GpioDataRegs.GPDCLEAR.bit.GPIO111 = 1; // LED7 Off
turnrate = -10.0;
FwdBckOffset = -1.0;
}
else {
// Column 1 OFF
GpioDataRegs.GPACLEAR.bit.GPIO22 = 1; // LED1 Off
// Column 2 OFF
GpioDataRegs.GPDCLEAR.bit.GPIO111 = 1; // LED7 Off
// Column 3 OFF
GpioDataRegs.GPACLEAR.bit.GPIO7 = 1; // LED13 Off
turnrate = 0;
FwdBckOffset = 0;
}
yb_1 = yb; //save audio voltage vvalue to previosu state
// Print ADCIND0 and ADCIND1’s voltage value to TeraTerm every 0.25ms
ADCB_count++;
// if ((ADCB_count % 700) == 0) {
// UARTPrint = 1;
// }
AdcbRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //clear interrupt flag
PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
}
__interrupt void SPIB_isr(void){
numSPIB_ISRcalls++;
// Code inside SPIB Interrupt Service Routine
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;
accelx = accelXraw*4.0/32767.0; // g's
accely = accelYraw*4.0/32767.0; // g's
accelz = accelZraw*4.0/32767.0; // g's
gyrox = gyroXraw*(250.0)/32767.0; // rad/s
gyroy = gyroYraw*(250.0)/32767.0; // rad/s
gyroz = gyroZraw*(250.0)/32767.0; // rad/s
//Code to be copied into SPIB_ISR interrupt function after the IMU measurements have been collected.
if(calibration_state == 0){
calibration_count++;
if (calibration_count == 2000) {
calibration_state = 1;
calibration_count = 0;
}
} else if(calibration_state == 1){
accelx_offset+=accelx;
accely_offset+=accely;
accelz_offset+=accelz;
gyrox_offset+=gyrox;
gyroy_offset+=gyroy;
gyroz_offset+=gyroz;
calibration_count++;
if (calibration_count == 2000) {
calibration_state = 2;
accelx_offset/=2000.0;
accely_offset/=2000.0;
accelz_offset/=2000.0;
gyrox_offset/=2000.0;
gyroy_offset/=2000.0;
gyroz_offset/=2000.0;
calibration_count = 0;
doneCal = 1;
}
} else if(calibration_state == 2){
accelx -=(accelx_offset);
accely -=(accely_offset);
accelz -=(accelz_offset-accelzBalancePoint);
gyrox -= gyrox_offset;
gyroy -= gyroy_offset;
gyroz -= gyroz_offset;
/*--------------Kalman Filtering code start---------------------------------------------------------------------*/
float tiltrate = (gyrox*PI)/180.0; // rad/s
float pred_tilt, z, y, S;
// Prediction Step
pred_tilt = kalman_tilt + T*tiltrate;
pred_P = kalman_P + Q;
// Update Step
z = -accelz;
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;
}
void setupSpib(void) //Call this function in main() somewhere after the DINT; line of code.
{
int16_t temp = 0;
//************ Step 1. ****************
// cut and paste here all the SpibRegs initializations you found for part 3. Change so that 16 bits are
// transmitted each TX FIFO write and change the delay in between each transfer to 0. Also don’t forget to
// cut and paste the GPIO settings for GPIO2, 63, 64, 65, 66 which are also a part of the SPIB setup.
//Here we go
GPIO_SetupPinMux(2, GPIO_MUX_CPU1, 0); // Set as GPIO2 and used as DAN777 SS
GPIO_SetupPinOptions(2, GPIO_OUTPUT, GPIO_PUSHPULL); // Make GPIO2 an Output Pin
GpioDataRegs.GPASET.bit.GPIO2 = 1; //Initially Set GPIO2/SS High so DAN777 is not selected
GPIO_SetupPinMux(66, GPIO_MUX_CPU1, 0); // Set as GPIO66 and used as MPU-9250 SS
GPIO_SetupPinOptions(66, GPIO_OUTPUT, GPIO_PUSHPULL); // Make GPIO66 an Output Pin
GpioDataRegs.GPCSET.bit.GPIO66 = 1; //Initially Set GPIO66/SS High so MPU-9250 is not selected
GPIO_SetupPinMux(63, GPIO_MUX_CPU1, 15); //Set GPIO63 pin to SPISIMOB
GPIO_SetupPinMux(64, GPIO_MUX_CPU1, 15); //Set GPIO64 pin to SPISOMIB
GPIO_SetupPinMux(65, GPIO_MUX_CPU1, 15); //Set GPIO65 pin to SPICLKB
EALLOW;
GpioCtrlRegs.GPBPUD.bit.GPIO63 = 0; // Enable Pull-ups on SPI PINs Recommended by TI for SPI Pins
GpioCtrlRegs.GPCPUD.bit.GPIO64 = 0;
GpioCtrlRegs.GPCPUD.bit.GPIO65 = 0;
GpioCtrlRegs.GPBQSEL2.bit.GPIO63 = 3; // Set prequalifier for SPI PINS
GpioCtrlRegs.GPCQSEL1.bit.GPIO64 = 3; // The prequalifier eliminates short noise spikes
GpioCtrlRegs.GPCQSEL1.bit.GPIO65 = 3; // by making sure the serial pin stays low for 3 clock periods.
EDIS;
// ---------------------------------------------------------------------------
SpibRegs.SPICCR.bit.SPISWRESET = 0; // Put SPI in Reset
SpibRegs.SPICTL.bit.CLK_PHASE = 1; //This happens to be the mode for both the DAN777 and
SpibRegs.SPICCR.bit.CLKPOLARITY = 0; //The MPU-9250, Mode 01.
SpibRegs.SPICTL.bit.MASTER_SLAVE = 1; // Set to SPI Master
SpibRegs.SPICCR.bit.SPICHAR = 0xF; // Set to transmit and receive 16 bits each write to SPITXBUF
...
This file has been truncated, please download it to see its full contents.
Comments