UIUC SE423 LiDAR Robot Final Project

//#############################################################################
// 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
#define FEETINONEMETER 3.28083989501312

// Only one of these can be uncommented at a time
#define MATLAB
//#define SIMULINK

#ifdef MATLAB
void matlab_serialRX(serial_t *s, char data);
#endif
#ifdef SIMULINK
void simulink_serialRX(serial_t *s, char data);
#endif


#pragma DATA_SECTION(matlabLock, ".my_vars")
float matlabLock = 0;
float matlabLockshadow = 0;

#pragma DATA_SECTION(matlab_dist, ".my_arrs")
float matlab_dist[360];

#pragma DATA_SECTION(matlab_angle, ".my_arrs")
float matlab_angle[360];

#pragma DATA_SECTION(matlab_x, ".my_arrs")
float matlab_x[360];

#pragma DATA_SECTION(matlab_y, ".my_arrs")
float matlab_y[360];

// 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);

void serialRXA(serial_t *s, char data);
void setupSpib(void); // pre-define for ex4
void serialRXD(serial_t *s, char data);

// pre define for lab 6
void init_eQEPs(void);
float readEncLeft(void);
float readEncRight(void);
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;
uint16_t sampleSize = 0;
uint16_t RC_value1 = 1500;
uint16_t RC_value2 = 1500;
uint16_t count_mode = 1;
uint16_t print_count = 0;
float volt1 = 0.0;
float volt2 = 0.0;

// EX4
int16_t dummy = 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;
float accelXreading = 0.0;
float accelYreading = 0.0;
float accelZreading = 0.0;
float gyroXreading = 0.0;
float gyroYreading = 0.0;
float gyroZreading = 0.0;

// Lab6
float LeftWheel = 0.0;
float RightWheel = 0.0;
float dist_left = 0.0;
float dist_right = 0.0;
float dist_factor = 9.7;
float uLeft = 0.0;
float uRight = 0.0;
float dist_left_1 = 0.0;
float dist_right_1 = 0.0;
float VLeftK = 0.0;
float VRightK = 0.0;

float Vref = 0.5;
float ek_left = 0.0;
float ek_left_1 = 0.0;
float ek_right = 0.0;
float ek_right_1 = 0.0;

float Ik_left = 0.0;
float Ik_left_1 = 0.0;
float Ik_right = 0.0;
float Ik_right_1 = 0.0;
float Kp = 3.0;
float Ki = 25.0;

// lab 6 ex 4
float eturn = 0.0;
float turn = 0.0;
float Kturn = 3.0;

// lab 6 ex 5
float Wr = 0.61;  // robot width
float Rw = 0.10625; // Radius wheel
float phi_R = 0.0; //robot pose angle
float xR = 0.0; // robot pose X coordinate
float yR = 0.0; // robot pose Y coordinate
float xR_dot = 0.0; // pose X dot
float yR_dot = 0.0; // pose Y dot
float theta_avg = 0.0;
float theta_avg_dot = 0.0;
float LeftWheel_1 = 0.0;
float RightWheel_1 = 0.0;
float xR_dot_1 = 0.0;
float yR_dot_1 = 0.0;
float theta_avg_1 = 0.0;

//spi values
int16_t spivalue1 = 0;
int16_t spivalue2 = 0;
int16_t spivalue3 = 0;

// variables for finding compass angle
int16_t mXraw = 0;
int16_t mYraw = 0;
int16_t mZraw = 0;
float mx = 0.0;
float my = 0.0;
float mz = 0.0;
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 accelxSide = 1.502;      //accel x offset for when starting on the side
float accelxSideStart = 1.502;
float accelzSide = -0.76;
float gravitySide = 0.70;       //gravity offset for accely when starting on the side.

float mx_offset = (96 + -18)/2.0; //(max_mx + min_mx)/2.0
float my_offset = (108 + 15)/2.0; // (max_my + min_my)/2.0
float mz_offset = (273 + 125)/2.0; // (max_mz + min_mz)/2.0
uint16_t count=0;
uint16_t calc_offset=0;
float gyroZint = 0.0;
float gyroZint_1 = 0.0;
float gyroZintdisplay = 0.0;
int16_t revcountgyro = 0;
float gyroZreading_1 = 0.0;
float phi = 0.0;
float R = 1.275; //wheel radius in inches
float W = 7.35;  //base width in inches
int16_t doneCal = 0;
float compass_angle = 0.0;
int16_t revcountphi = 0;
float x_A_offs_add = .100;
float y_A_offs_add = -.07;
float z_A_offs_add = .070;

float x_G_offs = 1;
float y_G_offs = 6;
float z_G_offs = 1;

// printing variables
uint16_t car_status = 0;

// car controlling variables
uint16_t turn_state = 0;
uint16_t run_count = 0;
uint16_t ob_position = 0;


//the structure to store data points.
// distance is the measured distance;
//timestamp is the time that the point is captured
//rawAngle is directly calculated with the start and end angle
//cor_angle is calculated based on the correction
typedef struct data_point{
    double distance;
    uint32_t timestamp;
    double rawAngle;
    double cor_angle;
}data_pt;

//pts is the array to store collected data points
data_pt pts[600];

//the structure to store the distance and timestamp(use angle as the index)
typedef struct dis_time{
    double distance;
    uint32_t timestamp;
}dis_time;
dis_time pingpts[360];
dis_time pongpts[360];

double x[360]; //original x
double y[360]; //original y
dis_time x_f[360]; //final x
dis_time y_f[360]; //final y

//initial condition
double pose_x; //x_position of the robot car
double pose_y; //x_position of the robot car
double pose_theta = 45; //angle of robot car
double pose_rad; //angle in rad

//for debug
uint32_t stamp315 = 0;
float dist315 = 0;
uint32_t stamp0 = 0;
float dist0 = 0;
uint32_t stamp180 = 0;
float dist180 = 0;
uint32_t stamp225 = 0;
float dist225 = 0;
uint32_t stamp270 = 0;
float dist270 = 0;
uint32_t stamp90 = 0;
float dist90 = 0;
uint32_t stamp345 = 0;
float dist345 = 0;

//to check if it is a full revolution
float anglesum = 0;
float dist;
int16_t checkfullrevolution = 0;
int16_t pingpongflag = 1;
int16_t UseLIDARping = 0;
int16_t UseLIDARpong = 1;
int16_t write_matlab_data = 1;
int16_t matlab_count = 0;
float Vref_1 = 0.1;
uint16_t obstacle = 0;
uint16_t ob_count = 0;
uint16_t run_yes = 1;
uint16_t follow = 0;
uint16_t turn_yes = 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;
    // for SPIB_ISR
    PieVectTable.SPIB_RX_INT = &SPIB_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, 1000);
    ConfigCpuTimer(&CpuTimer1, 200, 4000);
    ConfigCpuTimer(&CpuTimer2, 200, 40000);

    // Enable CpuTimer Interrupt bit TIE
    CpuTimer0Regs.TCR.all = 0x4000;
    CpuTimer1Regs.TCR.all = 0x4000;
    CpuTimer2Regs.TCR.all = 0x4000;

#ifdef MATLAB
    init_serial(&SerialA,115200,matlab_serialRX);
#else
#ifdef SIMULINK

    init_serial(&SerialA,115200,simulink_serialRX);
#else
    init_serial(&SerialA,115200,serialRXA);
#endif
#endif

    //init_serial(&SerialA,115200,serialRXA);
//    init_serial(&SerialC,115200,serialRXC);
//    init_serial(&SerialD,115200,serialRXD);
    init_serial(&SerialD,115200,serialRXD);


    // EPWM2A & EPWM2B (segbot motors)
    EPwm2Regs.TBCTL.bit.FREE_SOFT = 3;
    EPwm2Regs.TBCTL.bit.CTRMODE = 0;
    EPwm2Regs.TBCTL.bit.PHSEN = 0;
    EPwm2Regs.TBCTL.bit.CLKDIV = 0;
    EPwm2Regs.TBPRD = 2500;
    EPwm2Regs.TBCTR = 0;
    EPwm2Regs.CMPA.bit.CMPA = 0;
    EPwm2Regs.CMPB.bit.CMPB = 0;
    EPwm2Regs.AQCTLA.bit.CAU = 1;
    EPwm2Regs.AQCTLB.bit.CBU = 1;
    EPwm2Regs.AQCTLA.bit.ZRO = 3;
    EPwm2Regs.AQCTLB.bit.ZRO = 3;
    EPwm2Regs.TBPHS.bit.TBPHS = 0;


    init_eQEPs(); // setup eQEP channel for 3 wheel car
    setupSpib(); // setup MPU-9250

    // Pinmux select for EPwms
    GPIO_SetupPinMux(22, GPIO_MUX_CPU1, 5);
    // for motors
    GPIO_SetupPinMux(2, GPIO_MUX_CPU1, 1);
    GPIO_SetupPinMux(3, GPIO_MUX_CPU1, 1);

    EALLOW; // Below are protected registers
    GpioCtrlRegs.GPAPUD.bit.GPIO2 = 1; // For EPWM2A
    GpioCtrlRegs.GPAPUD.bit.GPIO3 = 1; // For EPWM2B
    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;  // SPIB

    // 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_RX in the PIE: Group 6 interrupt 3
    PieCtrlRegs.PIEIER6.bit.INTx3 = 1;

    // Enable global Interrupts and higher priority real-time debug events
    EINT;  // Enable Global interrupt INTM
    ERTM;  // Enable Global realtime interrupt DBGM

    //setup matlab_angle
    int16_t i = 0;
    for (i = 0; i < 360; i++) {
        matlab_angle[i] = i;
    }

    
    // IDLE loop. Just sit and loop forever (optional):
    while(1)
    {
        if (UARTPrint == 1 ) {
//                //serial_printf(&SerialA,"accelX: %.3f g, accelY: %.3f g, and accelZ: %.3f g\r\n",accelXreading, accelYreading, accelZreading);
//                //serial_printf(&SerialA,"gyroX: %.3f dps, gyroY: %.3f dps, and gyroZ: %.3f dps\r\n", gyroXreading, gyroYreading, gyroZreading);
//                //serial_printf(&SerialA, "Left angle: %.3f, Right angle: %.3f\r\n", LeftWheel, RightWheel);
////                serial_printf(&SerialA, "Left dist: %.3f, Right dist: %.3f\r\n", dist_left, dist_right);
//            //serial_printf(&SerialA, "VLeftK: %.3f, VRightK: %.3f.\r\n", VLeftK, VRightK);
            //serial_printf(&SerialA,"gyroZint = %.1f phi = %.1f Compass Angle = %.1f Revolutions = %i \r\n", gyroZintdisplay, phi, compass_angle, revcountphi);            UARTPrint = 0;
            //serial_printf(&SerialA, "Position: (%.3f, %.3f, %.3f), Compass angle: %.1f\r\n", xR, yR, phi_R,compass_angle);
            //serial_printf(&SerialA,"TimeStamp %ld Dist %.3f\r\n",stamp45,dist45);

            if (car_status == 1){
                if(ob_position == 1){
                    serial_printf(&SerialA, "Obstacle @ left(%.3f, %.3f, %.3f), Compass angle: %.1f\r\n", xR, yR, phi_R,compass_angle);
                }else if (ob_position == 2){
                    serial_printf(&SerialA, "Obstacle @ right(%.3f, %.3f, %.3f), Compass angle: %.1f\r\n", xR, yR, phi_R,compass_angle);
                }else if (ob_position == 3){
                    serial_printf(&SerialA, "Obstacle @ right front(%.3f, %.3f, %.3f), Compass angle: %.1f\r\n", xR, yR, phi_R,compass_angle);
                }else if(ob_position == 4){
                    serial_printf(&SerialA, "Obstacle @ front(%.3f, %.3f, %.3f), Compass angle: %.1f\r\n", xR, yR, phi_R,compass_angle);
                }else if(ob_position == 5){
                    serial_printf(&SerialA, "Obstacle @ left front(%.3f, %.3f, %.3f), Compass angle: %.1f\r\n", xR, yR, phi_R,compass_angle);
                }
                //serial_printf(&SerialA, "Obstacle @ (%.3f, %.3f, %.3f), Compass angle: %.1f\r\n", xR, yR, phi_R,compass_angle);
            }else if(car_status == 0){
                serial_printf(&SerialA, "Area Clear\r\n!");

            }

            UARTPrint = 0;
        }

        if (UseLIDARping == 1) {
            UseLIDARping = 0;
            dist0 = pingpts[1].distance;
            stamp0 = pingpts[1].timestamp;
            dist180 = pingpts[180].distance;
            stamp180 = pingpts[180].timestamp;
            dist315 = pingpts[315].distance;
            stamp315 = pingpts[315].timestamp;
            dist225 = pingpts[225].distance;
            stamp225 = pingpts[225].timestamp;
            dist270 = pingpts[270].distance;
            stamp270 = pingpts[270].timestamp;
            dist90 = pingpts[90].distance;
            stamp90 = pingpts[90].timestamp;
            dist345 = pingpts[345].distance;
            stamp345 = pingpts[345].timestamp;
            //UARTPrint = 1;
            uint16_t i = 0;

            for (i = 0; i < 360; i++) {
                //if the data is not available, set all of them to 0 to avoid messing things up
                if (pingpts[i].distance ==0) {
                    x_f[i].distance = 0;
                    y_f[i].distance = 0;
                    x_f[i].timestamp = pingpts[i].timestamp;
                    y_f[i].timestamp = pingpts[i].timestamp;
                } else {
                    x[i] = pingpts[i].distance*cos(i*0.01745329);//0.017453292519943 is pi/180
                    y[i] = pingpts[i].distance*sin(i*0.01745329);
                    x_f[i].distance = (x[i]+pose_x)*cos(pose_rad)+(y[i]+pose_y)*sin(pose_rad); //change basis
                    y_f[i].distance = -(x[i]+pose_x)*sin(pose_rad)+(y[i]+pose_y)*cos(pose_rad); //change basis
                    x_f[i].timestamp = pingpts[i].timestamp;
                    y_f[i].timestamp = pingpts[i].timestamp;
                }
            }

        }
        if (UseLIDARpong == 1) {
            UseLIDARpong = 0;
            dist0 = pongpts[1].distance;
            stamp0 = pongpts[1].timestamp;
            dist180 = pongpts[180].distance;
            stamp180 = pongpts[180].timestamp;
            dist315 = pongpts[315].distance;
            stamp315 = pongpts[315].timestamp;
            dist225 = pongpts[225].distance;
            stamp225 = pongpts[225].timestamp;
            dist270 = pongpts[270].distance;
            stamp270 = pongpts[270].timestamp;
            dist90 = pongpts[90].distance;
            stamp90 = pongpts[90].timestamp;
            dist345 = pongpts[345].distance;
            stamp345 = pongpts[345].timestamp;
            //UARTPrint = 1;
            uint16_t i = 0;
            pose_rad = pose_theta*0.017453292519943; //convert to rads
            for (i = 0; i < 360; i++) {
                if (pongpts[i].distance == 0) {
                    x_f[i].distance = 0;
                    y_f[i].distance = 0;
                    x_f[i].timestamp = pongpts[i].timestamp;
                    y_f[i].timestamp = pongpts[i].timestamp;

                } else {
                    x[i] = pongpts[i].distance*cos(i*0.01745329);//0.017453292519943 is pi/180
                    y[i] = pongpts[i].distance*sin(i*0.01745329);
                    x_f[i].distance = (x[i]+pose_x)*cos(pose_rad)+(y[i]+pose_y)*sin(pose_rad);
                    y_f[i].distance = -(x[i]+pose_x)*sin(pose_rad)+(y[i]+pose_y)*cos(pose_rad);
                    x_f[i].timestamp = pongpts[i].timestamp;
                    y_f[i].timestamp = pongpts[i].timestamp;
                    matlab_x[i] = x_f[i].distance;
                    matlab_y[i] = y_f[i].distance;
                }
            }
        }

//        if (UARTPrint == 1 ) {
//#ifndef MATLAB
//#ifndef SIMULINK
//            serial_printf(&SerialA,"TimeStamp %ld Dist %.3f\r\n",stamp45,dist45);
//            //serial_printf(&SerialA,"gyroZint = %.1f phi = %.1f Compass Angle = %.1f Revolutions = %i \r\n", gyroZintdisplay, phi, compass_angle, revcountphi);
//#endif
//#endif
//
//            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;

}

// 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 (count_mode == 1){ // counting up  For Ex3
//        RC_value1 = RC_value1 + 10;
//        RC_value2 = RC_value2 + 10;
//
//        if (RC_value1 >= 4500){
//            count_mode = 0;
//        }
//    }else if (count_mode == 0){ // counting down
//        RC_value1 = RC_value1 - 10;
//        RC_value2 = RC_value2 - 10;
//
//        if (RC_value1 <= 1500){
//            count_mode = 1;
//        }
//    }

    //Clear GPIO9 Low to act as a Slave Select. Right now, just to scope. Later to select DAN28027 chip
    //GpioDataRegs.GPACLEAR.bit.GPIO9 = 1;
    // The CS for the MPU9250 is GPIO66
    GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;

    if(count<3000) {
        count++;
    } else {
    if(calc_offset==0) {
         calc_offset = 1;
         count = 0;
    } else if(calc_offset == 1)
          calc_offset = 2;
    }

    SpibRegs.SPIFFRX.bit.RXFFIL = 12; // Issue the SPIB_RX_INT when two values are in the RX FIFO

    SpibRegs.SPITXBUF = ((0x8000)|(0x3A00)); //start reading from address 0x3A
    SpibRegs.SPITXBUF = 0x0000; // send to read from 0x3B, 0x3C
    SpibRegs.SPITXBUF = 0x0000; // send to read from 0x3D, 0x3E
    SpibRegs.SPITXBUF = 0x0000; // send to read from 0x3F, 0x40
    SpibRegs.SPITXBUF = 0x0000; // send to read from 0x41, 0x42
    SpibRegs.SPITXBUF = 0x0000; // send to read from 0x43, 0x44
    SpibRegs.SPITXBUF = 0x0000; // send to read from 0x45, 0x46
    SpibRegs.SPITXBUF = 0x0000; // send to read from 0x47, 0x48
    SpibRegs.SPITXBUF = 0x0000; // dummy
    SpibRegs.SPITXBUF = 0x0000; // CompassX MSB and LSB
    SpibRegs.SPITXBUF = 0x0000; // CompassY MSB and LSB
    SpibRegs.SPITXBUF = 0x0000; // CompassZ MSB and LSB



    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
__interrupt void cpu_timer1_isr(void)
{
    LeftWheel = readEncLeft();
    RightWheel = readEncRight();

    phi_R = (Rw/Wr) * (RightWheel - LeftWheel); // calculate the phi_R (orientation angle)
    theta_avg = 0.5*(RightWheel + LeftWheel); // finding theta_avg
    theta_avg_dot = (theta_avg - theta_avg_1)/0.004;
    xR_dot = Rw * theta_avg_dot * cos(phi_R);
    yR_dot = Rw * theta_avg_dot * sin(phi_R);
    xR = xR + 0.004 * (xR_dot + xR_dot_1)/2; // figuring out the position x
    yR = yR + 0.004 * (yR_dot + yR_dot_1)/2; // figuring out the position y

    dist_left = LeftWheel / dist_factor;
    dist_right = RightWheel / dist_factor;

    VLeftK = (dist_left - dist_left_1)/0.004; // linear speed for Left wheel
    VRightK = (dist_right - dist_right_1)/0.004; // linear speed for right wheel

    eturn = turn + (VLeftK - VRightK);  // error in turn by the turn command
    ek_left = Vref - VLeftK - Kturn * eturn; // turn logic 1
    ek_right = Vref - VRightK + Kturn * eturn; // turn logic 2

    if (uLeft >= 10.0){  // anti-windup controller left
        Ik_left = Ik_left_1;
    }else if (uLeft <= -10.0){
        Ik_left = Ik_left_1;
    }else{
        Ik_left = Ik_left_1 + 0.004*(ek_left + ek_left_1)/2;
    }

    if (uRight >= 10.0){  // anti-windup controller right
        Ik_right = Ik_right_1;
    }else if (uRight <= -10.0){
        Ik_right = Ik_right_1;
    }else{
        Ik_right = Ik_right_1 + 0.004*(ek_right + ek_right_1)/2;
    }

    uLeft = Kp * ek_left + Ki * Ik_left;  // find out the control effort
    uRight = Kp * ek_right + Ki * Ik_right;

    setEPWM2B(-uLeft); // set the control effort to the motor
    setEPWM2A(uRight);


    dist_left_1 = dist_left;  // saving state to be used as old-state for the next cycle
    dist_right_1 = dist_right;
    ek_left_1 = ek_left;
    ek_right_1 = ek_right;
    Ik_left_1 = Ik_left;
    Ik_right_1 = Ik_right;
    LeftWheel_1 = LeftWheel;
    RightWheel_1 = RightWheel;
    xR_dot_1 = xR_dot;
    yR_dot_1 = yR_dot;
    theta_avg_1 = theta_avg;

    CpuTimer1.InterruptCount++;
}

// cpu_timer2_isr CPU Timer2 ISR
__interrupt void cpu_timer2_isr(void)
{
    // car running logic and determine car status
    if (dist0 <= 0.4){
        ob_position = 1;
        Vref = 0.0;
        turn = 0.0;
        car_status = 1;
        obstacle = 1;
    }else if(dist180 <=0.4){
        ob_position = 2;
        Vref = 0.0;
        turn = 0.0;
        car_status = 1;
        obstacle = 1;
    }else if(dist225 <= 0.2){
        ob_position = 3;
        Vref = 0.0;
        turn = 0.0;
        car_status = 1;
        obstacle = 1;
    }else if(dist270 <= 0.1){
        ob_position = 4;
        Vref = 0.0;
        turn = 0.0;
        car_status = 1;
        obstacle = 1;
    }else if(dist315 <= 0.2){
        ob_position = 5;
        Vref = 0.0;
        turn = 0.0;
        car_status = 1;
        obstacle = 1;
    }else{
        car_status = 0;
        ob_position = 0;
        obstacle = 0;
        if(run_yes ==1){
        // car running logic here
            if (run_count <= 100){
                Vref = 0.1;
                turn = 0.0;
                obstacle = 2;
            }else if(run_count <= 300){
                Vref = 0.15;
                turn= -0.15;
                obstacle = 2;
            }else if (run_count <= 600){
                Vref = 0.175;
                turn = -0.15;
                obstacle = 2;
            }else if (run_count <= 900){
                Vref = 0.20;
                turn = -0.15;
                obstacle = 2;
            }else if (run_count <= 1200){
                Vref = 0.25;
                turn = -0.15;
                obstacle = 2;
            }else if (run_count <= 1500){
                Vref = 0.3;
                turn = -0.15;
                obstacle = 2;
            }else if (run_count <= 1800){
                Vref = 0.35;
                turn = -0.15;
                obstacle = 2;
            }else if (run_count <= 2100){
                Vref = 0.4;
                turn = -0.15;
                obstacle = 2;
            }else if (run_count <= 2400){
                Vref = 0.5;
                turn = -0.15;
                obstacle = 2;
            }else if (run_count <= 2700){
                Vref = 0.6;
                turn = -0.15;
                obstacle = 2;
            }
        }

    }

    if (obstacle == 1){
        if(ob_count <=100){
            if((dist270 <= 0.5)){
                ob_count ++;
                }
        }
        else{
            run_yes = 0;
            follow = 1;
     }
    }
     else if(obstacle ==2){
        run_count ++;
        obstacle = 0;
        }
    if (follow == 1){
        if ((dist345>=0.38)){
            if((dist345<= 0.42)){
                Vref = 0.35;
                turn = 0.0;
            }else if(dist315>=0.5){
                Vref=0.125;
                turn = 0.54;

            }else if(dist345<0.5){
                Vref = 0.35;
                turn = 0.3;  //  turn right
            }
    }else if(dist345 < 0.38){
        Vref = 0.35;
        turn = -0.40;

    }

    }




    // Blink LaunchPad Blue LED
    GpioDataRegs.GPATOGGLE.bit.GPIO31 = 1;

    CpuTimer2.InterruptCount++;

    if ((CpuTimer2.InterruptCount % 50) == 0) {
        //UARTPrint = 1;
    }
}


// This function is called each time a char is recieved over UARTA.
void serialRXA(serial_t *s, char data) {
    numRXA ++;
    if (data == 'q') { // wire control logic
        turn = turn + 0.05;
    } else if (data == 'r') {
        turn = turn - 0.05;
    } else if (data == '3') {
        Vref = Vref + 0.1;
    } else if (data == 'z') {
        Vref = 0;
        turn = 1.;
    } else if (data == 'v') {
        Vref = 0;
        turn = -1.;
    }else if (data == 'w') {
        Vref = 0.5;
        turn = 0;
    }
    else {
        turn = 0;
        Vref = 0;
    }
...

This file has been truncated, please download it to see its full contents.