//#############################################################################
// 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 "LEDPatterns.h"
#include "song.h"
#include "dsp.h"
#include "fpu32/fpu_rfft.h"
#define PI 3.1415926535897932384626433832795
#define TWOPI 6.283185307179586476925286766559
#define HALFPI 1.5707963267948966192313216916398
//*****************************************************************************
// the defines for FFT
//*****************************************************************************
#define RFFT_STAGES 10
#define RFFT_SIZE (1 << RFFT_STAGES)
//*****************************************************************************
// the globals
//*****************************************************************************
#ifdef __cplusplus
#pragma DATA_SECTION("FFT_buffer_2")
#else
#pragma DATA_SECTION(pwrSpec, "FFT_buffer_2")
#endif
float pwrSpec[(RFFT_SIZE/2)+1];
float maxpwr = 0;
int16_t maxpwrindex = 0;
#ifdef __cplusplus
#pragma DATA_SECTION("FFT_buffer_2")
#else
#pragma DATA_SECTION(test_output, "FFT_buffer_2")
#endif
float test_output[RFFT_SIZE];
#ifdef __cplusplus
#pragma DATA_SECTION("FFT_buffer_1")
#else
#pragma DATA_SECTION(ping_input, "FFT_buffer_1")
#endif
float ping_input[RFFT_SIZE];
#ifdef __cplusplus
#pragma DATA_SECTION("FFT_buffer_1")
#else
#pragma DATA_SECTION(pong_input, "FFT_buffer_1")
#endif
float pong_input[RFFT_SIZE];
#ifdef __cplusplus
#pragma DATA_SECTION("FFT_buffer_2")
#else
#pragma DATA_SECTION(RFFTF32Coef,"FFT_buffer_2")
#endif //__cplusplus
//! \brief Twiddle Factors
//!
float RFFTF32Coef[RFFT_SIZE];
//! \brief Object of the structure RFFT_F32_STRUCT
//!
RFFT_F32_STRUCT rfft;
//! \brief Handle to the RFFT_F32_STRUCT object
//!
RFFT_F32_STRUCT_Handle hnd_rfft = &rfft;
// Interrupt Service Routines predefinition
__interrupt void cpu_timer0_isr(void);
__interrupt void cpu_timer1_isr(void);
__interrupt void cpu_timer2_isr(void);
__interrupt void SWI_isr(void);
__interrupt void SPIB_isr(void); //Interrupt SPIB Predefinition
__interrupt void ADCA_ISR(void);
__interrupt void ADCB_ISR(void);
void serialRXA(serial_t *s, char data);
void setEPWM8A_RCServo(float angle); // EPWM8A Set function
void setEPWM8B_RCServo(float angle); // EPWM8B Set function
void setupSpib(void); //SetupSpib function definition
void init_eQEPs(void); //eQEP function definition
int16_t updown3 = 1; // Updown counting variable
float gvalue = 0; // global float variabe for angle in Servo
//int16_t temp = 0; //global temp variable
//Function definition for reading wheel turn in radians
float readEncLeft(void);
float readEncRight(void);
//Function definition for DC Motors
void SetEPWM2A(float controleffort);
void SetEPWM2B(float controleffort);
// Count variables
uint32_t numTimer0calls = 0;
uint32_t numSWIcalls = 0;
uint32_t numRXA = 0;
uint16_t UARTPrint = 0;
uint16_t LEDdisplaynum = 0;
uint32_t SPIB_Calls = 0;
uint16_t ADCA_Calls = 0;
uint16_t DanceCalls = 0;
uint16_t ADCBcalls = 0;
//Variable definitions for Steering and Balance Control
float LDistance = 0;
float RDistance = 0;
float uLeft=0;
float uRight=0;
float PosLeft_K = 0;
float PosLeft_K_1 = 0;
float PosRight_K = 0;
float PosRight_K_1 = 0;
float VLeftK = 0;
float VRightK = 0;
float LeftWheel = 0;
float RightWheel = 0;
float Vref=0;
float ErrorLeft=0;
float ErrorLeftK_1=0;
float ILeftK=0;
float ILeftK_1=0;
float ErrorRight=0;
float ErrorRightK_1=0;
float IRightK=0;
float IRightK_1=0;
float eTurn=0;
float turn = 0;
float eLeft=0;
float eRight=0;
float Kturn = 3;
float RobotWidth = 0.61;
float RadiusWheel = 0.10625;
float ThetaRight_K=0;
float ThetaLeft_K=0;
float ThetaRight_K_1=0;
float ThetaLeft_K_1=0;
float ThetaAvg=0;
float DThetaAvg=0;
float RobotPoseAngle=0;
float XPose_K=0;
float XPose_K_1=0;
float YPose_K=0;
float YPose_K_1=0;
float IXPose_K=0;
float IXPose_K_1=0;
float IYPose_K=0;
float IYPose_K_1=0;
float vel_Right_K;
float vel_Right_K_1;
float vel_Left_K;
float vel_Left_K_1;
float WhlDiff;
float WhlDiff_1;
float vel_WhlDiff;
float vel_WhlDiff_1;
float turnref;
float errorDiff;
float errorDiff_1;
float IerrorDiff;
float IerrorDiff_1;
float kp = 3.0;
float ki = 20.0;
float kd = 0.08;
float FwdBackOffset = 0;
float turnrate;
float turnrate_1;
float turnref_1;
float k1 = -60;
float k2 = -4.5;
float k3 = -1.1;
float Ubal = 0;
//Global variables to read ADC Values
float spivalue2_volt = 0;
float spivalue3_volt = 0;
//Global Variables to read accelerometer and gyro values
int16_t AccelX = 0;
int16_t AccelY = 0;
int16_t AccelZ = 0;
int16_t GyroX = 0;
int16_t GyroY = 0;
int16_t GyroZ = 0;
float accelx = 0;
float accely = 0;
float accelz = 0;
float gyrox = 0;
float gyroy = 0;
float gyroz = 0;
//ADCA Results variables
int16_t ADCINA2_results = 0;
int16_t ADCINA3_results = 0;
//ADCA voltage variables
float ADCINA2_volt = 0;
float ADCINA3_volt = 0;
// Needed global Variables for Kalman Filter
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 = -.78;
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;
//Global variables to read RX FIFO
int16_t spivalue1 = 0;
int16_t spivalue2 = 0;
int16_t spivalue3 = 0;
//Global variables for ping and pong
float runping = 1;
float runpong = 0;
uint16_t ping_pong_index = 0;
float ping = 1;
float pong = 0;
//ADCB voltage variables
float ADCINB4_volt = 0;
//ADCB Results variables
int16_t ADCINB4_results = 0;
//Global variables for beat detection state machine
float a = 0;
float b = 0;
float c = 0;
float d = 0;
float e = 0;
float f = 0;
float g = 0;
float h = 0;
float z = 0;
char tempnote;
//Global variables for dance routine
float danceturnseq[32] = {4,4,4,4, 6,6,6,6, -4,-4,-4,-4, -6,-6,-6,-6, 0,0,0,0, 0,0,0,0, 4,4,4,4, 6,6,6,6};
float fwdbackseq[32] = {0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0.75,0.75,0.75,0.75, -0.75,-0.75,-0.75,-0.75, 0,0,0,0, 0,0,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(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;
//WIZNET Reset
GPIO_SetupPinMux(0, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(0, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPASET.bit.GPIO0 = 1;
//ESP8266 Reset
GPIO_SetupPinMux(1, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(1, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPASET.bit.GPIO1 = 1;
//SPIRAM CS Chip Select
GPIO_SetupPinMux(19, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(19, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPASET.bit.GPIO19 = 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;
//DAN28027 CS Chip Select
GPIO_SetupPinMux(9, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(9, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPASET.bit.GPIO9 = 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;
//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.ADCA1_INT = &ADCA_ISR; //Add ADCA to PieVect Table
PieVectTable.SPIB_RX_INT = &SPIB_isr; //Add SPIB to PieVectTable
PieVectTable.ADCB1_INT = &ADCB_ISR; //Add ADCB to PieVect Table
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, 500000);
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);
setupSpib(); //Call SetupSpib function
//EPWM4 Initialization to trigger ADCB
EALLOW;
EPwm4Regs.ETSEL.bit.SOCAEN = 0; // Disable SOC on A group
EPwm4Regs.TBCTL.bit.CTRMODE = 3; // freeze counter
EPwm4Regs.ETSEL.bit.SOCASEL = 2; // Select Event when counter equal to PRD
EPwm4Regs.ETPS.bit.SOCAPRD = 1; // Generate pulse on 1st event (“pulse” is the same as “trigger”)
EPwm4Regs.TBCTR = 0x0; // Clear counter
EPwm4Regs.TBPHS.bit.TBPHS = 0x0000; // Phase is 0
EPwm4Regs.TBCTL.bit.PHSEN = 0; // Disable phase loading
EPwm4Regs.TBCTL.bit.CLKDIV = 0; // divide by 1 50Mhz Clock
EPwm4Regs.TBPRD = 5000; // Set period to 0.1 ms for last part in exercise 4
// Notice here that we are not setting CMPA or CMPB because we are not using the PWM signal
EPwm4Regs.ETSEL.bit.SOCAEN = 1; //enable SOCA
EPwm4Regs.TBCTL.bit.CTRMODE = 0; //unfreeze, and enter up count mode
EDIS;
//EPWM8 Initialization
EPwm8Regs.TBCTL.bit.CTRMODE = 0; // Set to count up mode
EPwm8Regs.TBCTL.bit.FREE_SOFT = 2; // Set Free soft emulation mode to free run
EPwm8Regs.TBCTL.bit.PHSEN = 0; // Disable phase loading
EPwm8Regs.TBCTL.bit.CLKDIV = 4; // Clock divide by 16
EPwm8Regs.TBCTR = 0; // Timers start at zero
EPwm8Regs.TBPRD = 62500; // Period
EPwm8Regs.CMPA.bit.CMPA = 0; // Duty cycle at 0
EPwm8Regs.CMPB.bit.CMPB = 0; // Duty cycle at 0
EPwm8Regs.AQCTLA.bit.CAU = 1; // Clear when CMPA is reached
EPwm8Regs.AQCTLB.bit.CBU = 1; // Clear when CMPB is reached
EPwm8Regs.AQCTLA.bit.ZRO = 2; // Set when TBCTR = 0
EPwm8Regs.AQCTLB.bit.ZRO = 2; // Set when TBCTR = 0
EPwm8Regs.TBPHS.bit.TBPHS = 0; // Phase zero
init_eQEPs(); // Call eQEP function
//EPWM2 Initialization
EPwm2Regs.TBCTL.bit.CTRMODE = 0; // Set to count up mode
EPwm2Regs.TBCTL.bit.FREE_SOFT = 2; // Set Free soft emulation mode to free run
EPwm2Regs.TBCTL.bit.PHSEN = 0; // Disable phase loading
EPwm2Regs.TBCTL.bit.CLKDIV = 0; // Clock divide by 1
EPwm2Regs.TBCTR = 0; // Timers start at zero
EPwm2Regs.TBPRD = 2500; // Period
EPwm2Regs.CMPA.bit.CMPA = 0; // Duty cycle at 0
EPwm2Regs.CMPB.bit.CMPB = 0; // Duty cycle at 0
EPwm2Regs.AQCTLA.bit.CAU = 1; //Clear when CMPA is reached
EPwm2Regs.AQCTLB.bit.CBU = 1; //Clear when CMPB is reached
EPwm2Regs.AQCTLA.bit.ZRO = 2; // Set when TBCTR = 0
EPwm2Regs.AQCTLB.bit.ZRO = 2; // Set when TBCTR = 0
EPwm2Regs.TBPHS.bit.TBPHS = 0; // Phase zero
GPIO_SetupPinMux(2,GPIO_MUX_CPU1,1); // PinMux for EPWM2A
GPIO_SetupPinMux(3,GPIO_MUX_CPU1,1); // PinMux for EPWM2B
GpioCtrlRegs.GPAPUD.bit.GPIO2 = 1; // For EPWM2A
GpioCtrlRegs.GPAPUD.bit.GPIO3 = 1; // For EPWM2B
//EPWM5 Initialization to trigger ADCA
EALLOW;
EPwm5Regs.ETSEL.bit.SOCAEN = 0; // Disable SOC on A group
EPwm5Regs.TBCTL.bit.CTRMODE = 3; // freeze counter
EPwm5Regs.ETSEL.bit.SOCASEL = 2; // Select Event when counter equal to PRD
EPwm5Regs.ETPS.bit.SOCAPRD = 1; // Generate pulse on 1st event (“pulse” is the same as “trigger”)
EPwm5Regs.TBCTR = 0x0; // Clear counter
EPwm5Regs.TBPHS.bit.TBPHS = 0x0000; // Phase is 0
EPwm5Regs.TBCTL.bit.PHSEN = 0; // Disable phase loading
EPwm5Regs.TBCTL.bit.CLKDIV = 0; // divide by 1 50Mhz Clock
EPwm5Regs.TBPRD = 50000; // Set Period to 1ms sample. Input clock is 50MHz.
//EPwm5Regs.TBPRD = 12500; // Set period to 0.25 ms for exercise 4
//EPwm5Regs.TBPRD = 5000; // Set period to 0.1 ms for last part in exercise 4
// 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;
//ADC Initialization
EALLOW;
//write configurations for all ADCs ADCA, ADCB, ADCC, ADCD
AdcaRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
AdcbRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
AdccRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
AdcdRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
AdcSetMode(ADC_ADCA, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE); //read calibration settings
AdcSetMode(ADC_ADCB, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE); //read calibration settings
AdcSetMode(ADC_ADCC, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE); //read calibration settings
AdcSetMode(ADC_ADCD, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE); //read calibration settings
//Set pulse positions to late
AdcaRegs.ADCCTL1.bit.INTPULSEPOS = 1;
AdcbRegs.ADCCTL1.bit.INTPULSEPOS = 1;
AdccRegs.ADCCTL1.bit.INTPULSEPOS = 1;
AdcdRegs.ADCCTL1.bit.INTPULSEPOS = 1;
//power up the ADCs
AdcaRegs.ADCCTL1.bit.ADCPWDNZ = 1;
AdcbRegs.ADCCTL1.bit.ADCPWDNZ = 1;
AdccRegs.ADCCTL1.bit.ADCPWDNZ = 1;
AdcdRegs.ADCCTL1.bit.ADCPWDNZ = 1;
//delay for 1ms to allow ADC time to power up
DELAY_US(1000);
//Select the channels to convert and end of conversion flag
//Many statements commented out, To be used when using ADCA or ADCB
//ADCA
AdcaRegs.ADCSOC0CTL.bit.CHSEL = 2; //SOC0 will convert Channel you choose Does not have to be A0
AdcaRegs.ADCSOC0CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
AdcaRegs.ADCSOC0CTL.bit.TRIGSEL = 0xD;// EPWM5 ADCSOCA or another trigger you choose will trigger SOC0
AdcaRegs.ADCSOC1CTL.bit.CHSEL = 3; //SOC1 will convert Channel you choose Does not have to be A1
AdcaRegs.ADCSOC1CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
AdcaRegs.ADCSOC1CTL.bit.TRIGSEL = 0xD;// EPWM5 ADCSOCA or another trigger you choose will trigger SOC1
AdcaRegs.ADCINTSEL1N2.bit.INT1SEL = 1; //set to last SOC that is converted and it will set INT1 flag ADCA1
AdcaRegs.ADCINTSEL1N2.bit.INT1E = 1; //enable INT1 flag
AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //make sure INT1 flag is cleared
//ADCB
AdcbRegs.ADCSOC0CTL.bit.CHSEL = 4; //SOC0 will convert Channel you choose Does not have to be B0
AdcbRegs.ADCSOC0CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
AdcbRegs.ADCSOC0CTL.bit.TRIGSEL = 0xB;// EPWM4 ADCSOCA or another trigger you choose will trigger SOC0
//AdcbRegs.ADCSOC1CTL.bit.CHSEL = ???; //SOC1 will convert Channel you choose Does not have to be B1
//AdcbRegs.ADCSOC1CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
//AdcbRegs.ADCSOC1CTL.bit.TRIGSEL = ???;// EPWM5 ADCSOCA or another trigger you choose will trigger SOC1
//AdcbRegs.ADCSOC2CTL.bit.CHSEL = ???; //SOC2 will convert Channel you choose Does not have to be B2
//AdcbRegs.ADCSOC2CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
//AdcbRegs.ADCSOC2CTL.bit.TRIGSEL = ???;// EPWM5 ADCSOCA or another trigger you choose will trigger SOC2
//AdcbRegs.ADCSOC3CTL.bit.CHSEL = ???; //SOC3 will convert Channel you choose Does not have to be B3
//AdcbRegs.ADCSOC3CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
//AdcbRegs.ADCSOC3CTL.bit.TRIGSEL = ???;// EPWM5 ADCSOCA or another trigger you choose will trigger SOC3
AdcbRegs.ADCINTSEL1N2.bit.INT1SEL = 0; //set to last SOC that is converted and it will set INT1 flag ADCB1
AdcbRegs.ADCINTSEL1N2.bit.INT1E = 1; //enable INT1 flag
AdcbRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //make sure INT1 flag is cleared
EDIS;
// 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; //Enable major interrupt
// Enable TINT0 in the PIE: Group 1 interrupt 7
PieCtrlRegs.PIEIER1.bit.INTx7 = 1;
// Enable SWI in the PIE: Group 12 interrupt 9
PieCtrlRegs.PIEIER12.bit.INTx9 = 1;
PieCtrlRegs.PIEIER6.bit.INTx3 = 1; //enable PIEIER
//Interrupt ADCB
PieCtrlRegs.PIEIER1.bit.INTx2 = 1;
//Interrupt for ADCA
PieCtrlRegs.PIEIER1.bit.INTx1 = 1;
int16_t i = 0;
// float samplePeriod = 0.0002;
// Clear input buffers:
for(i=0; i < RFFT_SIZE; i++){
ping_input[i] = 0.0f;
}
// for (i=0;i<RFFT_SIZE;i++) {
// ping_input[i] = sin(125*2*PI*i*samplePeriod)+2*sin(2400*2*PI*i*samplePeriod);
// }
hnd_rfft->FFTSize = RFFT_SIZE;
hnd_rfft->FFTStages = RFFT_STAGES;
hnd_rfft->InBuf = &ping_input[0]; //Input buffer
hnd_rfft->OutBuf = &test_output[0]; //Output buffer
hnd_rfft->MagBuf = &pwrSpec[0]; //Magnitude buffer
hnd_rfft->CosSinBuf = &RFFTF32Coef[0]; //Twiddle factor buffer
RFFT_f32_sincostable(hnd_rfft); //Calculate twiddle factor
for (i=0; i < RFFT_SIZE; i++){
test_output[i] = 0; //Clean up output buffer
}
for (i=0; i <= RFFT_SIZE/2; i++){
pwrSpec[i] = 0; //Clean up magnitude buffer
}
// run fft 10 times just to under what the FFT does
// int16_t tries = 0;
// while(tries < 10) {
// hnd_rfft->InBuf = &ping_input[0]; //Input buffer
// RFFT_f32(hnd_rfft); //Calculate real FFT
//
//#ifdef __TMS320C28XX_TMU__ //defined when --tmu_support=tmu0 in the project
// // properties
// RFFT_f32_mag_TMU0(hnd_rfft); //Calculate magnitude
//#else
// RFFT_f32_mag(hnd_rfft); //Calculate magnitude
//#endif
// maxpwr = 0;
// maxpwrindex = 0;
//
// for (i=0;i<(RFFT_SIZE/2);i++) {
// if (pwrSpec[i]>maxpwr) {
// maxpwr = pwrSpec[i];
// maxpwrindex = i;
// }
// }
//
// tries++;
// for (i=0;i<RFFT_SIZE;i++) {
// ping_input[i] = sin((125 + tries*125)*2*PI*i*samplePeriod)+2*sin((2400-tries*200)*2*PI*i*samplePeriod);
// }
// }
// Enable global Interrupts and higher priority real-time debug events
EINT; // Enable Global interrupt INTM
ERTM; // Enable Global realtime interrupt DBGM
// IDLE loop. Just sit and loop forever (optional):
while(1)
{
if (UARTPrint == 1 ) {
serial_printf(&SerialA,"Tilt Value: %.3f Gyro Value: %.3f Left Wheel Angle: %.3f Right Wheel Angle: %.3f MaxPwrIndex: %i, MaxPwr: %f \r\n",tilt_value,gyro_value,LeftWheel,RightWheel,maxpwrindex,maxpwr);
UARTPrint = 0;
}
//Run FFT Ping function
if (runping == 1) {
runping = 0;
hnd_rfft->InBuf = &ping_input[0]; //Input buffer
RFFT_f32(hnd_rfft); //Calculate real FFT
#ifdef __TMS320C28XX_TMU__ //defined when --tmu_support=tmu0 in the project
// properties
RFFT_f32_mag_TMU0(hnd_rfft); //Calculate magnitude
#else
RFFT_f32_mag(hnd_rfft); //Calculate magnitude
#endif
maxpwr = 0;
maxpwrindex = 0;
for (i=5;i<(RFFT_SIZE/2);i++) {
if (pwrSpec[i]>maxpwr) {
maxpwr = pwrSpec[i];
maxpwrindex = i;
}
}
UARTPrint = 1;
// State Machine based on maxpwrindex and maxpwr values
if(maxpwrindex == 17 && maxpwr > 120){
a = 1; //Flag for Dance Routine
}
else if(maxpwrindex == 18 && maxpwr > 120){
b = 1; //Flag for Dance Routine
}
else if(maxpwrindex == 20 && maxpwr > 120){
c = 1; //Flag for Dance Routine
}
else if(maxpwrindex == 34 && maxpwr > 130){
d = 1; //Flag for decreasing turn rate
}
else if(maxpwrindex == 36 && maxpwr > 130){
e = 1; //State for increasing turn rate
}
else if(maxpwrindex == 40 && maxpwr > 130){
f = 1; //State for decreasing FwdBackOffset
}
else if(maxpwrindex == 45 && maxpwr > 130){
g = 1; //State for increasing FwdBackOffset
}
else if(maxpwrindex == 25 && maxpwr > 130){
h = 1; //State for setting all offset values to zero
}
}
//Run FFT Pong Function
if (runpong == 1) {
runpong = 0;
hnd_rfft->InBuf = &pong_input[0]; //Input buffer
RFFT_f32(hnd_rfft); //Calculate real FFT
#ifdef __TMS320C28XX_TMU__ //defined when --tmu_support=tmu0 in the project
// properties
RFFT_f32_mag_TMU0(hnd_rfft); //Calculate magnitude
#else
RFFT_f32_mag(hnd_rfft); //Calculate magnitude
#endif
maxpwr = 0;
maxpwrindex = 0;
for (i=5;i<(RFFT_SIZE/2);i++) {
if (pwrSpec[i]>maxpwr) {
maxpwr = pwrSpec[i];
maxpwrindex = i;
}
}
// State Machine based on maxpwrindex and maxpwr values
if(maxpwrindex == 17 && maxpwr > 120){
a = 1; //Flag for Dance routine
}
else if(maxpwrindex == 18 && maxpwr > 120){
b = 1; // Flag for Dance Routine
}
else if(maxpwrindex == 20 && maxpwr > 120){
c = 1; //Flag for Dance Routine
}
else if(maxpwrindex == 34 && maxpwr > 130){
d = 1; //State for decreasing turn rate
}
else if(maxpwrindex == 36 && maxpwr > 130){
e = 1; //State for increasing turn rate
}
else if(maxpwrindex == 40 && maxpwr > 130){
f = 1; //State for decreasing FwdBackOffset
}
else if(maxpwrindex == 45 && maxpwr > 130){
g = 1; //State for increasing FwdBackOffset
}
else if(maxpwrindex == 25 && maxpwr > 130){
h = 1; //State for setting all offset values to zero
}
}
}
}
// 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.......
//Silence set tempnote to 'x'
if((maxpwrindex <20)){
tempnote='x';
}
//If silence is detected then wait for note detection
if(tempnote =='x'){
if((d == 1) && (tempnote != 'd')){
turnrate = turnrate - 0.2;
tempnote = 'd';
d = 0;
}
else if ((e == 1) && (tempnote != 'e' )){
turnrate = turnrate + 0.2;
tempnote = 'e';
e = 0;
}
else if ((f == 1) && (tempnote != 'f')){
FwdBackOffset = FwdBackOffset - 0.2;
tempnote = 'f';
f = 0;
}
else if ((g == 1) && (tempnote != 'g')){
FwdBackOffset = FwdBackOffset + 0.2;
tempnote = 'g';
g = 0;
}
else if ((h == 1) && (tempnote != 'h')){
turnrate = 0;
FwdBackOffset = 0;
tempnote = 'h';
h = 0;
a = 0;
b = 0;
c = 0;
}
else{
tempnote = 'z'; //Set tempnote to 'z' if the same note is heard twice
}
}
//Read wheen angles in radians
PosLeft_K = LeftWheel;
PosRight_K = RightWheel;
//Calculate velocity
vel_Left_K = 0.6*vel_Left_K_1 + 100*(PosLeft_K - PosLeft_K_1);
vel_Right_K = 0.6*vel_Right_K_1 + 100*(PosRight_K - PosRight_K_1);
//Calculate balance control
Ubal = -k1*tilt_value - k2*gyro_value - k3*(vel_Left_K + vel_Right_K)/2.0;
//Calculate difference in angle between wheels
WhlDiff = LeftWheel - RightWheel;
//Rate of change of wheel difference
vel_WhlDiff = 0.333*vel_WhlDiff_1 + 166.667*(WhlDiff - WhlDiff_1);
//Error between turnref and feedback signal
errorDiff = turnref - WhlDiff;
//Integrate error diff
IerrorDiff = IerrorDiff_1 + ((errorDiff + errorDiff_1)/2.0)*0.004;
//Calculate turn command
turn = kp*errorDiff + ki*IerrorDiff - kd*vel_WhlDiff;
//Guard against integral windup
if(fabs(turn) > 3){
IerrorDiff = IerrorDiff_1;
}
//Saturate between -4 and 4
if(turn > 4){
turn = 4;
}
else if(turn < -4){
turn = -4;
}
//Calcualte turnrate
turnref = turnref_1 + ((turnrate + turnrate_1)/2.0)*0.004;
//uRight and uLeft with steering and forward/back movement
uRight = Ubal/2.0 - turn + FwdBackOffset;
uLeft = Ubal/2.0 + turn + FwdBackOffset;
SetEPWM2B(-uLeft);
SetEPWM2A(uRight);
//Update states
PosLeft_K_1 = PosLeft_K;
PosRight_K_1 = PosRight_K;
vel_Right_K_1 = vel_Right_K;
vel_Left_K_1 = vel_Left_K;
//Update states
WhlDiff_1 = WhlDiff;
vel_WhlDiff_1 = vel_WhlDiff;
IerrorDiff_1 = IerrorDiff;
errorDiff_1 = errorDiff;
turnrate_1 = turnrate;
turnref_1 = turnref;
numSWIcalls++;
DINT;
}
// cpu_timer0_isr - CPU Timer0 ISR
__interrupt void cpu_timer0_isr(void)
{
CpuTimer0.InterruptCount++;
numTimer0calls++;
// if ((numTimer0calls%50) == 0) {
// PieCtrlRegs.PIEIFR12.bit.INTx9 = 1; // Manually cause the interrupt for the SWI
// }
/*
if ((numTimer0calls%250) == 0) {
displayLEDletter(LEDdisplaynum);
LEDdisplaynum++;
if (LEDdisplaynum == 0xFFFF) { // prevent roll over exception
...
This file has been truncated, please download it to see its full contents.
Comments