Hardware components | ||||||
 |
| × | 2 | |||
 |
| × | 1 | |||
 |
| × | 1 | |||
 |
| × | 1 | |||
| × | 1 | ||||
Software apps and online services | ||||||
 |
| |||||
Hand tools and fabrication machines | ||||||
 |
| |||||
 |
| |||||
The task of this project is to collect golf balls with the robot as efficiently as possible while traveling to five predefined waypoints and avoiding obstacles on the way. We decided to go with the A* path planning approach as opposed to wall-following due to the lower time it could give us and greater confidence in course compatibility. The robot utilizes the A* algorithm for path planning by communicating between the robot's Raspberry Pi generates a path each time Lidar detects an obstacle and the F28379D board that controls the robot's movement. Two different programs are used, a Linux program and a Code Composer program.
There are a few decisions that need to be made to create this project. When moving diagonally through the corner of an obstacle, the robot takes a risk of hitting it. Therefore, the robot only moves diagonally when there is no obstacle nearby. Since the Lidar might catch wrong signals when it's turning too fast, a threshold of 0.75 is set when detecting obstacles and the lidar data wouldn't be read when it's turning faster than that.
The Team (from left to right): Shixin Zhou, Alex Torres Soto, Rishi Puranik, Jason Zou, Jim Shi
//#############################################################################
// FILE: lab8starter_main.c
//
// TITLE: Lab8 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 "F2837xD_SWPrioritizedIsrLevels.h"
#include "driverlib.h"
#include "device.h"
#include "f28379dSerial.h"
#include "song.h"
#include "dsp.h"
#include "fpu32/fpu_rfft.h"
#include "xy.h"
#include "MatrixMath.h"
#include "SE423Lib.h"
#include "OptiTrack.h"
#define PI 3.1415926535897932384626433832795
#define TWOPI 6.283185307179586476925286766559
#define HALFPI 1.5707963267948966192313216916398
// The Launchpad's CPU Frequency set to 200 you should not change this value
#define LAUNCHPAD_CPU_FREQUENCY 200
#define MIN(A,B) (((A) < (B)) ? (A) : (B));
// Interrupt Service Routines predefinition
__interrupt void cpu_timer0_isr(void);
__interrupt void cpu_timer1_isr(void);
__interrupt void cpu_timer2_isr(void);
__interrupt void SWI1_HighestPriority(void);
__interrupt void SWI2_MiddlePriority(void);
__interrupt void SWI3_LowestPriority(void);
__interrupt void ADCC_ISR(void);
__interrupt void SPIB_isr(void);
void setF28027EPWM1A(float controleffort);
int16_t EPwm1A_F28027 = 1500;
void setF28027EPWM2A(float controleffort);
int16_t EPwm2A_F28027 = 1500;
//structure for pose and obstacle from F28
/*
total length is 2*2 + 2*1 + 22*1 = 28 16 bit chars
*/
typedef struct pose_obstacle{
float x; //4
float y;
short destrow; //astar end point row
short destcol; //astar end point col
char mapCondensed[22];
} pose_obstacle;
//union of char and pose_obstacle for reading data
typedef union {
char obs_data_char[28]; // char is 16bits on F28379D
pose_obstacle cur_obs;
} int_pose_union;
//LV control
float ColorBallIndex = 35;
float ColorOnes =0;
int dontspeakcount = 0;
float robotspeak = 0;
float da = 0;
float db = 0;
//end
extern char path_received[81];
extern int16_t newAstarPath;
int16_t robotdestSize = 39;
int16_t numpts = 0;
int16_t pathRow[50]; // made 50 long but only needs to be 40.
int16_t pathCol[50];
int16_t newpathY[50]; // made 50 long but only needs to be 40.
int16_t newpathX[50];
int16_t StartAstar = 0;
int16_t AstarDelay = 0;
int16_t AstarRunning = 0; // Initially 1 so the robot does not start until the first Astar command has run
int_pose_union SendAStarInfo;
char SendAStarRawData[36];
uint32_t AstarendEqualsStart = 0;
uint32_t AstaroutsideMap = 0;
uint32_t AstarstartstopObstacle = 0;
uint32_t AstarResetMap = 0;
// For A* Default map with no obstacles just door opening
char map[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',
'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',
'x', 'x', 'x', 'x', '0', '0', '0', 'x', 'x', 'x', 'x',
'x', 'x', 'x', '0', '0', '0', '0', '0', 'x', 'x', 'x',
'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' };
char mapstart[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',
'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',
'x', 'x', 'x', 'x', '0', '0', '0', 'x', 'x', 'x', 'x',
'x', 'x', 'x', '0', '0', '0', '0', '0', 'x', 'x', 'x',
'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' };
//add
int16_t obstacleFound = 0;
int16_t globalFoundObs = 0;
float toObsDist = 0;
char mymap[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',
'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',
'x', 'x', 'x', 'x', '0', '0', '0', 'x', 'x', 'x', 'x',
'x', 'x', 'x', '0', '0', '0', '0', '0', 'x', 'x', 'x',
'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'};
//obsdefine vari
typedef struct{
int r;
int c;
int x;
int y;
int tally;
int found;
int idx1;
int idx2;
int idx3;
int sendLV;
}edges;
edges obs[57];
uint32_t numTimer0calls = 0;
uint16_t UARTPrint = 0;
float printLV1 = 0;
float printLV2 = 0;
float printLinux1 = 0;
float printLinux2 = 0;
uint16_t LIDARi = 0;
char G_command[] = "G04472503\n"; //command for getting distance -120 to 120 degree
uint16_t G_len = 11; //length of command
xy lidar_pts[228]; //xy data
float LIDARrightfront = 0;
float LIDARfront = 0;
float LIDARright = 0;
float LIDARtemp_x = 0;
float LIDARtemp_y = 0;
extern datapts lidar_data[228];
extern uint16_t newLinuxCommands;
extern float LinuxCommands[11];
extern uint16_t NewLVData;
extern float fromLVvalues[LVNUM_TOFROM_FLOATS];
extern LVSendFloats_t DataToLabView;
extern char LVsenddata[LVNUM_TOFROM_FLOATS*4+2];
extern uint16_t LIDARpingpong;
extern uint16_t NewCAMDataThreshold1; // Flag new data
extern float fromCAMvaluesThreshold1[CAMNUM_FROM_FLOATS];
extern uint16_t NewCAMDataThreshold2; // Flag new data
extern float fromCAMvaluesThreshold2[CAMNUM_FROM_FLOATS];
float MaxAreaThreshold1 = 0;
float MaxColThreshold1 = 0;
float MaxRowThreshold1 = 0;
float NextLargestAreaThreshold1 = 0;
float NextLargestColThreshold1 = 0;
float NextLargestRowThreshold1 = 0;
float NextNextLargestAreaThreshold1 = 0;
float NextNextLargestColThreshold1 = 0;
float NextNextLargestRowThreshold1 = 0;
float MaxAreaThreshold2 = 0;
float MaxColThreshold2 = 0;
float MaxRowThreshold2 = 0;
float NextLargestAreaThreshold2 = 0;
float NextLargestColThreshold2 = 0;
float NextLargestRowThreshold2 = 0;
float NextNextLargestAreaThreshold2 = 0;
float NextNextLargestColThreshold2 = 0;
float NextNextLargestRowThreshold2 = 0;
uint32_t numThres1 = 0;
uint32_t numThres2 = 0;
pose ROBOTps = {0,0,0}; //robot position
pose LADARps = {3.5/12.0,0,1}; // 3.5/12 for front mounting, theta is not used in this current code
float LADARxoffset = 0;
float LADARyoffset = 0;
//add timecount
uint32_t timecount = 0;
uint32_t timecount2 = 0;
uint32_t timecount3 = 0;
int16_t RobotState = 1;
int16_t checkfronttally = 0;
int32_t WallFollowtime = 0;
//add
float max_area = 60;//change from 75 to 60
int reverse_count = 0;
#define NUMWAYPOINTS 9
uint16_t statePos = 0; // it was set to 0 before. For some unknown reason statePos starts at 1. If found cause, change back to 0. This is a bandaid fix to get it to go to state 0 first
pose robotdest[NUMWAYPOINTS]; // array of waypoints for the robot (with 2 additional for chute drop offs
uint16_t i = 0;//for loop
uint16_t astar_pos_idx = 0; // index to follow in astar path planning
uint16_t right_wall_follow_state = 2; // right follow
//add adjust
float ref_right_wall = 1.2;
float left_turn_Start_threshold = 1.5;
float left_turn_Stop_threshold = 2;
float Kp_right_wal = -3.5;
float Kp_front_wall = -1.5;
float front_turn_velocity = 0.4;
float foward_velocity = 1.2; // robot speed
float turn_saturation = 3.0;
float x_pred[3][1] = {{0},{0},{0}}; // predicted state
//more kalman vars
float B[3][2] = {{1,0},{1,0},{0,1}}; // control input model
float u[2][1] = {{0},{0}}; // control input in terms of velocity and angular velocity
float Bu[3][1] = {{0},{0},{0}}; // matrix multiplication of B and u
float z[3][1]; // state measurement
float eye3[3][3] = {{1,0,0},{0,1,0},{0,0,1}}; // 3x3 identity matrix
float K[3][3] = {{1,0,0},{0,1,0},{0,0,1}}; // optimal Kalman gain
#define ProcUncert 0.0001
#define CovScalar 10
float Q[3][3] = {{ProcUncert,0,ProcUncert/CovScalar},
{0,ProcUncert,ProcUncert/CovScalar},
{ProcUncert/CovScalar,ProcUncert/CovScalar,ProcUncert}}; // process noise (covariance of encoders and gyro)
#define MeasUncert 1
float R[3][3] = {{MeasUncert,0,MeasUncert/CovScalar},
{0,MeasUncert,MeasUncert/CovScalar},
{MeasUncert/CovScalar,MeasUncert/CovScalar,MeasUncert}}; // measurement noise (covariance of OptiTrack Motion Capture measurement)
float S[3][3] = {{1,0,0},{0,1,0},{0,0,1}}; // innovation covariance
float S_inv[3][3] = {{1,0,0},{0,1,0},{0,0,1}}; // innovation covariance matrix inverse
float P_pred[3][3] = {{1,0,0},{0,1,0},{0,0,1}}; // predicted covariance (measure of uncertainty for current position)
float temp_3x3[3][3]; // intermediate storage
float temp_3x1[3][1]; // intermediate storage
float ytilde[3][1]; // difference between predictions
// Optitrack Variables
int32_t OptiNumUpdatesProcessed = 0;
pose OPTITRACKps;
extern uint16_t new_optitrack;
extern float Optitrackdata[OPTITRACKDATASIZE];
int16_t newOPTITRACKpose=0;
int16_t adcc2result = 0;
int16_t adcc3result = 0;
int16_t adcc4result = 0;
int16_t adcc5result = 0;
float adcC2Volt = 0.0;
float adcC3Volt = 0.0;
float adcC4Volt = 0.0;
float adcC5Volt = 0.0;
int32_t numADCCcalls = 0;
float LeftWheel = 0;
float RightWheel = 0;
float LeftWheel_1 = 0;
float RightWheel_1 = 0;
float LeftVel = 0;
float RightVel = 0;
float uLeft = 0;
float uRight = 0;
float HandValue = 0;
float vref = 0;
float turn = 0;
float gyro9250_drift = 0;
float gyro9250_angle = 0;
float old_gyro9250 = 0;
float gyroLPR510_angle = 0;
float gyroLPR510_offset = 0;
float gyroLPR510_drift = 0;
float old_gyroLPR510 = 0;
float gyro9250_radians = 0;
float gyroLPR510_radians = 0;
int16_t readdata[25];
int16_t IMU_data[9];
float accelx = 0;
float accely = 0;
float accelz = 0;
float gyrox = 0;
float gyroy = 0;
float gyroz = 0;
// Needed global Variables
float accelx_offset = 0;
float accely_offset = 0;
float accelz_offset = 0;
//add
float equation = 0;
float equation2 = 0;
float colcentroid = 0;
float colcentroid2 = 0;
float kpvision = -0.03; //default: -0.05
uint32_t countState20 = 0; // count state for orange ball collection
uint32_t countState30 = 0; // count state for purple ball collection
uint32_t countState40 = 0; // count state for reversing out of chute
float rowmax = 220.0; //default: 220
float gyrox_offset = 0;
float gyroy_offset = 0;
float gyroz_offset = 0;
int16_t calibration_state = 0;
int32_t calibration_count = 0;
int16_t doneCal = 0;
// FINAL PROJECT GLOBAL VARIABLES
int current_balls = 0;
float gate_close_angle = 20.0;
float gate_open_angle = -30.0;
// count for the amount of balls being held by the robot
int16_t orange_ball_count = 0;
int16_t purple_ball_count = 0;
// when should robot drop off balls
int16_t ball_dropoff = 3;
#define MPU9250 1
#define DAN28027 2
int16_t CurrentChip = MPU9250;
int16_t DAN28027Garbage = 0;
int16_t dan28027adc1 = 0;
int16_t dan28027adc2 = 0;
uint16_t MPU9250ignoreCNT = 0; //This is ignoring the first few interrupts if ADCC_ISR and start sending to IMU after these first few interrupts.
void main(void)
{
// PLL, WatchDog, enable Peripheral Clocks
// This example function is found in the F2837xD_SysCtrl.c file.
InitSysCtrl();
InitGpio();
InitSE423DefaultGPIO();
//Scope for Timing
GPIO_SetupPinMux(11, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(11, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO11 = 1;
GPIO_SetupPinMux(32, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(32, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPBCLEAR.bit.GPIO32 = 1;
GPIO_SetupPinMux(52, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(52, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPBCLEAR.bit.GPIO52 = 1;
GPIO_SetupPinMux(60, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(60, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPBCLEAR.bit.GPIO60 = 1;
GPIO_SetupPinMux(61, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(61, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPBCLEAR.bit.GPIO61 = 1;
GPIO_SetupPinMux(67, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(67, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPCCLEAR.bit.GPIO67 = 1;
// 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.SCIB_RX_INT = &RXBINT_recv_ready;
PieVectTable.SCIC_RX_INT = &RXCINT_recv_ready;
PieVectTable.SCID_RX_INT = &RXDINT_recv_ready;
PieVectTable.SCIA_TX_INT = &TXAINT_data_sent;
PieVectTable.SCIB_TX_INT = &TXBINT_data_sent;
PieVectTable.SCIC_TX_INT = &TXCINT_data_sent;
PieVectTable.SCID_TX_INT = &TXDINT_data_sent;
PieVectTable.ADCC1_INT = &ADCC_ISR;
PieVectTable.SPIB_RX_INT = &SPIB_isr;
PieVectTable.EMIF_ERROR_INT = &SWI1_HighestPriority; // Using Interrupt12 interrupts that are not used as SWIs
PieVectTable.RAM_CORRECTABLE_ERROR_INT = &SWI2_MiddlePriority;
PieVectTable.FLASH_CORRECTABLE_ERROR_INT = &SWI3_LowestPriority;
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, Period (in uSeconds)
ConfigCpuTimer(&CpuTimer0, LAUNCHPAD_CPU_FREQUENCY, 10000); // Currently not used for any purpose
ConfigCpuTimer(&CpuTimer1, LAUNCHPAD_CPU_FREQUENCY, 100000); // !!!!! Important, Used to command LADAR every 100ms. Do not Change.
ConfigCpuTimer(&CpuTimer2, LAUNCHPAD_CPU_FREQUENCY, 40000); // Currently not used for any purpose
// Enable CpuTimer Interrupt bit TIE
CpuTimer0Regs.TCR.all = 0x4000;
CpuTimer1Regs.TCR.all = 0x4000;
CpuTimer2Regs.TCR.all = 0x4000;
DELAY_US(1000000); // Delay 1 second giving Lidar Time to power on after system power on
init_serialSCIA(&SerialA,115200,NULL);
init_serialSCIB(&SerialB,19200,NULL);
init_serialSCIC(&SerialC,19200,NULL);
init_serialSCID(&SerialD,115200,NULL);
for (LIDARi = 0; LIDARi < 228; LIDARi++) {
lidar_data[LIDARi].angle = ((3*LIDARi+44)*0.3515625-135)*0.01745329; //0.017453292519943 is pi/180, multiplication is faster; 0.3515625 is 360/1024
}
init_eQEPs();
init_EPWM1and2();
init_RCServoPWM_3AB_5AB_6A();
init_ADCsAndDACs();
setupSpib();
EALLOW;
EPwm4Regs.ETSEL.bit.SOCAEN = 0; // Disable SOC on A group
EPwm4Regs.TBCTL.bit.CTRMODE = 3; // freeze counter
EPwm4Regs.ETSEL.bit.SOCASEL = 2; // Select Event when counter equal to PRD
EPwm4Regs.ETPS.bit.SOCAPRD = 1; // Generate pulse on 1st event
EPwm4Regs.TBCTR = 0x0; // Clear counter
EPwm4Regs.TBPHS.bit.TBPHS = 0x0000; // Phase is 0
EPwm4Regs.TBCTL.bit.PHSEN = 0; // Disable phase loading
EPwm4Regs.TBCTL.bit.CLKDIV = 0; // divide by 1 50Mhz Clock
EPwm4Regs.TBPRD = 50000; // Set Period to 1ms sample. Input clock is 50MHz.
// Notice here that we are not setting CMPA or CMPB because we are not using the PWM signal
EPwm4Regs.ETSEL.bit.SOCAEN = 1; //enable SOCA
//EPwm4Regs.TBCTL.bit.CTRMODE = 0; //unfreeze, wait to do this right before enabling interrupts
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_INT6;
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;
PieCtrlRegs.PIEIER1.bit.INTx3 = 1;
PieCtrlRegs.PIEIER6.bit.INTx3 = 1;
PieCtrlRegs.PIEIER12.bit.INTx9 = 1; //SWI1
PieCtrlRegs.PIEIER12.bit.INTx10 = 1; //SWI2
PieCtrlRegs.PIEIER12.bit.INTx11 = 1; //SWI3 Lowest priority
//new dest
// robotdest[0].x = -5; robotdest[0].y = -3;
//
// robotdest[1].x = 3; robotdest[1].y = 7;
// //middle of bottom
// robotdest[2].x = -3; robotdest[2].y = 7;
// //outside the course
// robotdest[3].x = 5; robotdest[3].y = -3;
// //back to middle
// robotdest[4].x = 0; robotdest[4].y = 11;
// robotdest[5].x = 0; robotdest[5].y = -1;
// // location of chutes
// robotdest[6].x = -2; robotdest[6].y = -4;
// robotdest[7].x = 2; robotdest[7].y = -4;
robotdest[0].x = -1; robotdest[0].y = -2;
robotdest[1].x = -5; robotdest[1].y = -3; // first waypoint
robotdest[2].x = 3; robotdest[2].y = 7;
//middle of bottom
robotdest[3].x = -3; robotdest[3].y = 7;
//outside the course
robotdest[4].x = 5; robotdest[4].y = -3;
//back to middle
robotdest[5].x = 0; robotdest[5].y = 10.5;
robotdest[6].x = 0; robotdest[6].y = -1;
// location of chutes
robotdest[7].x = -2; robotdest[7].y = -4;
robotdest[8].x = 2; robotdest[8].y = -4;
//add obsdefine
//first row
obs[0].r = 1; obs[0].c =0; // r, c are in A-star co-ordinate system. x,y are in robot co-ordinate system
obs[1].r = 1; obs[1].c =2;
obs[2].r = 1; obs[2].c= 4;
obs[3].r = 1; obs[3].c =6;
obs[4].r = 1; obs[4].c= 8;
obs[5].r = 1; obs[5].c =10;
//first colum
obs[6].r = 2; obs[6].c = 1;
obs[7].r = 2; obs[7].c = 3;
obs[8].r = 2; obs[8].c = 5;
obs[9].r = 2; obs[9].c = 7;
obs[10].r = 2; obs[10].c = 9;
//second set of rows
obs[11].r = 3; obs[11].c =0;
obs[12].r = 3; obs[12].c =2;
obs[13].r = 3; obs[13].c= 4;
obs[14].r = 3; obs[14].c =6;
obs[15].r = 3; obs[15].c= 8;
obs[16].r = 3; obs[16].c =10;
//second set of columns
obs[17].r = 4; obs[17].c = 1;
obs[18].r = 4; obs[18].c = 3;
obs[19].r = 4; obs[19].c = 5;
obs[20].r = 4; obs[20].c = 7;
obs[21].r = 4; obs[21].c = 9;
//Third set of rows
obs[22].r = 5; obs[22].c =0;
obs[23].r = 5; obs[23].c =2;
obs[24].r = 5; obs[24].c= 4;
obs[25].r = 5; obs[25].c =6;
obs[26].r = 5; obs[26].c= 8;
obs[27].r = 5; obs[27].c =10;
//Third set of columns
obs[28].r = 6; obs[28].c = 1;
obs[29].r = 6; obs[29].c = 3;
obs[30].r = 6; obs[30].c = 5;
obs[31].r = 6; obs[31].c = 7;
obs[32].r = 6; obs[32].c = 9;
//Fourth set of rows
obs[33].r = 7; obs[33].c =0;
obs[34].r = 7; obs[34].c =2;
obs[35].r = 7; obs[35].c= 4;
obs[36].r = 7; obs[36].c =6;
obs[37].r = 7; obs[37].c= 8;
obs[38].r = 7; obs[38].c =10;
//Fourth set of columns
obs[39].r = 8; obs[39].c = 1;
obs[40].r = 8; obs[40].c = 3;
obs[41].r = 8; obs[41].c = 5;
obs[42].r = 8; obs[42].c = 7;
obs[43].r = 8; obs[43].c = 9;
//Fifth set of rows
obs[44].r = 9; obs[44].c =0;
obs[45].r = 9; obs[45].c =2;
obs[46].r = 9; obs[46].c= 4;
obs[47].r = 9; obs[47].c =6;
obs[48].r = 9; obs[48].c= 8;
obs[49].r = 9; obs[49].c =10;
//Fifth set of columns
obs[50].r = 10; obs[50].c = 1;
obs[51].r = 10; obs[51].c = 3;
obs[52].r = 10; obs[52].c = 5;
obs[53].r = 10; obs[53].c = 7;
obs[54].r = 10; obs[54].c = 9;
obs[55].r = 11; obs[55].c = 4; obs[55].x = -1; obs[55].y = 0; obs[55].sendLV =0;
obs[55].idx1 = 124 ; obs[55].idx2 = 125 ; obs[55].idx3 = 126 ; obs[55].found = 0 ; obs[55].tally = 0;
obs[56].r = 11; obs[56].c = 6; obs[56].x = 1; obs[56].y = 0; obs[56].sendLV =0;
obs[56].idx1 = 126 ; obs[56].idx2 = 127 ; obs[56].idx3 = 128 ; obs[56].found = 0 ; obs[56].tally = 0;
int16_t ii = 0;
for( ii= 0; ii < 55 ; ii++){
obs[ii].x = obs[ii].c - 5;
obs[ii].y = 11-obs[ii].r;
obs[ii].tally = 0;
obs[ii].found = 0;
obs[ii].sendLV =0;
obs[ii].idx2 = obs[ii].r*11 + obs[ii].c;
if( (obs[ii].r%2) !=0){
if ( obs[ii].c == 0){
obs[ii].idx1 = 400;
}
else{
obs[ii].idx1 = obs[ii].idx2 - 1;
}
if ( obs[ii].c == 10){
obs[ii].idx3 = 400;
}
else{
obs[ii].idx3 = obs[ii].idx2 + 1;
}
}
else {
if ( obs[ii].r != 10) {
obs[ii].idx1 = obs[ii].idx2 -11 ;
obs[ii].idx3 = obs[ii].idx2 +11;
}
else {
obs[ii].idx1 = obs[ii].idx2 -11 ;
obs[ii].idx3 = 400;
}
}
}
obs[52].r = 10; obs[52].c = 5; obs[52].idx1 = 104; obs[52].idx2 = 115 ; obs[52].idx3 = 126;
//end
// ROBOTps will be updated by Optitrack during gyro calibration
// TODO: specify the starting position of the robot
ROBOTps.x = 0; //the estimate in array form (useful for matrix operations)
ROBOTps.y = 0;
ROBOTps.theta = 0; // was -PI: need to flip OT ground plane to fix this
x_pred[0][0] = ROBOTps.x; //estimate in structure form (useful elsewhere)
x_pred[1][0] = ROBOTps.y;
x_pred[2][0] = ROBOTps.theta;
AdccRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //clear interrupt flag
PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
SpibRegs.SPIFFRX.bit.RXFFOVFCLR=1; // Clear Overflow flag
SpibRegs.SPIFFRX.bit.RXFFINTCLR=1; // Clear Interrupt flag
PieCtrlRegs.PIEACK.all = PIEACK_GROUP6;
EPwm4Regs.TBCTL.bit.CTRMODE = 0; //unfreeze, and enter up count mode
// Enable global Interrupts and higher priority real-time debug events
EINT; // Enable Global interrupt INTM
ERTM; // Enable Global realtime interrupt DBGM
char S_command[19] = "S1152000124000\n";//this change the baud rate to 115200
uint16_t S_len = 19;
serial_sendSCIC(&SerialC, S_command, S_len);
DELAY_US(1000000); // Delay letting Lidar change its Baud rate
init_serialSCIC(&SerialC,115200,NULL);
// LED1 is GPIO22
GpioDataRegs.GPACLEAR.bit.GPIO22 = 1;
// LED2 is GPIO94
GpioDataRegs.GPCCLEAR.bit.GPIO94 = 1;
// LED3 is GPIO95
GpioDataRegs.GPCCLEAR.bit.GPIO95 = 1;
// LED4 is GPIO97
GpioDataRegs.GPDCLEAR.bit.GPIO97 = 1;
// LED5 is GPIO111
GpioDataRegs.GPDCLEAR.bit.GPIO111 = 1;
// IDLE loop. Just sit and loop forever (optional):
while(1)
{
if (UARTPrint == 1 ) {
if (readbuttons() == 0) {
UART_printfLine(1,"state: %d",RobotState);
UART_printfLine(2,"x:%.2f:y:%.2f:a%.2f",ROBOTps.x,ROBOTps.y,ROBOTps.theta);
} else if (readbuttons() == 1) {
UART_printfLine(1,"O1A:%.0fC:%.0fR:%.0f",MaxAreaThreshold1,MaxColThreshold1,MaxRowThreshold1);
UART_printfLine(2,"P1A:%.0fC:%.0fR:%.0f",MaxAreaThreshold2,MaxColThreshold2,MaxRowThreshold2);
//UART_printfLine(1,"LV1:%.3f LV2:%.3f",printLV1,printLV2);
//UART_printfLine(2,"Ln1:%.3f Ln2:%.3f",printLinux1,printLinux2);
} else if (readbuttons() == 2) {
UART_printfLine(1,"O2A:%.0fC:%.0fR:%.0f",NextLargestAreaThreshold1,NextLargestColThreshold1,NextLargestRowThreshold1);
UART_printfLine(2,"P2A:%.0fC:%.0fR:%.0f",NextLargestAreaThreshold2,NextLargestColThreshold2,NextLargestRowThreshold2);
// UART_printfLine(1,"%.2f %.2f",adcC2Volt,adcC3Volt);
// UART_printfLine(2,"%.2f %.2f",adcC4Volt,adcC5Volt);
} else if (readbuttons() == 4) {
UART_printfLine(1,"O3A:%.0fC:%.0fR:%.0f",NextNextLargestAreaThreshold1,NextNextLargestColThreshold1,NextNextLargestRowThreshold1);
UART_printfLine(2,"P3A:%.0fC:%.0fR:%.0f",NextNextLargestAreaThreshold2,NextNextLargestColThreshold2,NextNextLargestRowThreshold2);
// UART_printfLine(1,"L:%.3f R:%.3f",LeftVel,RightVel);
// UART_printfLine(2,"uL:%.2f uR:%.2f",uLeft,uRight);
} else if (readbuttons() == 8) {
UART_printfLine(1,"020x%.2f y%.2f",lidar_pts[20].x,lidar_pts[20].y);
UART_printfLine(2,"150x%.2f y%.2f",lidar_pts[150].x,lidar_pts[150].y);
} else if (readbuttons() == 3) {
UART_printfLine(1,"Vrf:%.2f trn:%.2f",vref,turn);
UART_printfLine(2,"MPU:%.2f LPR:%.2f",gyro9250_radians,gyroLPR510_radians);
} else if (readbuttons() == 5) {
UART_printfLine(1,"Ox:%.2f:Oy:%.2f:Oa%.2f",OPTITRACKps.x,OPTITRACKps.y,OPTITRACKps.theta);
UART_printfLine(2,"State:%d : %d",RobotState,statePos);
}
UARTPrint = 0;
}
}
}
__interrupt void SPIB_isr(void){
uint16_t i;
GpioDataRegs.GPBSET.bit.GPIO32 = 1;
GpioDataRegs.GPCSET.bit.GPIO66 = 1; //Pull CS high as done R/W
GpioDataRegs.GPASET.bit.GPIO29 = 1; // Pull CS high for DAN28027
if (CurrentChip == MPU9250) {
for (i=0; i<8; i++) {
readdata[i] = SpibRegs.SPIRXBUF; // readdata[0] is garbage
}
PostSWI1(); // Manually cause the interrupt for the SWI1
} else if (CurrentChip == DAN28027) {
DAN28027Garbage = SpibRegs.SPIRXBUF;
dan28027adc1 = SpibRegs.SPIRXBUF;
dan28027adc2 = SpibRegs.SPIRXBUF;
CurrentChip = MPU9250;
SpibRegs.SPIFFCT.bit.TXDLY = 0;
}
SpibRegs.SPIFFRX.bit.RXFFOVFCLR=1; // Clear Overflow flag
SpibRegs.SPIFFRX.bit.RXFFINTCLR=1; // Clear Interrupt flag
PieCtrlRegs.PIEACK.all = PIEACK_GROUP6;
GpioDataRegs.GPBCLEAR.bit.GPIO32 = 1;
}
//adcd1 pie interrupt
__interrupt void ADCC_ISR (void)
{
GpioDataRegs.GPASET.bit.GPIO11 = 1;
adcc2result = AdccResultRegs.ADCRESULT0;
adcc3result = AdccResultRegs.ADCRESULT1;
adcc4result = AdccResultRegs.ADCRESULT2;
adcc5result = AdccResultRegs.ADCRESULT3;
// Here covert ADCIND0 to volts
adcC2Volt = adcc2result*3.0/4095.0;
adcC3Volt = adcc3result*3.0/4095.0;
adcC4Volt = adcc4result*3.0/4095.0;
adcC5Volt = adcc5result*3.0/4095.0;
if (MPU9250ignoreCNT >= 1) {
CurrentChip = MPU9250;
SpibRegs.SPIFFCT.bit.TXDLY = 0;
SpibRegs.SPIFFRX.bit.RXFFIL = 8;
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;
SpibRegs.SPITXBUF = ((0x8000)|(0x3A00));
SpibRegs.SPITXBUF = 0;
SpibRegs.SPITXBUF = 0;
SpibRegs.SPITXBUF = 0;
SpibRegs.SPITXBUF = 0;
SpibRegs.SPITXBUF = 0;
SpibRegs.SPITXBUF = 0;
SpibRegs.SPITXBUF = 0;
} else {
MPU9250ignoreCNT++;
}
numADCCcalls++;
AdccRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //clear interrupt flag
PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
GpioDataRegs.GPACLEAR.bit.GPIO11 = 1;
}
// cpu_timer0_isr - CPU Timer0 ISR
__interrupt void cpu_timer0_isr(void)
{
CpuTimer0.InterruptCount++;
numTimer0calls++;
if ((numTimer0calls%5) == 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)
{
serial_sendSCIC(&SerialC, G_command, G_len);
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 ((CpuTimer2.InterruptCount % 10) == 0) {
// UARTPrint = 1;
// }
}
void setF28027EPWM1A(float controleffort){
if (controleffort < -10) {
controleffort = -10;
}
if (controleffort > 10) {
controleffort = 10;
}
float value = (controleffort+10)*3000.0/20.0;
EPwm1A_F28027 = (int16_t)value; // Set global variable that is sent over SPI to F28027
}
void setF28027EPWM2A(float controleffort){
if (controleffort < -10) {
controleffort = -10;
}
if (controleffort > 10) {
controleffort = 10;
}
float value = (controleffort+10)*3000.0/20.0;
EPwm2A_F28027 = (int16_t)value; // Set global variable that is sent over SPI to F28027
}
//
// Connected to PIEIER12_9 (use MINT12 and MG12_9 masks):
//
__interrupt void SWI1_HighestPriority(void) // EMIF_ERROR
{
GpioDataRegs.GPBSET.bit.GPIO61 = 1;
// Set interrupt priority:
volatile Uint16 TempPIEIER = PieCtrlRegs.PIEIER12.all;
IER |= M_INT12;
IER &= MINT12; // Set "global" priority
PieCtrlRegs.PIEIER12.all &= MG12_9; // Set "group" priority
PieCtrlRegs.PIEACK.all = 0xFFFF; // Enable PIE interrupts
__asm(" NOP");
EINT;
//##############################################################################################################
IMU_data[0] = readdata[1];
IMU_data[1] = readdata[2];
IMU_data[2] = readdata[3];
IMU_data[3] = readdata[5];
IMU_data[4] = readdata[6];
IMU_data[5] = readdata[7];
accelx = (((float)(IMU_data[0]))*4.0/32767.0);
accely = (((float)(IMU_data[1]))*4.0/32767.0);
accelz = (((float)(IMU_data[2]))*4.0/32767.0);
gyrox = (((float)(IMU_data[3]))*250.0/32767.0);
gyroy = (((float)(IMU_data[4]))*250.0/32767.0);
gyroz = (((float)(IMU_data[5]))*250.0/32767.0);
if(calibration_state == 0){
calibration_count++;
if (calibration_count == 2000) {
calibration_state = 1;
calibration_count = 0;
}
} else if(calibration_state == 1){
accelx_offset+=accelx;
accely_offset+=accely;
accelz_offset+=accelz;
gyrox_offset+=gyrox;
gyroy_offset+=gyroy;
gyroz_offset+=gyroz;
gyroLPR510_offset+=adcC5Volt;
calibration_count++;
if (calibration_count == 2000) {
calibration_state = 2;
accelx_offset/=2000.0;
accely_offset/=2000.0;
accelz_offset/=2000.0;
gyrox_offset/=2000.0;
gyroy_offset/=2000.0;
gyroz_offset/=2000.0;
gyroLPR510_offset/=2000.0;
calibration_count = 0;
}
} else if(calibration_state == 2) {
gyroLPR510_drift += (((adcC5Volt-gyroLPR510_offset) + old_gyroLPR510)*.0005)/(2000);
old_gyroLPR510 = adcC5Volt-gyroLPR510_offset;
gyro9250_drift += (((gyroz-gyroz_offset) + old_gyro9250)*.0005)/(2000);
old_gyro9250 = gyroz-gyroz_offset;
calibration_count++;
if (calibration_count == 2000) {
calibration_state = 3;
calibration_count = 0;
doneCal = 1;
newAstarPath = 0;
newOPTITRACKpose = 0;
AstarDelay = 0;
}
} else if(calibration_state == 3){
if (AstarDelay == 1000) {
StartAstar = 1; // First Astar to get first path.
AstarDelay++;
} else {
AstarDelay++;
}
accelx -=(accelx_offset);
accely -=(accely_offset);
accelz -=(accelz_offset);
gyrox -= gyrox_offset;
gyroy -= gyroy_offset;
gyroz -= gyroz_offset;
LeftWheel = readEncLeft();
RightWheel = readEncRight();
HandValue = readEncWheel();
gyro9250_angle = gyro9250_angle + (gyroz + old_gyro9250)*.0005 - gyro9250_drift;
old_gyro9250 = gyroz;
gyroLPR510_angle = gyroLPR510_angle + ((adcC5Volt-gyroLPR510_offset) + old_gyroLPR510)*.0005 - gyroLPR510_drift;
old_gyroLPR510 = adcC5Volt-gyroLPR510_offset;
gyro9250_radians = (gyro9250_angle * (PI/180.0));
gyroLPR510_radians = gyroLPR510_angle * 400 * (PI/180.0);
LeftVel = (1.235/12.0)*(LeftWheel - LeftWheel_1)*1000;
RightVel = (1.235/12.0)*(RightWheel - RightWheel_1)*1000;
if (newLinuxCommands == 1) {
newLinuxCommands = 0;
printLinux1 = LinuxCommands[0];
printLinux2 = LinuxCommands[1];
//?? = LinuxCommands[2];
//?? = LinuxCommands[3];
//?? = LinuxCommands[4];
//?? = LinuxCommands[5];
//?? = LinuxCommands[6];
//?? = LinuxCommands[7];
//?? = LinuxCommands[8];
...
This file has been truncated, please download it to see its full contents.
/*
Whenever it sees the semaphore for send is 1, get the data from shared memory and process A*
Send the path to send shm, and post the semaphore for read.
*/
#include <sys/ioctl.h>
#include <sched.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <string.h>
#include <unistd.h>
#include <semaphore.h>
#include <sys/mman.h>
#include <stdio.h> //for printf and scanf
#include <math.h> //for abs
#include <termios.h>
#include <sys/signal.h>
#include <sys/time.h>
#include <asm/ioctls.h>
#include <linux/serial.h>
/*
functions implemented in pQueue.cpp
priority queue functions: --> necessary for open list and closed list
size()
peek()
push()
pop()
*/
/*
priority queue elements aka nodes properties
position (x and y position for this case) called xPox, yPos
total distance (f), distance traveled from start (g), distance to goal(h) called totalDist, distTravelFromStart, distToGoal
--> defined in pQueue.h
*/
#include "pQueue.h"
// Types and Variables for AstarApp COM *************************
#define SENDTO_ASTARAPP_SEM_MUTEX_NAME "/sem-AstarApp-sendto-pose-obstacle"
#define SENDTO_ASTARAPP_SHARED_MEM_NAME "/sharedmem-AstarApp-sendto-pose-obstacle"
#define READFROM_ASTARAPP_SEM_MUTEX_NAME "/sem-AstarApp-readfrom-new-path"
#define READFROM_ASTARAPP_SHARED_MEM_NAME "/sharedmem-AstarApp-readfrom-new-path"
#define MIN(A,B) (((A) < (B)) ? (A) : (B));
//structure for pose and obstacle from F28
/*
total length is 2*4 + 2*2 + 22*1 = 34 bytes
*/
typedef struct pose_obstacle{
float x; //4
float y;
short destrow; //astar end point row
short destcol; //astar end point col
char mapCondensed[22];
} pose_obstacle;
//union of char and pose_obstacle for reading data
typedef union {
char obs_data_char[34];
pose_obstacle cur_obs;
} int_pose_union;
struct shared_memory_sendto_AstarApp
{
int_pose_union new_pose;
};
struct shared_memory_readfrom_AstarApp
{
char read_path[81]; // Path up to 80 long. 81st char is a status char. char81 = [bit7=NA
//bit6=NA
//bit5=NA
//bit4=NA
//bit3=start or stop are obstacles
//bit2=start or stop outside map
//bit1=1 start == endpoint
//bit0=1 Need to reset Map]
};
// Print system error and exit
void error (char *msg)
{
perror (msg);
exit (1);
}
//path data to send to F28, 10 points in the format of row1,col1,row2,col2...initialize to 199(large number that can never happen)
int sem_count_read = 0;
int_pose_union pose_obs_data;
char mychar;
static int gs_quit = 0;
char planned_path[80]={[0 ... 79] = 199}; //the path generated from A*
int first_time = 0;//first time enter the program, used to get the initial path
//blocking get keyboard input
int mygetch(void)
{
struct termios oldt,
newt;
int ch;
tcgetattr(STDIN_FILENO, &oldt);
newt = oldt;
newt.c_lflag &= ~(ICANON | ECHO);
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
ch = getchar();
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
return ch;
}
//non-blocking keyboard input detection
int _kbhit()
{
static const int STDIN = 0;
static int initialized = 0;
if (!initialized)
{
// Use termios to turn off line buffering
struct termios term;
tcgetattr(STDIN, &term);
term.c_lflag &= ~ICANON;
tcsetattr(STDIN, TCSANOW, &term);
setbuf(stdin, NULL);
initialized = 1;
}
int bytesWaiting;
ioctl(STDIN, FIONREAD, &bytesWaiting);
return bytesWaiting;
}
//global variables necessary for the algorithm
node_t neighbors[8]; //used to get a list of neighboring nodes only need 4 but made it 8 if you want to add corners later
double 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];
int endRow = 0; //row of destination
int endCol = 0; //col of destination
// int oldendRow = 0; //store the last endrow
// int oldendCol = 0; //store the last endcol
int retvalue = 0;
char returnstatus = 0;
char map[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',
'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',
'x', 'x', 'x', 'x', '0', '0', '0', 'x', 'x', 'x', 'x',
'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' };
char map_sol[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',
'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',
'x', 'x', 'x', 'x', '0', '0', '0', 'x', 'x', 'x', 'x',
'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' };
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;
//preconditions: rowCurr, colCurr, rowGoal, and colGoal are feasible locations in the matrix
//returns the distance between two points as the sum of the row and column differences "Euclidean" distance on a square grid
double heuristic(int rowCurr, int colCurr, int rowGoal, int colGoal)
{
int rowDiff = rowCurr - rowGoal;
int colDiff = colCurr - colGoal;
return sqrt(rowDiff * rowDiff + colDiff * colDiff);
}
//assumes that canTravel is called with neighboring points of a valid starting point
//precondition: row, col must be valid indices for a matrix
//returns 0 for false and 1 for true
//(row, col) is considered an unacceptable traveling point if it is represented as an 'x' in map, or if it is out of bounds of the map
int canTravel(int row, int col)
{
//check for out of bounds
int mapIdx = row*mapColSize + col; //map data stored as 1d array
if(mapIdx >= (mapRowSize*mapColSize) || mapIdx < 0) //index out of bounds
return 0; //0 for cannot travel
else if(col >= mapColSize || col < 0) //too many columns, will result in a location in the next row
return 0;
else if(map[mapIdx] == 'x') //wall reached, cannot travel
return 0;
else
return 1;
}
//parameters rowCurr and colCurr must be valid starting points
//returns the number of neighbors which are viable traveling points
//fills the node_t array called neighbors, with neighboring nodes to which the path can travel
//note: not using diagonal neighbors here, so there are only for possibilities for neighboring nodes
int getNeighbors(int rowCurr, int colCurr)
{
node_t nodeToAdd;
int numNeighbors = 0;
if(canTravel(rowCurr-1, colCurr) == 1) //can travel up
{
nodeToAdd.row = rowCurr-1;
nodeToAdd.col = colCurr;
neighbors[numNeighbors] = nodeToAdd;
numNeighbors++;
}
if(canTravel(rowCurr, colCurr-1) == 1) //can travel left
{
nodeToAdd.row = rowCurr;
nodeToAdd.col = colCurr-1;
neighbors[numNeighbors] = nodeToAdd;
numNeighbors++;
}
if(canTravel(rowCurr,colCurr+1) == 1) //can travel right
{
nodeToAdd.row = rowCurr;
nodeToAdd.col = colCurr+1;
neighbors[numNeighbors] = nodeToAdd;
numNeighbors++;
}
if(canTravel(rowCurr+1, colCurr) == 1) //can travel down
{
nodeToAdd.row = rowCurr+1;
nodeToAdd.col = colCurr;
neighbors[numNeighbors] = nodeToAdd;
numNeighbors++;
}
return numNeighbors;
}
//helper function called inside astar to return the path from the goal point back to the starting point
//the list of points is put into the array pathRow and pathCol in reverse order, pathLen is the number of inserted points
void reconstructPath(int rowEnd, int colEnd, dictElem_t nodeTrack[])
{
pathLen = 0; //global variable, reset so start inserting at beginning of path array
int currRow = rowEnd;
int currCol = colEnd;
while(currRow != 400 && currCol != 400)
{
//while node is not null "400", put it into path starting at end point
pathRow[pathLen] = currRow; //global array for Row
pathCol[pathLen] = currCol; //global array for Column
pathLen++;
// printf("currPath: (%d, %d), %d\n", pathRow[pathLen-1], pathCol[pathLen-1], pathLen);
int nodeTrackIdx = currRow*mapColSize+currCol;
currRow = nodeTrack[nodeTrackIdx].parentRow;
currCol = nodeTrack[nodeTrackIdx].parentCol;
// printf("next location: (%d, %d), %d\n", currRow, currCol, pathLen);
}
// printf("done with reconstruction\n");
}
//path planning algorithm
//parameters rowStart, colStart, rowEnd, and colEnd must be valid locations
//they must be both within the indices of the matrix size and must not be points where barriers ('x') exist
int astar(int rowStart, int colStart, int rowEnd, int colEnd)
{
//pseudo code instruction: initialize open and closed sets
//initialize a dictionary to keep track of parents of nodes for easy reconstruction and for keeping track of
// heap indexes for easy retrieval from heaps
//set the values of the dictionary, open and closed sets to be zero
// so that no old values sitting in memory will produce weird results
int resetNodeCnt;
for(resetNodeCnt=0; resetNodeCnt<176; resetNodeCnt++)
{
nodeTrack[resetNodeCnt].heapId = ' ';
nodeTrack[resetNodeCnt].heapIdx = 0;
nodeTrack[resetNodeCnt].parentRow = 0;
nodeTrack[resetNodeCnt].parentCol = 0;
}
startHeap(&openSet);
startHeap(&closedSet);
currDist = 0;
node_t start;
/* initialize a start node to be the starting position
since this is the start node, it should have zero distance traveled from the start, the normal predicted distance to the goal,
and the total distance is the sum of the distance traveled from the start and the distance to the goal.*/
/* update the dictionary, use row and column location as a key in the dictionary,
use 400 for null parent values and don't forget to indicate which heap, open or closed set, the node is being placed*/
/* put starting node on open set*/
start.row = rowStart;
start.col = colStart;
start.distTravelFromStart = 0;
start.distToGoal = heuristic(rowStart,colStart,rowEnd,colEnd);
start.totalDist = start.distTravelFromStart+start.distToGoal;
int startIdx = (start.row*mapColSize)+start.col;
(nodeTrack[startIdx]).heapId = 'o'; //o for open set
nodeTrack[startIdx].parentRow = 400; //use 400 as NULL, if 400, know reached beginning in reconstruction
nodeTrack[startIdx].parentCol = 400; //no parent value = 400 since out of bounds
if(rowStart == rowEnd && colStart == colEnd) { //if start point is the end point, don't do anything more!!!
printf("!!!!!!!! Exit on rowStart == rowEnd, Returning 2 !!!!!!\n");
return 2;
}
push_(start, &openSet, nodeTrack, mapColSize); // put start node on the openSet
char goalFound = 'f'; //false
/*while open set not empty and goal not yet found*/
while(openSet.numElems > 0 && goalFound != 't')
{
/*find the node with the least total distance (f_score) on the open list, call it q (called minDistNode in code)*/
node_t minDistNode = peek_(&openSet); //find node with least total cost, make that the current node
//printf("Current Node, Row=%d,Col=%d,g=%d,h=%d,f=%d\n",minDistNode.row,minDistNode.col,minDistNode.distTravelFromStart,minDistNode.distToGoal,minDistNode.totalDist);
/*set the current distance to the current node's distance traveled from the start.*/
//1. currDist is set to q's distance traveled from the Start. Explain why this could be different from the Manhattan distance to the Start position
// This question is just asking for comments.
currDist = minDistNode.distTravelFromStart;
(nodeTrack[(minDistNode.row*mapColSize)+minDistNode.col]).heapId = 'r'; //r for removed from any set
//2. pop q (which is currently the minimum) off which queue?
// Choose one of these two lines of code
// IF the Openset
pop_(&openSet, nodeTrack, mapColSize);
// IF closedSet
//pop(&closedSet, nodeTrack, mapColSize);
/*generate q's 4 neighbors*/
// 3. Pass q's row and col to getNeighbors
int numNeighbors = getNeighbors(minDistNode.row, minDistNode.col); //get list of neighbors
/*for each neighbor*/
int cnt = 0;
for(cnt = 0; cnt<numNeighbors; cnt++) //for each found neighbor
{
// 4. Just add comments here. Find where the structure node_t is defined and inside commments here copy its definition for
// viewing reference.
// All the answer for 4. will be commented.
node_t next = neighbors[cnt];
/*if neighbor is the goal, stop the search*/
/*update the dictionary so this last node's parents are set to the current node*/
if((next.row == rowEnd) && (next.col == colEnd)) //if neighbor is the goal, found the end, so stop the search
{
// 5. set current neighbor's parents. Set parentRow to q's row. Set parentCol to q's col since q is the parent of this neighbor
(nodeTrack[next.row*mapColSize+next.col]).parentRow = minDistNode.row; //set goal node's parent position to current position
(nodeTrack[next.row*mapColSize+next.col]).parentCol = minDistNode.col;
goalFound = 't';
break;
}
/*neighbor.distTravelFromStart (g) = q.distTravelFromStart + distance between neighbor and q which is always 1 when search just top left bottom right*/
// 6. Set this neighbor's distance traveled from the start. Remember you have the variable "currDist" that is the distance of q to Start
//need to consider diagonal distance now
next.distTravelFromStart = currDist + 1;
// printf("next.distTravelFromStart %f\n", next.distTravelFromStart);
/*neighbor.distToGoal (h) = distance from goal to neighbor, heuristic function (estimated distance to goal)*/
// 7. Pass the correct parameters to "heuristic" to calculate the distance this neighbor is from the goal.
// Remember that we have the variables rowEnd and colEnd which are the grid coordinates of the goal
next.distToGoal = heuristic(next.row, next.col, rowEnd, colEnd);
/*neighbor.totalDist (f) = neighbor.distTravelFromStart + neighbor.distToGoal
(total estimated distance as sum of distance traveled from start and distance to goal)*/
// 8. Find f, (totalDist) for this neighbor
next.totalDist = next.distTravelFromStart + next.distToGoal;
// 9. Just comments for this question.
// Explain in your own words (not copying the comments below) what the next 19 lines of C code are doing
/*if a node with the same position as neighbor is in the OPEN list
which has a lower total distance than neighbor, skip putting this neighbor onto the open set*/
//check if node is on the open set already
int nodeTrackIdx = (next.row*mapColSize)+next.col;
char skipPush = 'f';
if(nodeTrack[nodeTrackIdx].heapId == 'o') //on the open set
{
int heapIndex = nodeTrack[nodeTrackIdx].heapIdx;
node_t fromOpen = openSet.elems[heapIndex];
if(fromOpen.totalDist <= next.totalDist)
skipPush = 't'; //skip putting this node onto the openset, a better one already on there
}
/*if a node with the same position as neighbor is in the CLOSED list
which has a lower f than neighbor, skip putting this neighbor onto the open set*/
else if(nodeTrack[nodeTrackIdx].heapId == 'c') //on closed set
{
int heapIndex = nodeTrack[nodeTrackIdx].heapIdx;
node_t fromClosed = closedSet.elems[heapIndex];
if(fromClosed.totalDist <= next.totalDist)
skipPush = 't'; //skip putting this node onto the openset, already part of possible solution
}
/*if not skipping putting this node on the set, then push node onto the open set
and update the dictionary to indicate the node is on the open set and set the parents of this node to the current node*/
//(can't be an ordinary else to the things above because then it won't push onto the open set if already on open or closed set)
if(skipPush != 't')
{
nodeTrack[nodeTrackIdx].heapId = 'o'; //mark in nodetrack that this is going onto the open set
(nodeTrack[nodeTrackIdx]).parentRow = minDistNode.row; //set neighbor's parent position to current position
(nodeTrack[nodeTrackIdx]).parentCol = minDistNode.col;
//10. push this neighbor on which queue?
// Choose one of these two lines of code
// IF openSet
push_(next, &openSet, nodeTrack, mapColSize);
//printf("Push to OpenList, Row=%d,Col=%d,g=%d,h=%d,f=%d\n",next.row,next.col,next.distTravelFromStart,next.distToGoal,next.totalDist);
// IF closedSet
//push(next, &closedSet, nodeTrack, mapColSize);
}
}/* end for loop*/
int nodeTrackIdx = minDistNode.row*mapColSize+minDistNode.col;
nodeTrack[nodeTrackIdx].heapId = 'c';
//11. push q (current node) on which queue?
// Choose one of these two lines of code
// IF openSet
//push(minDistNode, &openSet, nodeTrack, mapColSize);
// IF closedSet
push_(minDistNode, &closedSet, nodeTrack, mapColSize);
//printf("Push to ClosedList, Row=%d,Col=%d\n",minDistNode.row,minDistNode.col);
} /*end while loop*/
/*if a path was found from the start to the goal, then reconstruct the path*/
if(goalFound == 't') {
// 12. Pass the correct varaibles to "reconstructPath" in order for it to fill in the global arrays pathRow, pathCol
// and integer pathLen. Note that the path is in reverse order in pathRow and pathCol.
reconstructPath(rowEnd, colEnd, nodeTrack);
return 1;
}
return 0;
}
int main() {
struct shared_memory_sendto_AstarApp *send_shared_mem_ptr;
struct shared_memory_readfrom_AstarApp *read_shared_mem_ptr;
sem_t *send_mutex_sem;
sem_t *read_mutex_sem;
int send_fd_shm;
int read_fd_shm;
int i,j = 0;//for loop index
int entered_time;//dummy variable for path
int temp_obs_row = 0;
int temp_obs_col = 0;
int robot_row = 0;
int robot_col = 0;
int re_astar = 0;
//create the semaphore for send
if ((send_mutex_sem = sem_open(SENDTO_ASTARAPP_SEM_MUTEX_NAME, 0, 0, 0)) == SEM_FAILED)
error("send sem_open");
//create the semaphore for read path
if ((read_mutex_sem = sem_open(READFROM_ASTARAPP_SEM_MUTEX_NAME, 0, 0, 0)) == SEM_FAILED)
error("read sem_open");
// create shared memory for send
if ((send_fd_shm = shm_open(SENDTO_ASTARAPP_SHARED_MEM_NAME, O_RDWR, 0)) == -1)
error("shm_open");
//map the memory to virtual address
if ((send_shared_mem_ptr = mmap(NULL, sizeof(struct shared_memory_sendto_AstarApp), PROT_READ | PROT_WRITE, MAP_SHARED,
send_fd_shm, 0)) == MAP_FAILED)
error("mmap");
// create shared memory for read
if ((read_fd_shm = shm_open(READFROM_ASTARAPP_SHARED_MEM_NAME, O_RDWR, 0)) == -1)
error("shm_open");
//map the memory to virtual address
if ((read_shared_mem_ptr = mmap(NULL, sizeof(struct shared_memory_readfrom_AstarApp), PROT_READ | PROT_WRITE, MAP_SHARED,
read_fd_shm, 0)) == MAP_FAILED)
error("mmap");
while (!gs_quit) {
int kb_state = _kbhit(); //check if hit keyboard
if (kb_state == 1)
{
//read the entered character
mychar = mygetch();
if (mychar == 'q')
{ //exit if hit 'q'
gs_quit = 1;
}
}
//check if the semaphore has been posted, got new data
if (sem_trywait(send_mutex_sem) == 0) {
for (i = 0; i < 34; i++){
pose_obs_data.obs_data_char[i] = send_shared_mem_ptr->new_pose.obs_data_char[i]; //get the pose from shm
}
for(i = 0; i<176; i++) { // Intialize the Map to all '0' 176 = mapRowSize*mapColSize
map[i] = '0';
}
int current_char = 0;
for(i = 0; i<176; i++) {
if ((pose_obs_data.cur_obs.mapCondensed[current_char]&0x1)==0x1){
map[i] = 'x';
}
i++;
if ((pose_obs_data.cur_obs.mapCondensed[current_char]&0x2)==0x2){
map[i] = 'x';
}
i++;
if ((pose_obs_data.cur_obs.mapCondensed[current_char]&0x4)==0x4){
map[i] = 'x';
}
i++;
if ((pose_obs_data.cur_obs.mapCondensed[current_char]&0x8)==0x8){
map[i] = 'x';
}
i++;
if ((pose_obs_data.cur_obs.mapCondensed[current_char]&0x10)==0x10){
map[i] = 'x';
}
i++;
if ((pose_obs_data.cur_obs.mapCondensed[current_char]&0x20)==0x20){
map[i] = 'x';
}
i++;
if ((pose_obs_data.cur_obs.mapCondensed[current_char]&0x40)==0x40){
map[i] = 'x';
}
i++;
if ((pose_obs_data.cur_obs.mapCondensed[current_char]&0x80)==0x80){
map[i] = 'x';
}
current_char++;
}
//update the destination of the robot
endRow = pose_obs_data.cur_obs.destrow;
endCol = pose_obs_data.cur_obs.destcol;
robot_col = round(pose_obs_data.cur_obs.x) +5; //get the robot pose with round as the starting point
robot_row = 11- round(pose_obs_data.cur_obs.y);
returnstatus = 0;
if (endRow == robot_row && endCol == robot_col) {
re_astar = 0;
returnstatus |= 0x2; //set status that end == start
printf("A* Start Point Equals A* Stop Point: No A* Run\n");
} else {
re_astar = 1;
}
printf("pose %.3f, %.3f", pose_obs_data.cur_obs.x, pose_obs_data.cur_obs.y);
printf("\n");
//##################### put path planning algorithm here ############################
if (re_astar == 1) {
re_astar = 0; //set back to 0
//print map, this is the map with obstacles as x
for(i=0; i<mapRowSize; i++){
for(j = 0; j<mapColSize; j++) {
printf("%c ", map[i*mapColSize+j]);
}
printf("\n");
}
printf("\n");
printf("doing A*");
if(robot_row>=0 && robot_row<mapRowSize && robot_col>=0 && robot_col<mapColSize && endRow>=0 && endRow<mapRowSize && endCol>=0 && endCol<mapColSize) //if in bounds
{
printf("here");
if(map[robot_row*mapColSize+robot_col]!='x' && map[endRow*mapColSize+endCol]!='x') //make sure valid start and end points
{
retvalue = astar(robot_row,robot_col,endRow,endCol);
printf("A* starts at %d %d ends at %d %d \n", robot_row, robot_col, endRow, endCol); //print starting point of A*
if (retvalue == 1) { //if found a path
/* Fill in the points array for the robot to visit in reverse order of A* result
* x = col - 5
* y = 11 - row
*/
int numpts;//number of points that needed to fill in
numpts = MIN(pathLen,40); //find the minimum of the path length and 40(the max path length possible)
//since the points array is reused every A*, need to reset every time
for (i = 0; i < 40; i++) {
// points[i].x = 0;
// points[i].y = 0;
planned_path[2*i] = 199; //clear the sending to f28 array
planned_path[2*i+1] = 199;
}
for (i = 0; i < numpts; i++){
planned_path[i*2] = pathRow[i];
planned_path[i*2+1] = pathCol[i];
}
} else {
returnstatus |= 0x1; //Tell F28379D to reset the map
printf("No Path Found, Which is impossible so Resetting Map to Default\n");
planned_path[0] = 99;
}
} else {
returnstatus |= 0x9; // start or stop are obstacles and reset map
printf("A* Start or Stop are obstacles\n");
planned_path[0] = 99;
}
} else {
returnstatus |= 0x4; // start or stop outside of map
printf("A* Start or Stop are outside of map coordinates\n");
planned_path[0] = 99;
}
/*
print the solution
*/
for (i = 0; i < 176; i++){
map_sol[i]= map[i];
}
//put the solution on map only when there's no error
if (planned_path[0] != 99) {
for(i = 0; i< pathLen; i++) { //put solution on map
int mapIdx = pathRow[i]*mapColSize + pathCol[i];
// printf("pathRow is %d, pathCol is %d\n", pathRow[i], pathCol[i]);
planned_path[i*2] = pathRow[i];
planned_path[i*2+1] = pathCol[i];
map_sol[mapIdx] = '-';
}
}
//print map with solution
for(i=0; i<mapRowSize; i++) {
for(j = 0; j<mapColSize; j++) {
printf("%c ", map_sol[i*mapColSize+j]);
}
printf("\n");
}
printf("\n");
//###################################################################################
for (i = 0; i < 80; i++) {
read_shared_mem_ptr->read_path[i] = 199; //clear the path(set to 199 as no path code)
}
for (i = 0; i < pathLen*2; i++) {
read_shared_mem_ptr->read_path[i] = planned_path[i]; // fill in the path
// printf("send is %d\n",read_shared_mem_ptr->read_path[i]);
}
read_shared_mem_ptr->read_path[80] = returnstatus;
//check if the previous data has been processed
if (sem_getvalue(read_mutex_sem, &sem_count_read) == 0) {
if (sem_post(read_mutex_sem) == -1){
error("sem_post: read mutex");
}
}
} else {
for (i = 0; i < 80; i++) {
read_shared_mem_ptr->read_path[i] = 199; //clear the path(set to 199 as no path code)
}
read_shared_mem_ptr->read_path[80] = returnstatus;
//check if the previous data has been processed
if (sem_getvalue(read_mutex_sem, &sem_count_read) == 0) {
if (sem_post(read_mutex_sem) == -1){
error("sem_post: read mutex");
}
}
}
}
}
}
Thanks to Dan Block.
Comments