Scott Manhart
Published © GPL3+

Autonomous Path Planning with LIDAR and Motion Capture

Obstacle Detection with LIDAR to create and update a map so that an A* algorithm can solve a path for a robot car to follow.

IntermediateWork in progressOver 1 day3,454
Autonomous Path Planning with LIDAR and Motion Capture

Things used in this project

Hardware components

YDLIDAR-X2
Most Lazer Radars will work
×1
Orange Pi Zero
Can be swapped for a Razzberry PI. For Network communication.
×1
DC motor (generic)
For running the robot car.
×2
LAUNCHXL-F28379D C2000 Delfino LaunchPad
Texas Instruments LAUNCHXL-F28379D C2000 Delfino LaunchPad
×1
Micro SD
To save code to Orange PI
×1
SE 423 PCB
PCB Created for SE423 at UIUC. Used for more than just this project. See the schematic below.
×1

Software apps and online services

Code Composer Studio
Texas Instruments Code Composer Studio
YDLidar Tool

Story

Read more

Custom parts and enclosures

Lidar Mount

Motion Capture Mount

Lidar Mount

Motion Capture Mount

Schematics

SE 423 PCB Schematic

Code

Astar and Obstacle Detection

C/C++
SE423 Final Project
//#############################################################################
// FILE:   WallFollow_And_Astar.c
//
// TITLE: Final Project
//#############################################################################

// 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 "dsp.h"
#include "fpu32/fpu_rfft.h"
#include "pQueue.h"
#include "stdafx.h"

#define PI          3.1415926535897932384626433832795
#define TWOPI       6.283185307179586476925286766559
#define HALFPI      1.5707963267948966192313216916398
#define NUMPOINTS   6
#define min(a,b) \
    ({a < b ? a : b;})

// 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[1000];
//
//#pragma DATA_SECTION(matlab_angle, ".my_arrs")
//float matlab_angle[1000];

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

void serialRXA(serial_t *s, char data);
void serialRXD(serial_t *s, char data);
void serialRXC(serial_t *s, char data);
void ObsIntit();
void readState();
void measureDist();
void init_eQEPs(void);
float readEncLeft(void);
float readEncRight(void);
void setEPWM2A(float controleffort);
void setEPWM2B(float controleffort);
//Predef A*
int heuristic(int rowCurr, int colCurr, int rowGoal, int colGoal);
int canTravel(int row, int col);
int getNeighbors(int rowCurr, int colCurr);
void reconstructPath(int rowEnd, int colEnd, dictElem_t nodeTrack[]);
int astar(int rowStart, int colStart, int rowEnd, int colEnd);
//Predef xy
void ReadSwitches(void);
int xy_control(float *vref_forxy, float *turn_forxy,float turn_thres, float x_pos,float y_pos,float x_desired,float y_desired,
                float thetaabs,float target_radius,float target_radius_near);
float my_atanf(float dy, float dx);
//Obstacle search
void searchObs();

// 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 astarC = 0;

int PathFunction = 3; // 0 for wall following 1 for A*


typedef struct data_point{
    double distance;
    uint32_t timestamp;
//    double startAngle;
//    double endAngle;
    double rawAngle;
    double cor_angle;
}data_pt;
data_pt pts[600];

typedef struct dis_time{
    double distance;
    uint32_t timestamp;
}dis_time;
dis_time pingpts[360];
dis_time pongpts[360];

typedef struct dis_timeC{
    double distanceX;
    double distanceY;
}dis_timeC;
dis_timeC pingptsC[360];

uint32_t stamp45 = 0;
float dist45 = 0;

float anglesum = 0;
int16_t checkfullrevolution = 0;
int16_t pingpongflag = 1;
int16_t UseLIDARping = 0;
int16_t UseLIDARpong = 1;

int state_ = 0;

float LeftWheel = 0;
float RightWheel = 0;
float radftL = 9.55; //rad/ft
float radftR = 9.55; //rad/ft
float uLeft = 0;
float uRight = 0;
float XLeftK = 0;
float XLeftK1 = 0;
float XRightK = 0;
float XRightK1 = 0;
float VLeftK = 0;
float VRightK = 0;
float IkLeft = 0;
float IkRight = 0;
float IkLeft1 = 0;
float IkRight1 = 0;
float ekLeft = 0;
float ekRight = 0;
float ekLeft1 = 0;
float ekRight1 = 0;
float Vref = 0;
float Kp = 3;
float Ki = 25;
float turn = 0;
float eturn = 0;
float Kturn = 3;

float thetaAvg = 0;
float thetaAvg1 = 0;
float thetaAvgp = 0;
float Xr = 0;
float Xr1 = 0;
float Xrp = 0;
float Xrp1 = 0;
float Yr = 0;
float Yr1 = 0;
float Yrp = 0;
float Yrp1 = 0;
float pose_d = 0;
float pose_d1 = 0;
float thetaR = 0;
float thetaL = 0;
float thetaR1 = 0;
float thetaL1 = 0;
float VthetaR = 0;
float VthetaL = 0;


int startRow = 4;
int startCol = 5;
int endRow = 15;
int endCol = 1;
char mapCourseStart[176] =      //16x11
{   '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    'x', 'x', 'x', 'x', '0', '0', '0', 'x', 'x', 'x', 'x',    //start row
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
    '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0'   };
//global variables necessary for the algorithm
node_t neighbors[4];        //used to get a list of neighboring nodes
int currDist = 0;   //distance traveled from starting point
int pathLen = 0;            //used to keep track of number of points in reconstructed path
int pathRow[400];           //reconstructed path in reverse order
int pathCol[400];
char map[176];
int mapRowSize = 16;
int mapColSize = 11;
dictElem_t nodeTrack[176];      //to track location of nodes in the heap, and to track parents of nodes technically only needs to be mapRowSize*mapColsize long
heap_t openSet, closedSet;
int solution = 0;
int retvalue = 0;

// structure for node path
typedef struct node_path{
    uint16_t row;
    uint16_t col;
}node_path;
node_path path[60];

//xy_control
float dx,dy,alpha;
float dist = 0.0;
float dir = 0;
float theta = 0;
int target_near = 0;
float turnerror = 0;
float x_pos = 0;
float y_pos = 0;
float x_desired = 0;
float y_desired = 0;
float thetaabs = 0;
float vref_forxy = 0;
float turn_forxy = 0;
float turn_thres = 0.5;
float target_radius_near = 0.4; //ft
float target_radius = 0.1; //ft
float ang = 0;
int nodenumber = 0;

int Obst = 0;
int maxR = 0;
//Bug
float turn_forBug = 0;
float Vref_forBug = 0;
float Kpb = 0.02;
int desiredwall = 300;
float wallerror = 0;
float left[30];
float middle[120];
float right[40];
float minR = 0;

//ORANGE PI
float Opti_x = 0;
float Opti_y = 0;
float Opti_theta = 0;
float Opti_Body = 0;
float Opti_Stamp = 0;
int16_t numRXC = 0;

// a pose (position and orientation) of the robot
typedef struct
{
    float x;        //in feet
    float y;        //in feet
    float theta;    // in radians between -PI and PI.  O radians is along the +x axis, PI/2 is the +y axis
} pose;


typedef struct
{
    float x;        //in feet
    float y;        //in feet
} xy;

xy points[NUMPOINTS] = {{4,4},{0,8},{-3,9},{-4,2},{-4,-1},{0,0}};
int16_t pointsindex = 0;
pose UpdateOptitrackStates(pose localROBOTps);

#define FEETINONEMETER 3.28083989501312
int32_t Opti_error = 0;
int16_t firstOptiData = 1;
int16_t OptiTrackableID = 0;
int32_t OptitrackableIDerror = 0;
float Optiprevious_frame = 0;
float temp_theta = 0;
float tempOPTITRACK_theta = 0;
float *cpu2tocpu1;
float x;
float y;
float theta;
float number;
float framecount;

pose OPTITRACKps;
pose ROBOTps = {0,0,0};

typedef union optiData_s {
    uint16_t rawdata[10];
    float data[5];
} optiData_t;

optiData_t OptiRecv;

char pastRecChar[5] = {0,0,0,0,0};

int16_t newData = 0;

//Obstacles
typedef struct{
    int r;
    int c;
    int x;
    int y;
    int tally;
    int found;
    int idx1;
    int idx2;
    int idx3;
}edges;
int obsSearch = 0;
edges obs[49];
float obsDist = 0;
float obsDisx = 0;
float obsDisy = 0;

//OptiTrack Change Basis
double x_ori[360]; //original x
double y_ori[360]; //original y
dis_time x_f[360]; //final x
dis_time y_f[360]; //final y


//initial condition
double pose_x = 4; //x_position of the robot car
double pose_y = 5; //x_position of the robot car
//*For some strange reason, need to put negative real angle*
double pose_theta = 0; //angle of robot car
double pose_rad; //angle in rad

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

    //EPWM2A Pin Setup
    GPIO_SetupPinMux(2, GPIO_MUX_CPU1, 1); //GPIO PinName, CPU, Mux Index
    //EPWM2B Pin Setup
    GPIO_SetupPinMux(3, GPIO_MUX_CPU1, 1); //GPIO PinName, CPU, Mux Index
    EALLOW; // Below are protected registers
    GpioCtrlRegs.GPAPUD.bit.GPIO2 = 1; // For EPWM2A
    GpioCtrlRegs.GPAPUD.bit.GPIO3 = 1; // For EPWM2B

    EDIS;

    // 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.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, 400000);

    // 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(&SerialC,1500000,serialRXC);
    init_eQEPs();

    //Write Rules for Obstacles
    ObsIntit();

    //EPWM2A/B
    EPwm2Regs.TBCTL.bit.CTRMODE = 0;
    EPwm2Regs.TBCTL.bit.FREE_SOFT = 2;
    EPwm2Regs.TBCTL.bit.PHSEN = 0;
    EPwm2Regs.TBCTL.bit.CLKDIV = 0;
    EPwm2Regs.TBCTR = 0;
    EPwm2Regs.TBPRD = 2500; //20Khz or period of 50microsec
    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;

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

    // 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 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(PathFunction == 3){
            Vref = 0;
            turn = 0;
        }

        if (PathFunction == 1){ // 1 for A* path
            // create the map
            mapRowSize = 16;
            mapColSize = 11;
            //reset Search flag
            obsSearch = 0;
            if(astarC == 0){
                startRow = 4;
                startCol = 5;
            }else{
                //floor the astar start point
                startRow = floorf(ROBOTps.y);
                startCol = floorf(ROBOTps.x);
            }
            astarC++;
            int i = 0;
            for(i = 0; i<mapRowSize*mapColSize; i++){        //copy m1 into map for solving
                map[i] = mapCourseStart[i];
            }
            astar(startRow,startCol,endRow,endCol); // solve path
            if(startRow>=0 && startRow<mapRowSize && startCol>=0 && startCol<mapColSize && endRow>=0 && endRow<mapRowSize && endCol>=0 && endCol<mapColSize)        //if in bounds{
            {
                if(map[startRow*mapColSize+startCol]!='x' && map[endRow*mapColSize+endCol]!='x')        //make sure valid start and end points
                {
                    retvalue = astar(startRow,startCol,endRow,endCol);
                    if (retvalue == 1) {
                        serial_printf(&SerialA,"   pathLength %d\r\n", pathLen);
                        int i = 0;
                        for(i = 0; i< pathLen; i++)     //put solution on map
                        {
                            int mapIdx = pathRow[i]*mapColSize + pathCol[i];
                            map[mapIdx] = '-';
                        }
                        //put path nodes into structure node_path
                        int p = 0;
                        for(p = 0; p < pathLen; p++)
                        {
                            path[p].row = pathRow[(pathLen-1)-p];
                            path[p].col = pathCol[(pathLen-1)-p];
                        }

                        //print map with solution
                        i = 0;
                        int j = 0;
                        for(i=0; i<mapRowSize; i++)
                        {
                            for(j = 0; j<mapColSize; j++)
                            serial_printf(&SerialA,"   %c  ", map[i*mapColSize+j]);
                            serial_printf(&SerialA,"\r\n");
                        }
                    } else if (retvalue == 2) {
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"!!!!!!!!!!!!!!!!Error!!!!!!!!!!!!!!!\r\n");
                        serial_printf(&SerialA,"Already at this point.  No Astar needed.\r\n");
                        serial_printf(&SerialA,"!!!!!!!!!!!!!!!!Error!!!!!!!!!!!!!!!\r\n");
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"\r\n");
                    } else {
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"!!!!!!!!!!!!!!!!Error!!!!!!!!!!!!!!!\r\n");
                        serial_printf(&SerialA,"No Path Found Obstacle in the path.\r\n");
                        serial_printf(&SerialA,"!!!!!!!!!!!!!!!!Error!!!!!!!!!!!!!!!\r\n");
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"\r\n");
                        serial_printf(&SerialA,"\r\n");
                    }
                }
            }
            PathFunction = 2; // set to 3 for waiting for A* again !!PathFunction = 2 to drive path!!
        }

        if (UseLIDARping == 1) {
            UseLIDARping = 0;
            int i = 0;
            // change of basis
            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_ori[i] = pingpts[i].distance*cos(i*0.01745329)/304;//0.017453292519943 is pi/180
                    y_ori[i] = pingpts[i].distance*sin(i*0.01745329)/304;
                    x_f[i].distance = (x_ori[i])*sin(ROBOTps.theta)+(y_ori[i])*cos(ROBOTps.theta)+(ROBOTps.x); //change basis
                    y_f[i].distance = (x_ori[i])*cos(ROBOTps.theta)+(y_ori[i])*sin(ROBOTps.theta)+(ROBOTps.y); //change basis
//                    x_f[i].distance = (x_ori[i]+pose_x)*cos(pose_rad)-(y_ori[i]+pose_y)*sin(pose_rad); //change basis
//                    y_f[i].distance = (x_ori[i]+pose_x)*sin(pose_rad)+(y_ori[i]+pose_y)*cos(pose_rad); //change basis
                }
            }
            if (PathFunction == 2){
                searchObs();
            }
//            UARTPrint = 1;
        }
        if (UseLIDARpong == 1) {
            UseLIDARpong = 0;
//            dist45 = pongpts[45].distance;
//            stamp45 = pongpts[45].timestamp;
//            UARTPrint = 1;
        }
        if (UARTPrint == 1 ) {
            //serial_printf(&SerialA,"%s\r",RXBuff);
//            serial_printf(&SerialA,"X:%.3f, Y:%.3f, Th:%.3f, %.3f, %.3f\r\n",Opti_x,Opti_y,Opti_theta,Opti_Body,Opti_Stamp);
//            serial_printf(&SerialA,"x is %.3f, y is %.3f, theta is %.3f, turn is %.3f, vref is %.3f \r\n",ROBOTps.x, ROBOTps.y, ROBOTps.theta, turn, Vref);

            UARTPrint = 0;
        }
        if (solution == 1 ) {
#ifndef MATLAB
#ifndef SIMULINK

#endif
#endif
        }
    }
}


// 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++;
    // PathFunction == 0 is for wall following
    if (PathFunction == 0){
        measureDist();
        readState();
    }


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

    // Run XY control for Bug
    if (PathFunction == 0){
        x_pos = ROBOTps.x;
        y_pos = ROBOTps.y;
        x_desired = endRow;
        y_desired = endCol;
        thetaabs = ROBOTps.theta;
        xy_control(&vref_forxy, &turn_forxy,turn_thres,x_pos,y_pos,x_desired,y_desired,
                   thetaabs,target_radius,target_radius_near);
        if(Obst == 1){
            if(1 >= alpha){
                // find closest wall reading (right) no zeros
                int i = 0;
//                for (i = 0 ; i < 40; i++ ){
//                    int tempindx = 0;
//                    if(right[i] < right[tempindx]){
//                        if(right[i] > 0){
//                            tempindx = i;
//                            minR = right[tempindx];
//                        }
//                    }
//                }
                wallerror = desiredwall - minR;
                int direction = 1;
                if(wallerror >= 0){
                    direction = 1;
                }else{
                    direction = -1;
                }
                if(wallerror > 75){
                    wallerror = 0;
                }
                if(wallerror < -75){
                    wallerror = 0;
                }
                // Use Kp of 0.1 to start
                turn_forBug = direction*Kpb*wallerror;
                turn = turn_forBug;
                Vref_forBug = 1 - (Kpb * wallerror); // Vref should be slower as it reaches the wall
                Vref = min(fabs(Vref_forBug) , 1);
            }
        }else{
            //vref and turn to XY control
            Vref = 0.5;
            turn = 0;
        }
    }
    if(PathFunction == 2){ // DRIVE THE PATH AFTER A*  !!USE Lidar and obstacle indentification to interrupt PATHFUCTION = 2
        //Run xycontrol for first path node changing desired x and y after each iteration until PathLen amount of times.
        //This should end at the end of the path !!MUST WATCH FOR SLIPPING OF WHEELS!!
        if(target_near == 1){
            nodenumber++;
            target_near = 0;
        }
        //A*
        if(obsSearch == 1){
            if(astarC < 5){
                PathFunction = 1;
                nodenumber = 0;
            }
//            PathFunction = 1;
//            nodenumber = 0;
        }
        x_desired = (path[nodenumber+1].col);
        y_desired = (path[nodenumber+1].row);
        x_pos = ROBOTps.x;
        y_pos = ROBOTps.y;
        thetaabs = ROBOTps.theta;
        xy_control(&vref_forxy, &turn_forxy,turn_thres,x_pos,y_pos,x_desired,y_desired,
                   thetaabs,target_radius,target_radius_near);
        Vref = vref_forxy;
        turn = turn_forxy;
        if(nodenumber == pathLen-1){
            PathFunction = 3;
            Vref = 0;
            turn = 0;
        }
    }

    // distance ft
    XLeftK = 1/(radftL/LeftWheel);
    XRightK = 1/(radftR/RightWheel);
    // vel ft/s
    VLeftK = (XLeftK - XLeftK1)/0.004;
    VRightK = (XRightK - XRightK1)/0.004;

    // PID

    eturn = turn + (VLeftK - VRightK);

    ekLeft = Vref - VLeftK - (Kturn*eturn);
    IkLeft = IkLeft1 + 0.004*((ekLeft + ekLeft1)/2);
    uLeft = (Kp*ekLeft) + (Ki*IkLeft);

    ekRight = Vref - VRightK + (Kturn*eturn);
    IkRight = IkRight1 + 0.004*((ekRight + ekRight1)/2);
    uRight = (Kp+ekRight) + (Ki*IkRight);

    //integral
    if(fabs(uLeft) >= 10) {
        IkLeft = IkLeft1;
    }
    if(fabs(uRight) >= 10){
        IkRight = IkRight1;
    }


    setEPWM2A(uRight);
    setEPWM2B(-uLeft);

    //states
    XLeftK1 = XLeftK;
    XRightK1 = XRightK;
    IkLeft1 = IkLeft;
    IkRight1 = IkRight;
    ekLeft1 = ekLeft;
    ekRight1 = ekRight;

    //POSE and BEARING
    float wr = 0.609375; //width robot
    float rw = 0.104167; //radius wheel
    //wheel angle
    thetaR = RightWheel;
    thetaL = LeftWheel;
    VthetaR = (thetaR -thetaR1)/0.004;
    VthetaL = (thetaL -thetaL1)/0.004;
    pose_d = -(rw/wr)*(VthetaR - VthetaL);
    ROBOTps.theta = ROBOTps.theta + 0.004*(pose_d+pose_d1)/2;
    thetaAvg = 0.5*(RightWheel+LeftWheel); //avg wheel angle
    //RATES
    thetaAvgp = (thetaAvg - thetaAvg1)/0.004;
    Xrp = rw*thetaAvgp*cos(ROBOTps.theta); // rate /s  !!CHECK THIS!! is this ending in rad*ft/s
    Yrp = rw*thetaAvgp*sin(ROBOTps.theta);
    //trap rule = (b-a)*(f(a)+f(b)/2)
    Xr1 = ((Xrp+Xrp1)/2)*0.004;
    Yr1 = ((Yrp+Yrp1)/2)*0.004;
    ROBOTps.x = ROBOTps.x + Xr1;
    ROBOTps.y = ROBOTps.y + Yr1;

//    phi_r_dot = Rwh/Wr*(Vtheta_r-Vtheta_l);
//
//    ROBOTps.theta = ROBOTps.theta + 0.004*(phi_r_dot+phi_r_dot_1)/2;
//    theta_avg = 1.0/2.0*(theta_r+theta_l);
//
//    Vtheta_avg = 1.0/2.0*(Vtheta_r+Vtheta_l);
//    Vxr = Rwh*Vtheta_avg*cos(ROBOTps.theta);
//    Vyr = Rwh*Vtheta_avg*sin(ROBOTps.theta);
//    ROBOTps.x = ROBOTps.x + 0.004*(Vxr+Vxr_1)/2;
//    ROBOTps.y = ROBOTps.y + 0.004*(Vyr+Vyr_1)/2;

    //states
    Xrp1 = Xrp;
    Yrp1 = Yrp;
    thetaR1 = thetaR;
    thetaL1 = thetaL;
    thetaAvg1=thetaAvg;
    pose_d1 = pose_d;
//    Xr = Xr + Xr1;
//    Yr = Yr + Yr1;

    CpuTimer1.InterruptCount++;
}

// cpu_timer2_isr CPU Timer2 ISR
__interrupt void cpu_timer2_isr(void)
{
	
    ReadSwitches(); //for running the PathFunction

    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 ++;

}
//----------------------next step: calculate the correction data---------------------------//
int16_t lidar_data[200];
int16_t lidar_count = 0;
int16_t state = 0; //read data
int16_t sample_size[100]; //sample size array
float start_angle1[100]; // start angle array
float start_angle2[100]; // for debug
float start_angle3[100];  //for debug
float end_angle1[100]; //end angle array
float angle_diff[100]; //array that keep track of angle diff

double end_angle;
double start_angle;
double cal_angle;

float angles[15][40];//record all the angles by calculating with the start/end angle
uint16_t angle_index = 0;
float anglescor[15][40]; //the corrected angles

float distance[15][40]; //distance array
//int16_t packetcount=-1; // count packet, since increase in the first time entering the loop, should set to -1
int16_t startangleLSB = 0; //lsb of start angle
...

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

Credits

Scott Manhart

Scott Manhart

2 projects • 1 follower
Thanks to Yixiao Liu.

Comments