//#############################################################################
// FILE: finalproject_main.c
//
// TITLE: finalproject
// HELPFUL CODE FOR MAGNETOMETER: https://www.hackster.io/evanah2/comparing-direction-of-travel-methods-me461-uiuc-f14c92
//#############################################################################
// 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 "buzzer_sounds.h"
#include "dsp.h"
#include "fpu32/fpu_rfft.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 ADCD_ISR(void);
__interrupt void SWI_isr(void);
__interrupt void SPIB_isr(void);
void setupSpib(void);
void init_eQEPs(void);
float readEncRight(void);
float readEncLeft(void);
void setEPWM2A(float controleffort);
void setEPWM2B(float controleffort);
float uLeft = 5.0;
float uRight = 5.0;
void serialRXA(serial_t *s, char data);
// Count variables
uint32_t numTimer0calls = 0;
uint32_t numSWIcalls = 0;
uint32_t numRXA = 0;
uint16_t UARTPrint = 0;
uint16_t LEDdisplaynum = 0;
uint16_t adcdcount = 0;
uint16_t adcd0result = 0;
uint16_t adcd1result = 0;
uint16_t state = 10;
uint16_t songstate = 0;
int16_t songspot = 0;
float IR1Volt = 0.0;
float IR2Volt = 0.0;
float accelX = 0.0;
float accelY = 0.0;
float accelZ = 0.0;
float gyroX = 0.0;
float gyroY = 0.0;
float gyroZ = 0.0;
float magX = 0.0;
float magY = 0.0;
float magZ = 0.0;
float magX_offset = (30 + -70)/2.0;
float magY_offset = (105 + 3)/2.0;
float magZ_offset = (70 + -37)/2.0;
float compass_angle = 0.0;
float compass_avg = 0.0;
float eC = 0.0;
float Kc = 0.055;
float leftwheel = 0.0;
float rightwheel = 0.0;
float leftfeet = 0.0;
float rightfeet = 0.0;
float leftposK_1 = 0.0;
float rightposK_1 = 0.0;
float VLeft = 0.0;
float VRight = 0.0;
float Kp = 3.0;
float Ki = 25.0;
float Kturn = 3.0;
float Vref = 0.0;
float turn = 0.0;
float eTurn = 0.0;
float eR = 0.0;
float eR_1 = 0.0;
float eL = 0.0;
float eL_1 = 0.0;
float IR = 0.0;
float IR_1 = 0.0;
float IL = 0.0;
float IL_1 = 0.0;
float RobWid = 0.61;
float WheelRad = 0.10625;
float RotRight = 0.0;
float RotLeft = 0.0;
float PoseAngle = 0.0;
float theta_avg = 0.0;
float thetadot_avg = 0.0;
float xRdot = 0.0;
float yRdot = 0.0;
float xRdotK_1 = 0.0;
float yRdotK_1 = 0.0;
float xR = 0.0;
float yR = 0.0;
float XCoord = 4.0;
float YCoord = 4.0;
float XCoordK_1 = 0.0;
float YCoordK_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(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.SPIB_RX_INT = &SPIB_isr;
PieVectTable.ADCD1_INT = &ADCD_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); //SPI currently using CPUTimer0 for interrupt 10ms default
ConfigCpuTimer(&CpuTimer1, 200, 4000); // Set to timeout every 4 ms
ConfigCpuTimer(&CpuTimer2, 200, 125000); // Set to have each note of each song play for 125 ms
// 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);
EPwm2Regs.TBCTL.bit.CTRMODE = 0; // motor initializations
EPwm2Regs.TBCTL.bit.FREE_SOFT = 2;
EPwm2Regs.TBCTL.bit.CLKDIV = 0;
EPwm2Regs.TBCTR = 0;
EPwm2Regs.TBPRD = 2500;
EPwm2Regs.CMPA.bit.CMPA = 0;
EPwm2Regs.CMPB.bit.CMPB = 0;
EPwm2Regs.AQCTLA.bit.CAU = 1;
EPwm2Regs.AQCTLB.bit.CBU = 1;
EPwm2Regs.AQCTLA.bit.ZRO = 2;
EPwm2Regs.AQCTLB.bit.ZRO = 2;
EPwm2Regs.TBPHS.bit.TBPHS = 0;
EPwm9Regs.TBCTL.bit.CTRMODE = 0; // buzzer initializations
EPwm9Regs.TBCTL.bit.FREE_SOFT = 2;
EPwm9Regs.TBCTL.bit.CLKDIV = 1;
EPwm9Regs.TBCTR = 0;
EPwm9Regs.TBPRD = 2500;
// EPwm9Regs.CMPA.bit.CMPA = 0;
EPwm9Regs.AQCTLA.bit.CAU = 0; // 0 ignores the CMPA register
EPwm9Regs.AQCTLA.bit.ZRO = 3; // 3 makes allows for the frequency to change when TBPRD changes
EPwm9Regs.TBPHS.bit.TBPHS = 0;
GPIO_SetupPinMux(2, GPIO_MUX_CPU1, 1); // Mux changed so right motor PWM is with EPWM2A
GPIO_SetupPinMux(3, GPIO_MUX_CPU1, 1); // Mux changed so left motor PWM is with EPWM2B
GPIO_SetupPinMux(16, GPIO_MUX_CPU1, 5); // Mux changed so buzzer is with EPWM9A
GPIO_SetupPinMux(9, GPIO_MUX_CPU1, 0); // Set as GPIO9 and used as DAN28027 SS
GPIO_SetupPinOptions(9, GPIO_OUTPUT, GPIO_PUSHPULL); // Make GPIO9 an Output Pin
GpioDataRegs.GPASET.bit.GPIO9 = 1; //Initially Set GPIO9/SS High so DAN28027 is not selected
EALLOW; // Below are protected registers
GpioCtrlRegs.GPAPUD.bit.GPIO2 = 1; // For EPWM2A
GpioCtrlRegs.GPAPUD.bit.GPIO3 = 1; // For EPWM2B
GpioCtrlRegs.GPAPUD.bit.GPIO16 = 1; // For EPWM9A
EDIS;
EALLOW; //Here we are using EPwm5 as a timer, info on registers in EPWM tech reference
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 = 2; // divide by 4 50Mhz Clock
EPwm5Regs.TBPRD = 50000; // Set Period to 4ms sample. (50MHz input clock divided by (250Hz*clkdiv=4) is 50000)
// 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);
//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 = 13; // 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 = 13; // EPWM5 ADCSOCA will trigger SOC1
//AdcdRegs.ADCSOC2CTL.bit.CHSEL = ???; //set SOC2 to convert pin D2
//AdcdRegs.ADCSOC2CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
//AdcdRegs.ADCSOC2CTL.bit.TRIGSEL = ???; // EPWM5 ADCSOCA will trigger SOC2
//AdcdRegs.ADCSOC3CTL.bit.CHSEL = ???; //set SOC3 to convert pin D3
//AdcdRegs.ADCSOC3CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
//AdcdRegs.ADCSOC3CTL.bit.TRIGSEL = ???; // 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;
setupSpib();
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;
IER |= M_INT6;
// Enable TINT0 in the PIE: Group 1 interrupt 7
PieCtrlRegs.PIEIER1.bit.INTx7 = 1;
// Enable SWI in the PIE: Group 12 interrupt 9
PieCtrlRegs.PIEIER12.bit.INTx9 = 1;
//Enable SPIB in the PIE: Group 6 interrupt 3
PieCtrlRegs.PIEIER6.bit.INTx3 = 1;
// Enable ADCD1 in the PIE: Group 1 interrupt 6
PieCtrlRegs.PIEIER1.bit.INTx6 = 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):
while(1)
{
if (UARTPrint == 1 ) {
serial_printf(&SerialA,"IR 1:%.3f IR 2:%.3f Bearing:%.3f X:%.3f Y:%.3f pathstate:%d turn:%.3f Vref:%.3f\r\n"
,IR1Volt,IR2Volt,compass_avg,XCoord,YCoord,state,turn,Vref);
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.......
numSWIcalls++;
DINT;
}
__interrupt void ADCD_ISR (void)
{
adcd0result = AdcdResultRegs.ADCRESULT0;
adcd1result = AdcdResultRegs.ADCRESULT1;
// Here covert ADCIND0, ADCIND1 to volts
IR1Volt = 3.0/4095.0 * (float)(adcd0result);
IR2Volt = 3.0/4095.0 * (float)(adcd1result);
//Clear GPIO9 Low to act as a Slave Select.
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;
SpibRegs.SPIFFRX.bit.RXFFIL = 12; // Issue the SPIB_RX_INT when 12 values are in the RX FIFO
SpibRegs.SPITXBUF = ((0x8000)|(0x3A00)); // Start command
SpibRegs.SPITXBUF = 0x0000; // Read 3B: 0, 3C: 0 (accel x high, low)
SpibRegs.SPITXBUF = 0x0000; // Read 3D: 0, 3E: 0 (accel y high, low)
SpibRegs.SPITXBUF = 0x0000; // Read 3F: 0, 40: 0 (accel z high, low)
SpibRegs.SPITXBUF = 0x0000; // Read 41: 0, 42: 0 (Don't really need temp_out values)
SpibRegs.SPITXBUF = 0x0000; // Read 43: 0, 44: 0 (gyro x high, low)
SpibRegs.SPITXBUF = 0x0000; // Read 45: 0, 46: 0 (gyro y high, low)
SpibRegs.SPITXBUF = 0x0000; // Read 47: 0, 48: 0 (gyro z high, low)
SpibRegs.SPITXBUF = 0x0000; // Read 49: 0, 4A: 0 (EXT_SENS_DATA_00, 01) Information and Status1
SpibRegs.SPITXBUF = 0x0000; // Read 4B: 0, 4C: 0 (EXT_SENS_DATA_02 - compass X high, 03 - compass X low)
SpibRegs.SPITXBUF = 0x0000; // Read 4D: 0, 4E: 0 (EXT_SENS_DATA_04 - compass Y high, 05 - compass Y low)
SpibRegs.SPITXBUF = 0x0000; // Read 4F: 0, 50: 0 (EXT_SENS_DATA_06 - compass Z high, 07 - compass Z low)
// Print ADCIND0 and ADCIND1’s voltage value to TeraTerm every 100ms
adcdcount++;
if (adcdcount % 25 == 0) {
UARTPrint = 1;
}
AdcdRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //clear interrupt flag
PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
}
// 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
LEDdisplaynum = 0;
}
}
// Blink LaunchPad Red LED
GpioDataRegs.GPBTOGGLE.bit.GPIO34 = 1;
// Acknowledge this interrupt to receive more interrupts from group 1
PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
}
// cpu_timer1_isr - CPU Timer1 ISR
float uR = 0.0;
float uL = 0.0;
__interrupt void cpu_timer1_isr(void)
{
leftwheel = -readEncLeft(); // Reading the encoder to see how much the wheel spins
rightwheel = readEncRight(); // These values are also the radians the wheel spins
leftfeet = (float)(leftwheel)/9.8; // Converting the encoder values to distance in feet
rightfeet = (float)(rightwheel)/9.8;
VLeft = (leftfeet - leftposK_1)/0.004; // Calculating the velocity of each wheel
RotLeft = VLeft * 9.8; // Calculating the rotation angle speed by multiplying by the conversion factor that got rid of angles originally
VRight = (rightfeet - rightposK_1)/0.004;
RotRight = VRight * 9.8; // Calculating rotation angle speed
theta_avg = 0.5 * (rightwheel + leftwheel);
thetadot_avg = 0.5 * (RotRight + RotLeft);
PoseAngle = WheelRad / RobWid * (rightwheel - leftwheel);
xRdot = WheelRad * thetadot_avg * cos(PoseAngle);
yRdot = WheelRad * thetadot_avg * sin(PoseAngle);
xR = 0.004 * (xRdot + xRdotK_1) / 2.0;
yR = 0.004 * (yRdot + yRdotK_1) / 2.0;
XCoord = xR + XCoord;
YCoord = yR + YCoord;
// Assign the current values to be the previous values for the next calculation
leftposK_1 = leftfeet;
xRdotK_1 = xRdot;
XCoordK_1 = XCoord;
rightposK_1 = rightfeet;
yRdotK_1 = yRdot;
YCoordK_1 = YCoord;
// Need one of these below for each motor (k=R,L)
// eK = Vref - vK;
// IK = IK_1 + 0.004 * (eK + eK_1)/2;
// uK = Kp * eK + Ki * IK;
// eK_1 = eK;
// IK_1 = IK;
eTurn = turn + (VLeft - VRight);
eR = Vref - VRight + Kturn * eTurn;
IR = IR_1 + 0.004 * (eR + eR_1)/2;
uR = Kp * eR + Ki * IR;
eR_1 = eR;
if (fabs(uR) > 10.0){
IR = IR_1;
}
IR_1 = IR;
eL = Vref - VLeft - Kturn * eTurn;
IL = IL_1 + 0.004 * (eL + eL_1)/2;
uL = Kp * eL + Ki * IL;
eL_1 = eL;
if (fabs(uL) > 10.0){
IL = IL_1;
}
IL_1 = IL;
setEPWM2B(-uL);
setEPWM2A(uR);
CpuTimer1.InterruptCount++;
}
// cpu_timer2_isr CPU Timer2 ISR
__interrupt void cpu_timer2_isr(void)
{
// Blink LaunchPad Blue LED
GpioDataRegs.GPATOGGLE.bit.GPIO31 = 1;
CpuTimer2.InterruptCount++;
if(songstate == 0) {
EPwm9Regs.TBPRD = songarray0[songspot];
if (songspot == SONG_LENGTH0) {
songstate = 0;
}
if (songspot < SONG_LENGTH0) {
songspot++;
}
}
else if(songstate == 1) {
EPwm9Regs.TBPRD = songarray1[songspot]; // Song 1 code
if (songspot == SONG_LENGTH1) {
songstate = 0;
}
if (songspot < SONG_LENGTH1) {
songspot++;
} // incrementing through the song array until the final value of the array is reached
}
else if(songstate == 2) {
EPwm9Regs.TBPRD = songarray2[songspot]; // Song 2 code
if (songspot == SONG_LENGTH2) {
songstate = 0;
}
if (songspot < SONG_LENGTH2) {
songspot++;
} // incrementing through the song array until the final value of the array is reached
}
else if(songstate == 3) {
EPwm9Regs.TBPRD = songarray3[songspot]; // Song 3 code
if (songspot == SONG_LENGTH3) {
songstate = 0;
}
if (songspot < SONG_LENGTH3) {
songspot++;
} // incrementing through the song array until the final value of the array is reached
}
else if(songstate == 4) {
EPwm9Regs.TBPRD = songarray4[songspot]; // Song 4 code
if (songspot == SONG_LENGTH4) {
GPIO_SetupPinMux(16, GPIO_MUX_CPU1, 0); // Mux changed so buzzer is back to GPIO16
GPIO_SetupPinOptions(16, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO16 = 1;
songstate = 0;
} // when the song array reaches the final value, it muxes the GPIO16 pin back to GPIO16 instead of EPWM9
if (songspot < SONG_LENGTH4) {
songspot++;
} // incrementing through the song array until the final value of the array is reached
}
// if ((CpuTimer2.InterruptCount % 50) == 0) {
// UARTPrint = 1;
// }
}
int16_t starter = 0;
int16_t accelxraw = 0;
int16_t accelyraw = 0;
int16_t accelzraw = 0;
int16_t temperature = 0;
int16_t gyroxraw = 0;
int16_t gyroyraw = 0;
int16_t gyrozraw = 0;
int16_t stat1 = 0;
int16_t magxraw = 0;
int16_t magyraw = 0;
int16_t magzraw = 0;
float compass_array[20] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
__interrupt void SPIB_isr(void){
GpioDataRegs.GPCSET.bit.GPIO66 = 1; // Set GPIO 66 high to end Slave Select
// Assigning SPI reads to integer variables
starter = SpibRegs.SPIRXBUF;
accelxraw = SpibRegs.SPIRXBUF;
accelyraw = SpibRegs.SPIRXBUF;
accelzraw = SpibRegs.SPIRXBUF;
temperature = SpibRegs.SPIRXBUF;
gyroxraw = SpibRegs.SPIRXBUF;
gyroyraw = SpibRegs.SPIRXBUF;
gyrozraw = SpibRegs.SPIRXBUF;
stat1 = SpibRegs.SPIRXBUF;
magxraw = SpibRegs.SPIRXBUF;
magyraw = SpibRegs.SPIRXBUF;
magzraw = SpibRegs.SPIRXBUF;
// Convert the integer values to floats
accelX = 4.0/32767.0 * (float)(accelxraw);
accelY = 4.0/32767.0 * (float)(accelyraw);
accelZ = 4.0/32767.0 * (float)(accelzraw);
gyroX = 250.0/32767.0 * (float)(gyroxraw);
gyroY = 250.0/32767.0 * (float)(gyroyraw);
gyroZ = 250.0/32767.0 * (float)(gyrozraw);
magX = 1.0/1.0 * (float)(magxraw); // Max: 30 Min: -70
magY = 1.0/1.0 * (float)(magyraw); // Max: 105 Min: 3
magZ = 1.0/1.0 * (float)(magzraw); // Max: 70 Min: -37
magX = magX - magX_offset;
magY = magY - magY_offset;
magZ = magZ - magZ_offset;
//Use arctan to get compass angle
if(magY < 0) {
compass_angle = 270.0-(((float)atan((magX/magY)))*180.0/PI);
}
else if(magY > 0) {
compass_angle = 90.0-(((float)atan((magX/magY)))*180.0/PI);
}
else if((magY == 0) && (magX < 0)) {
compass_angle = 180.0;
}
if((magY == 0) && (magX > 0)) {
compass_angle = 0.0;
}
// 20-point average for the compass angle
compass_array[0] = compass_angle;
compass_avg = 0.05 * (compass_array[0]
+ compass_array[1]
+ compass_array[2]
+ compass_array[3]
+ compass_array[4]
+ compass_array[5]
+ compass_array[6]
+ compass_array[7]
+ compass_array[8]
+ compass_array[9]
+ compass_array[10]
+ compass_array[11]
+ compass_array[12]
+ compass_array[13]
+ compass_array[14]
+ compass_array[15]
+ compass_array[16]
+ compass_array[17]
+ compass_array[18]
+ compass_array[19]
);
// Start state machine for going to corners
if(state == 10) {
eC = 265.0 - compass_avg;
if(compass_avg < 260.0) {
songspot = 0;
songstate = 1;
Vref = 0.0;
turn = Kc*eC; //0.4; // Non-proportional control, open loop control
}
else if((compass_avg >= 260.0)&&(compass_avg <= 270.0)) {
turn = 0.0;
state = 20;
}
else {
Vref = 0.0;
turn = Kc*eC; //-0.4;
}
}
else if(state == 20) {
Vref = 0.5;
turn = 0.0;
if((XCoord <= 1.0)||(IR1Volt >= 1.25)) {
Vref = 0.0;
state = 30;
}
// if((265.0 - compass_avg) > 2.5) { // Attempting to apply P-control to the forward motion phase of the path, keeping the car oriented on the desired angle
// turn = Kc*(265.0 - compass_avg);
// }
// else if((265.0 - compass_avg) < -2.5) {
// turn = Kc*(265.0 - compass_avg);
// }
}
else if(state == 30) {
Vref = 0.0;
eC = 330.0 - compass_avg;
if(compass_avg < 325.0) {
turn = Kc*eC; //0.4;
}
else if((compass_avg >= 325.0)&&(compass_avg <= 335.0)) {
turn = 0.0;
state = 40;
}
else {
turn = Kc*eC; //-0.4;
}
}
else if(state == 40) {
Vref = 0.5;
turn = 0.0;
if(IR2Volt >= 2.5) {
turn = 0.35;
}
else if((YCoord <= 1.0)||(IR1Volt >= 1.25)) {
Vref = 0.0;
state = 50;
songspot = 0;
songstate = 2;
}
else if((YCoord > 1.0)&&(IR1Volt >= 1.25)) {
turn = 0.35;
}
// if((330.0 - compass_avg) < -2.5) {
// turn = Kc*(330.0 - compass_avg);
// }
}
else if(state == 50) {
eC = 60.0 - compass_avg;
if(compass_avg > 62.5) {
turn = Kc*eC; //-0.4;
Vref = 0.0;
}
else if((compass_avg >= 57.5)&&(compass_avg <= 62.5)) {
turn = 0.0;
state = 60;
}
else {
turn = Kc*eC; //0.4;
Vref = 0.0;
}
}
else if(state == 60) {
Vref = 0.5;
turn = 0.0;
if(IR2Volt >= 2.5) {
turn = 0.3;
}
else if((XCoord >= 11.0)||(IR1Volt >= 1.25)) {
Vref = 0.0;
state = 70;
songspot = 0;
songstate = 3;
}
else if((XCoord < 11.0)&&(IR1Volt >= 1.25)) {
turn = 0.3;
}
// if((60.0 - compass_avg) < -2.5) {
// turn = Kc*(60.0 - compass_avg);
// }
}
else if(state == 70) {
Vref = 0.0;
eC = 255.0 - compass_avg;
if(compass_avg < 250.0) {
turn = Kc*eC; //0.4;
Vref = 0.0;
}
else if((compass_avg >=250.0)&&(compass_avg <= 260.0)) {
turn = 0.0;
state = 80;
}
else {
turn = Kc*eC; //-0.4;
Vref = 0.0;
}
}
else if(state == 80) {
Vref = 0.5;
if((XCoord > 6.0)&&(YCoord < 6.0)) {
turn = 0.0;
Vref = 0.5;
}
else if((XCoord == 6.0)&&(YCoord > 6.0)) {
if(compass_avg > 185.0) {
eC = 180.0 - compass_avg;
turn = Kc*eC; //-0.4;
}
else if((compass_avg <= 185.0)&&(compass_avg >= 175.0)) {
turn = 0.0;
Vref = -0.5;
}
else {
eC = 180.0 - compass_avg;
turn = Kc*eC; //0.4;
}
}
else if((XCoord == 6.0)&&(YCoord < 6.0)) {
if(compass_avg > 182.5) {
eC = 180.0 - compass_avg;
turn = Kc*eC; //-0.4;
}
else if((compass_avg <= 182.5)&&(compass_avg >= 177.5)) {
turn = 0.0;
Vref = 0.5;
}
else {
eC = 180.0 - compass_avg;
turn = Kc*eC; //0.4;
}
}
else if((XCoord == 6.0)&&(YCoord == 6.0)) {
turn = 0.0;
Vref = 0.0;
state = 90;
}
else if(IR1Volt >= 1.75) {
Vref = 0.0;
turn = 0.0;
}
// if((250.0 - compass_avg) > 2.5) {
// turn = Kc*(250.0-compass_avg);
// }
// else if((250.0 - compass_avg) < -2.5) {
// turn = Kc*(250.0-compass_avg);
// }
}
else if(state == 90) {
songspot = 0;
songstate = 4;
state = 0;
}
// Storing previous compass values to get new average
int i;
for(i=19; i>=1; --i) {
compass_array[i] = compass_array[i-1];
}
SpibRegs.SPIFFRX.bit.RXFFOVFCLR = 1; // Clear Overflow flag just in case of an overflow
SpibRegs.SPIFFRX.bit.RXFFINTCLR = 1; // Clear RX FIFO Interrupt flag so next interrupt will happen
PieCtrlRegs.PIEACK.all = PIEACK_GROUP6; // Acknowledge INT6 PIE interrupt
}
// This function is called each time a char is recieved over UARTA.
void serialRXA(serial_t *s, char data) {
numRXA ++;
if (data == 'q') {
turn = turn + 0.05;
} else if (data == 'r') {
turn = turn - 0.05;
} else if (data == '3') {
Vref = Vref + 0.1;
} else if (data == 's') {
turn = 0.0;
Vref = 0.0;
} else if (data == '5') {
Vref = Vref - 0.1;
} else {
turn = 0;
Vref = 0.5;
}
}
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.
// Make sure the TXdelay in between each transfer to 0. Also don’t forget to
// cut and paste the GPIO settings for GPIO9, 63, 64, 65, 66 which are also a part of the SPIB setup.
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 I/O pin to asynchronous mode recommended for SPI
GpioCtrlRegs.GPCQSEL1.bit.GPIO64 = 3; // Set I/O pin to asynchronous mode recommended for SPI
GpioCtrlRegs.GPCQSEL1.bit.GPIO65 = 3; // Set I/O pin to asynchronous mode recommended for SPI
EDIS;
// SpibRegs.SPICCR.bit.SPICHAR = 0xF;
// SpibRegs.SPIFFCT.bit.TXDLY = 0x00;
//-----------------------------------------------------------------------------------------------------------------
SpibRegs.SPICCR.bit.SPISWRESET = 0; // Put SPI in Reset
SpibRegs.SPICTL.bit.CLK_PHASE = 1; //This happens to be the mode for both the DAN28027 and
...
This file has been truncated, please download it to see its full contents.
Comments