As a semester project of SE423 at UIUC, a contest was conducted. The goal of this contest is to program the course robot to navigate to five different points in their numbered order. There are three main objectives to be done.
1. Avoid obstacles which is randomly placed 2x2(in tiles) boxes
2. Detect 5 randomly placed weeds (pink and blue papers). 2 weeds had to be found to be considered as a successful run and additional weeds found, 40 seconds were subtracted assuming the weed location and its color is reported back to LABVIEW GUI we created.
3. Drive on top of the weed within.5 tiles radius of the actual position and report its position to LABVIEW. However driving to the weed that was located before was avoided (refer to code attached)
To accomplish objective 1, A* algorithm and LIDAR reading were combined to map the location of obstacles and produce optimal paths. To drive and locate the robot position wheel encoders and rate gyro were used which caused some drift. To correct the drift of dead reckoned position motion tracking camera system (Opti Track) was used. Dead reckoned position and position data from opti track was combined using the Kalman filter.
To accomplish objective 2, camera vision was used by setting number of pixels as threshold to detect which defined how far the robot wanted to see the weeds throughout the course. Our threshold was set at relatively low detecting weeds about 3 tiles away from robot's position. Center of the pixels was also calculated to locate the position of the weed.
To accomplish objective 3, Once the location of the weed was found, distance was calculated using robot's current position.
/*
* ======== main.c ========
*/
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <c6x.h> // register defines
#include <xdc/std.h>
#include <ti/sysbios/family/c64p/Cache.h>
#include <ti/sysbios/knl/Task.h>
#include <ti/sysbios/knl/Swi.h>
#include <xdc/runtime/Error.h>
#include <xdc/runtime/System.h>
#include <ti/sysbios/BIOS.h>
#include <ti/sysbios/knl/Clock.h>
#include "evmomapl138.h"
#include "evmomapl138_i2c.h"
#include "evmomapl138_timer.h"
#include "evmomapl138_led.h"
#include "evmomapl138_dip.h"
#include "evmomapl138_gpio.h"
#include "evmomapl138_vpif.h"
#include "evmomapl138_spi.h"
#include "COECSL_edma3.h"
#include "COECSL_mcbsp.h"
#include "COECSL_registers.h"
#include "mcbsp_com.h"
#include "ColorVision.h"
#include "ColorLCD.h"
#include "sharedmem.h"
#include "LCDprintf.h"
#include "ladar.h"
#include "xy.h"
#include "MatrixMath.h"
#include "projectinclude.h"
#define FEETINONEMETER 3.28083989501312
extern EDMA3_CCRL_Regs *EDMA3_0_Regs;
volatile uint32_t index;
// test variables
extern float enc1; // Left motor encoder
extern float enc2; // Right motor encoder
extern float enc3;
extern float enc4;
extern float adcA0; // ADC A0 - Gyro_X -400deg/s to 400deg/s Pitch
extern float adcB0; // ADC B0 - External ADC Ch4 (no protection circuit)
extern float adcA1; // ADC A1 - Gyro_4X -100deg/s to 100deg/s Pitch
extern float adcB1; // ADC B1 - External ADC Ch1
extern float adcA2; // ADC A2 - Gyro_4Z -100deg/s to 100deg/s Yaw
extern float adcB2; // ADC B2 - External ADC Ch2
extern float adcA3; // ADC A3 - Gyro_Z -400deg/s to 400 deg/s Yaw
extern float adcB3; // ADC B3 - External ADC Ch3
extern float adcA4; // ADC A4 - Analog IR1
extern float adcB4; // ADC B4 - USONIC1
extern float adcA5; // ADC A5 - Analog IR2
extern float adcB5; // ADC B5 - USONIC2
extern float adcA6; // ADC A6 - Analog IR3
extern float adcA7; // ADC A7 - Analog IR4
extern float compass;
extern float switchstate;
extern sharedmemstruct *ptrshrdmem;
float vref = 0;
float turn = 0;
int tskcount = 0;
char fromLinuxstring[LINUX_COMSIZE + 2];
char toLinuxstring[LINUX_COMSIZE + 2];
float LVvalue1 = 0;
float LVvalue2 = 0;
int new_LV_data = 0;
int newnavdata = 0;
float newvref = 0;
float newturn = 0;
extern sharedmemstruct *ptrshrdmem;
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 LADAR)
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
// deadreckoning
float vel1 = 0,vel2 = 0;
float vel1old = 0,vel2old = 0;
float enc1old = 0,enc2old = 0;
// SETTLETIME should be an even number and divisible by 3
#define SETTLETIME 6000
int settlegyro = 0;
float gyro_zero = 0;
float gyro_angle = 0;
float old_gyro = 0;
float gyro_drift = 0;
float gyro = 0;
int gyro_degrees = 0;
float gyro_radians = 0.0;
float gyro_x = 0,gyro_y = 0;
float gyro4x_gain = 1;
int statePos = 0; // index into robotdest
int robotdestSize = 11; // number of positions to use out of robotdest
pose robotdest[11]; // array of waypoints for the robot
extern float newLADARdistance[LADAR_MAX_DATA_SIZE]; //in mm
extern float newLADARangle[LADAR_MAX_DATA_SIZE]; // in degrees
float LADARdistance[LADAR_MAX_DATA_SIZE];
float LADARangle[LADAR_MAX_DATA_SIZE];
extern pose ROBOTps;
extern pose LADARps;
extern float newLADARdataX[LADAR_MAX_DATA_SIZE];
extern float newLADARdataY[LADAR_MAX_DATA_SIZE];
float LADARdataX[LADAR_MAX_DATA_SIZE];
float LADARdataY[LADAR_MAX_DATA_SIZE];
extern int newLADARdata;
// Optitrack Variables
int trackableIDerror = 0;
int firstdata = 1;
volatile int new_optitrack = 0;
volatile float previous_frame = -1;
int frame_error = 0;
volatile float Optitrackdata[OPTITRACKDATASIZE];
pose OPTITRACKps;
float temp_theta = 0.0;
float tempOPTITRACK_theta = 0.0;
volatile int temp_trackableID = -1;
int trackableID = -1;
int errorcheck = 1;
int robot_state = 0;
// Color Variables Declaration
extern float blue_object_x;
extern float blue_object_y;
extern int blue_numpels;
extern float pink_object_x;
extern float pink_object_y;
extern int pink_numpels;
extern int new_coordata;
float blue_visionx = 0;
float blue_visiony = 0;
int blue_visionpix = 0;
float pink_visionx = 0;
float pink_visiony = 0;
int pink_visionpix = 0;
float Kplight = 0.05;
float dist_object = 0;
unsigned long int colortimer = 0;
float blue_weed_x[3];
float blue_weed_y[3];
float pink_weed_x[3];
float pink_weed_y[3];
float temp_weed_x;
float temp_weed_y;
int numofblue = 0;
int numofpink = 0;
int numofweed = 0;
int weed_idx = 0;
unsigned long int move_time = 0;
unsigned long int wait_timer = 0;
int blue_ignore_flag = 0;
unsigned long int blue_ignore_timer = 0;
int pink_ignore_flag = 0;
unsigned long int pink_ignore_timer = 0;
int blue_weed_found_flag = 0;
int pink_weed_found_flag = 0;
float weed_x = 0;
float weed_y = 0;
unsigned long int LVcounter=0;
unsigned long int report_timer = 0;
// Astar Varaibles Declaration
int first_time = 0;
int astarRunning = 0;
int astarTrigger = 0; // Just a Debug variable value prints in Linux each A* run.
int astarDone = 0;
int pathLen = 0; //used to keep track of number of points in reconstructed path
int pathPos=0;
int pathRow[100]; //reconstructed path in reverse order
int pathCol[100];
int mapflag = 0;
float obstacle_threshold = 0.3;
struct obs{
int x; // x position
int y; // y position
int r; // row
int c; // column
int idx1;
int idx2;
int idx3;
int tally;
int flag;
int LVflag;
} obs[57];
int obsLVflag = 0;
float obsx1 = 0;
float obsy1 = 0;
float obsx2 = 0;
float obsy2 = 0;
float pwm3 = 5.0;
float pwm4 = 4.4;
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 originalMap[176] = //16 11
{ '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
'0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
'0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
'0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
'0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
'0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
'0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
'0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
'0', '0', '0', '0', '0', '0', '0', '0', '0', '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 myRound(float var) {
if ( ( fabs(var) - floorf( fabs(var) ) ) > 0.5) {
if (var > 0.0) {return ceil(var);}
else {return floor(var);}
}
else {
if (var > 0.0) {return floor(var);}
else {return ceil(var);}
}
}
// This is telling Astar to RUN!
void runAstar(void) {
float robotdestx = 0;
float robotdesty = 0;
float robotx = 0;
float roboty = 0;
int local_statePos = 0;
while(1) {
int i = 0;
Semaphore_pend(SEM_startAstar,BIOS_WAIT_FOREVER);
if(!GET_ASTAR_COMMAND && !astarRunning) {
astarRunning = 1;
astarTrigger = 0; // Debug print in Linux
robotx = ROBOTps.x;
roboty = ROBOTps.y;
local_statePos = statePos;
robotdestx = robotdest[local_statePos].x;
robotdesty = robotdest[local_statePos].y;
//For now, Update the shared variables that need to be sent for astar
for (i=0;i<176;i++) {
ptrshrdmem->sharedAstarMap[i] = map[i];
}
ptrshrdmem->astarTrigger = astarTrigger;
ptrshrdmem->sharedDestRow = (11.0 - robotdesty);
ptrshrdmem->sharedDestCol = (5.0 + robotdestx);
ptrshrdmem->sharedRobotRow = (11.0 - myRound(roboty));
ptrshrdmem->sharedRobotCol = (myRound(robotx) + 5.0);
Cache_wb((void *)ptrshrdmem,sizeof(sharedmemstruct), Cache_Type_ALL, EDMA3_CACHE_WAIT);
//BCACHE_wb((void *)ptrshrdmem,sizeof(sharedmemstruct),EDMA3_CACHE_WAIT);
SET_ASTAR_COMMAND;
}
}
}
// Periodic
void LinuxAstar(void) {
float robotdestx = 0;
float robotdesty = 0;
float robotx = 0;
float roboty = 0;
int local_statePos = 0;
int i = 0;
Task_sleep(10);
while(1) {
Task_sleep(5);
//GET_STAR_DONE is set when linux finishes running the a star algo irrespective of whether its succesful or failed
if (GET_ASTAR_DONE) {
//In case it failed-> couldn't find path, map is reset to original, only walls everything else cleared
if (GET_ASTAR_FAILED) {
CLR_ASTAR_DONE;
robotx = ROBOTps.x;
roboty = ROBOTps.y;
local_statePos = statePos;
robotdestx = robotdest[local_statePos].x;
robotdesty = robotdest[local_statePos].y;
astarRunning = 1;
astarTrigger = 5; // Debug print in Linux
// If we get here, there was no path found between the robot and one of the waypoints.
// The map is reset
for (i=0;i<176;i++) {
map[i] = originalMap[i];
ptrshrdmem->sharedAstarMap[i] = map[i];
}
//Reset Object flag
for (i=0;i<57;i++) {
obs[i].flag = 0;
obs[i].tally = 0;
obs[i].LVflag = 0;
}
// ALSO make sure to clear your obstacle tally an other obstacle recognition variables you have created.
ptrshrdmem->astarTrigger = astarTrigger;
ptrshrdmem->sharedDestRow = (11.0 - robotdesty);
ptrshrdmem->sharedDestCol = (5.0 + robotdestx);
ptrshrdmem->sharedRobotRow = (11.0 - myRound(roboty));
ptrshrdmem->sharedRobotCol = (myRound(robotx) + 5.0);
Cache_wb((void *)ptrshrdmem,sizeof(sharedmemstruct), Cache_Type_ALL, EDMA3_CACHE_WAIT);
//BCACHE_wb((void *)ptrshrdmem,sizeof(sharedmemstruct),EDMA3_CACHE_WAIT);
SET_ASTAR_COMMAND; //set every time we want to go into astar as opposed to done which is cleared only when we our astar is succesful
}
else { // Astar successful
Cache_inv((void *)ptrshrdmem,sizeof(sharedmemstruct), Cache_Type_ALL, EDMA3_CACHE_WAIT);
//BCACHE_inv((void *)ptrshrdmem,sizeof(sharedmemstruct),EDMA3_CACHE_WAIT);
pathLen = ptrshrdmem->sharedPathLen;
for (i=0;i<pathLen;i++) {
pathCol[pathLen - i - 1] = (ptrshrdmem->sharedPathCol[i] - 5.0);
pathRow[pathLen - i - 1] = (11.0 - ptrshrdmem->sharedPathRow[i]); // Inverting the order of array. And then converting from Astar to Robot coordinates
}
CLR_ASTAR_DONE;
if (pathLen > 1) {
pathPos = 1;
} else {
pathPos = 0;
}
astarRunning = 0; // basic functionality same as CLR_ASTAR_DONE but on DSP side this is more reliable
astarDone = 1;
//.. given CLR_ASTAR_DONE can also be set by linux
}
}
}
}
/*************************************************************************
Everything here forward was imported from the previous labs/exercieses prior to the final project
***************************************************************************/
pose UpdateOptitrackStates(pose localROBOTps, int * flag);
void ComWithLinux(void) {
int i = 0;
Task_sleep(100);
while(1) {
Cache_inv((void *)ptrshrdmem,sizeof(sharedmemstruct), Cache_Type_ALL, EDMA3_CACHE_WAIT);
//BCACHE_inv((void *)ptrshrdmem,sizeof(sharedmemstruct),EDMA3_CACHE_WAIT);
if (GET_DATA_FROM_LINUX) {
if (newnavdata == 0) {
newvref = ptrshrdmem->Floats_to_DSP[0];
newturn = ptrshrdmem->Floats_to_DSP[1];
newnavdata = 1;
}
CLR_DATA_FROM_LINUX;
}
if (GET_LVDATA_FROM_LINUX) {
if (ptrshrdmem->DSPRec_size > 256) ptrshrdmem->DSPRec_size = 256;
for (i=0;i<ptrshrdmem->DSPRec_size;i++) {
fromLinuxstring[i] = ptrshrdmem->DSPRec_buf[i];
}
fromLinuxstring[i] = '\0';
if (new_LV_data == 0) {
sscanf(fromLinuxstring,"%f%f",&LVvalue1,&LVvalue2);
new_LV_data = 1;
}
CLR_LVDATA_FROM_LINUX;
}
if ((tskcount%6)==0) {
if (GET_LVDATA_TO_LINUX) {
// Default
// ptrshrdmem->DSPSend_size = sprintf(toLinuxstring,"1.0 1.0 1.0 1.0");
// you would do something like this
weed_idx = numofblue + numofpink;
if( blue_weed_found_flag == 1 ){
weed_x = blue_weed_x[numofblue-1]+7;
weed_y = -blue_weed_y[numofblue-1]+13;
ptrshrdmem->DSPSend_size = sprintf(toLinuxstring,"%.1f %.1f %d %d %.1f %.1f %d %d %d %.1f %.1f %.1f %.1f",ROBOTps.x + 7,-ROBOTps.y+13, numofblue,numofpink, blue_weed_x[numofblue-1]+7 , -blue_weed_y[numofblue-1]+13 , 1 ,weed_idx, obsLVflag,obsx1+7,-obsy1+13,obsx2+7,-obsy2+13 );
blue_weed_found_flag = 0;
}else if(pink_weed_found_flag == 1){
weed_x = pink_weed_x[numofblue-1]+7;
weed_y = -pink_weed_y[numofblue-1]+13;
ptrshrdmem->DSPSend_size = sprintf(toLinuxstring,"%.1f %.1f %d %d %.1f %.1f %d %d %d %.1f %.1f %.1f %.1f",ROBOTps.x + 7,-ROBOTps.y+13, numofblue,numofpink, pink_weed_x[numofpink-1]+7 , -pink_weed_y[numofpink-1]+13 , 2 ,weed_idx,obsLVflag,obsx1+7,-obsy1+13,obsx2+7,-obsy2+13 );
pink_weed_found_flag = 0;
}
else{ // no weed found send weed_idx = 0 default case
weed_idx = 6;
ptrshrdmem->DSPSend_size = sprintf(toLinuxstring,"%.1f %.1f %d %d %.1f %.1f %d %d %d %.1f %.1f %.1f %.1f",ROBOTps.x + 7,-ROBOTps.y+13, numofblue,numofpink,weed_x,weed_y,0,weed_idx,obsLVflag,obsx1+7,-obsy1+13,obsx2+7,-obsy2+13 );
}
for (i=0;i<ptrshrdmem->DSPSend_size;i++) {
ptrshrdmem->DSPSend_buf[i] = toLinuxstring[i];
}
// if( blue_weed_found_flag == 1 ){
// ptrshrdmem->DSPSend_size = sprintf(toLinuxstring,"%.1f %.1f %d %d %.1f %.1f %d %d",ROBOTps.x + 7,-ROBOTps.y+13, numofblue,numofpink, blue_weed_x[numofblue-1]+7 , -blue_weed_y[numofblue-1]+13 , 1 ,weed_idx);
// blue_weed_found_flag = 0;
// }else if(pink_weed_found_flag == 1){
// ptrshrdmem->DSPSend_size = sprintf(toLinuxstring,"%.1f %.1f %d %d %.1f %.1f %d %d",ROBOTps.x + 7,-ROBOTps.y+13, numofblue,numofpink, pink_weed_x[numofpink-1]+7 , -pink_weed_y[numofpink-1]+13 , 2 ,weed_idx);
// pink_weed_found_flag = 0;
// }
// else{
// weed_idx = 0;
// ptrshrdmem->DSPSend_size = sprintf(toLinuxstring,"%.1f %.1f %d %d %.1f %.1f %d %d",ROBOTps.x + 7,-ROBOTps.y+13, numofblue,numofpink,0,0,0,weed_idx);
// }
//
// for (i=0;i<ptrshrdmem->DSPSend_size;i++) {
// ptrshrdmem->DSPSend_buf[i] = toLinuxstring[i];
// }
// Flush or write back source
Cache_wb((void *)ptrshrdmem,sizeof(sharedmemstruct), Cache_Type_ALL, EDMA3_CACHE_WAIT);
//BCACHE_wb((void *)ptrshrdmem,sizeof(sharedmemstruct),EDMA3_CACHE_WAIT);
CLR_LVDATA_TO_LINUX;
}
}
if (GET_DATAFORFILE_TO_LINUX) {
// This is an example write to scratch
// The Linux program SaveScratchToFile can be used to write the
// ptrshrdmem->scratch[0-499] memory to a .txt file.
// for (i=100;i<300;i++) {
// ptrshrdmem->scratch[i] = (float)i;
// }
// Flush or write back source
Cache_wb((void *)ptrshrdmem,sizeof(sharedmemstruct), Cache_Type_ALL, EDMA3_CACHE_WAIT);
//BCACHE_wb((void *)ptrshrdmem,sizeof(sharedmemstruct),EDMA3_CACHE_WAIT);
CLR_DATAFORFILE_TO_LINUX;
}
tskcount++;
Task_sleep(40);
}
}
/*
* ======== main ========
*/
//
Int main()
{
int i = 0;
for(i=0;i<30;i++){ //horizontal wall
obs[i].x = -5+2*(i%6);
obs[i].y = 10-2*(i/6);
obs[i].r = 1+2*(i/6);
obs[i].c = 2*(i%6);
obs[i].idx1 = (obs[i].r)*11 + obs[i].c-1;
obs[i].idx2 = (obs[i].r)*11 + obs[i].c;
obs[i].idx3 = (obs[i].r)*11 + obs[i].c+1;
obs[i].tally = 0;
obs[i].flag = 0;
obs[i].LVflag = 0;
}
for(i=30;i<55;i++){ // vertical wall
obs[i].x = -4 + 2*((i-30)%5);
obs[i].y = 9 - 2*((i-30)/5);
obs[i].r = 2 + 2*((i-30)/5);
obs[i].c = 1 + 2*((i-30)%5);
obs[i].idx1 = (obs[i].r-1)*11 + obs[i].c;
obs[i].idx2 = (obs[i].r)*11 + obs[i].c;
obs[i].idx3 = (obs[i].r+1)*11 + obs[i].c;
obs[i].tally = 0;
obs[i].flag = 0;
obs[i].LVflag = 0;
}
//
obs[55].x = -1;
obs[55].y = 0;
obs[55].r = 11;
obs[55].c = 4;
obs[55].idx1 = (obs[55].r)*11 + obs[55].c-1;
obs[55].idx2 = (obs[55].r)*11 + obs[55].c;
obs[55].idx3 = (obs[55].r)*11 + obs[55].c+1;
obs[55].tally = 0;
obs[55].flag = 0;
obs[55].LVflag = 0;
//
obs[56].x = 1;
obs[56].y = 0;
obs[56].r = 11;
obs[56].c = 6;
obs[56].idx1 = (obs[56].r)*11 + obs[56].c-1;
obs[56].idx2 = (obs[56].r)*11 + obs[56].c;
obs[56].idx3 = (obs[56].r)*11 + obs[56].c+1;
obs[56].tally = 0;
obs[56].flag = 0;
obs[56].LVflag = 0;
//
// unlock the system config registers.
SYSCONFIG->KICKR[0] = KICK0R_UNLOCK;
SYSCONFIG->KICKR[1] = KICK1R_UNLOCK;
SYSCONFIG1->PUPD_SEL |= 0x10000000; // change pin group 28 to pullup for GP7[12/13] (LCD switches)
// Initially set McBSP1 pins as GPIO ins
CLRBIT(SYSCONFIG->PINMUX[1], 0xFFFFFFFF);
SETBIT(SYSCONFIG->PINMUX[1], 0x88888880); // This is enabling the McBSP1 pins
CLRBIT(SYSCONFIG->PINMUX[16], 0xFFFF0000);
SETBIT(SYSCONFIG->PINMUX[16], 0x88880000); // setup GP7.8 through GP7.13
CLRBIT(SYSCONFIG->PINMUX[17], 0x000000FF);
SETBIT(SYSCONFIG->PINMUX[17], 0x00000088); // setup GP7.8 through GP7.13
//Rick added for LCD DMA flagging test
GPIO_setDir(GPIO_BANK0, GPIO_PIN8, GPIO_OUTPUT);
GPIO_setOutput(GPIO_BANK0, GPIO_PIN8, OUTPUT_HIGH);
GPIO_setDir(GPIO_BANK0, GPIO_PIN0, GPIO_INPUT);
GPIO_setDir(GPIO_BANK0, GPIO_PIN1, GPIO_INPUT);
GPIO_setDir(GPIO_BANK0, GPIO_PIN2, GPIO_INPUT);
GPIO_setDir(GPIO_BANK0, GPIO_PIN3, GPIO_INPUT);
GPIO_setDir(GPIO_BANK0, GPIO_PIN4, GPIO_INPUT);
GPIO_setDir(GPIO_BANK0, GPIO_PIN5, GPIO_INPUT);
GPIO_setDir(GPIO_BANK0, GPIO_PIN6, GPIO_INPUT);
GPIO_setDir(GPIO_BANK7, GPIO_PIN8, GPIO_OUTPUT);
GPIO_setDir(GPIO_BANK7, GPIO_PIN9, GPIO_OUTPUT);
GPIO_setDir(GPIO_BANK7, GPIO_PIN10, GPIO_OUTPUT);
GPIO_setDir(GPIO_BANK7, GPIO_PIN11, GPIO_OUTPUT);
GPIO_setDir(GPIO_BANK7, GPIO_PIN12, GPIO_INPUT);
GPIO_setDir(GPIO_BANK7, GPIO_PIN13, GPIO_INPUT);
GPIO_setOutput(GPIO_BANK7, GPIO_PIN8, OUTPUT_HIGH);
GPIO_setOutput(GPIO_BANK7, GPIO_PIN9, OUTPUT_HIGH);
GPIO_setOutput(GPIO_BANK7, GPIO_PIN10, OUTPUT_HIGH);
GPIO_setOutput(GPIO_BANK7, GPIO_PIN11, OUTPUT_HIGH);
CLRBIT(SYSCONFIG->PINMUX[13], 0xFFFFFFFF);
SETBIT(SYSCONFIG->PINMUX[13], 0x88888811); //Set GPIO 6.8-13 to GPIOs and IMPORTANT Sets GP6[15] to /RESETOUT used by PHY, GP6[14] CLKOUT appears unconnected
#warn GP6.15 is also connected to CAMERA RESET This is a Bug in my board design Need to change Camera Reset to different IO.
GPIO_setDir(GPIO_BANK6, GPIO_PIN8, GPIO_OUTPUT);
GPIO_setDir(GPIO_BANK6, GPIO_PIN9, GPIO_OUTPUT);
GPIO_setDir(GPIO_BANK6, GPIO_PIN10, GPIO_OUTPUT);
GPIO_setDir(GPIO_BANK6, GPIO_PIN11, GPIO_OUTPUT);
GPIO_setDir(GPIO_BANK6, GPIO_PIN12, GPIO_OUTPUT);
GPIO_setDir(GPIO_BANK6, GPIO_PIN13, GPIO_INPUT);
while ((T1_TGCR & 0x7) != 0x7) {
for (index=0;index<50000;index++) {} // small delay before checking again
}
USTIMER_init();
// Turn on McBSP1
EVMOMAPL138_lpscTransition(PSC1, DOMAIN0, LPSC_MCBSP1, PSC_ENABLE);
// If Linux has already booted It sets a flag so no need to delay
if ( GET_ISLINUX_BOOTED == 0) {
USTIMER_delay(4*DELAY_1_SEC); // delay allowing Linux to partially boot before continuing with DSP code
}
// init the us timer and i2c for all to use.
I2C_init(I2C0, I2C_CLK_100K);
init_ColorVision();
init_LCD_mem(); // added rick
EVTCLR0 = 0xFFFFFFFF;
EVTCLR1 = 0xFFFFFFFF;
EVTCLR2 = 0xFFFFFFFF;
EVTCLR3 = 0xFFFFFFFF;
init_DMA();
init_McBSP();
init_LADAR();
CLRBIT(SYSCONFIG->PINMUX[1], 0xFFFFFFFF);
SETBIT(SYSCONFIG->PINMUX[1], 0x22222220); // This is enabling the McBSP1 pins
CLRBIT(SYSCONFIG->PINMUX[5], 0x00FF0FFF);
SETBIT(SYSCONFIG->PINMUX[5], 0x00110111); // This is enabling SPI pins
CLRBIT(SYSCONFIG->PINMUX[16], 0xFFFF0000);
SETBIT(SYSCONFIG->PINMUX[16], 0x88880000); // setup GP7.8 through GP7.13
CLRBIT(SYSCONFIG->PINMUX[17], 0x000000FF);
SETBIT(SYSCONFIG->PINMUX[17], 0x00000088); // setup GP7.8 through GP7.13
init_LCD();
LADARps.x = 3.5/12; // 3.5/12 for front mounting
LADARps.y = 0;
LADARps.theta = 1; // not inverted
OPTITRACKps.x = 0;
OPTITRACKps.y = 0;
OPTITRACKps.theta = 0;
for(i = 0;i<LADAR_MAX_DATA_SIZE;i++)
{ LADARdistance[i] = LADAR_MAX_READING; } //initialize all readings to max value.
// 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;
// TODO: defined destinations that moves the robot around and outside the course
robotdest[0].x = -5; robotdest[0].y = -3;
robotdest[1].x = 0; robotdest[1].y = -1;
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 = 0; robotdest[4].y = -1;
robotdest[5].x = 5; robotdest[5].y = -3;
robotdest[6].x = 0; robotdest[6].y = -2;
//back to middle
robotdest[7].x = 0; robotdest[7].y = 11;
robotdest[8].x = 0; robotdest[8].y = -2;
robotdest[9].x = -2; robotdest[9].y = -4;
robotdest[10].x = 2; robotdest[10].y = -4;
// flag pins
GPIO_setDir(IMAGE_TO_LINUX_BANK, IMAGE_TO_LINUX_FLAG, GPIO_OUTPUT);
GPIO_setDir(OPTITRACKDATA_FROM_LINUX_BANK, OPTITRACKDATA_FROM_LINUX_FLAG, GPIO_OUTPUT);
GPIO_setDir(DATA_TO_LINUX_BANK, DATA_TO_LINUX_FLAG, GPIO_OUTPUT);
GPIO_setDir(DATA_FROM_LINUX_BANK, DATA_FROM_LINUX_FLAG, GPIO_OUTPUT);
GPIO_setDir(DATAFORFILE_TO_LINUX_BANK, DATAFORFILE_TO_LINUX_FLAG, GPIO_OUTPUT);
GPIO_setDir(LVDATA_FROM_LINUX_BANK, LVDATA_FROM_LINUX_FLAG, GPIO_OUTPUT);
GPIO_setDir(LVDATA_TO_LINUX_BANK, LVDATA_TO_LINUX_FLAG, GPIO_OUTPUT);
CLR_OPTITRACKDATA_FROM_LINUX; // Clear = tell linux DSP is ready for new Opitrack data
CLR_DATA_FROM_LINUX; // Clear = tell linux that DSP is ready for new data
CLR_DATAFORFILE_TO_LINUX; // Clear = linux not requesting data
SET_DATA_TO_LINUX; // Set = put float array data into shared memory for linux
SET_IMAGE_TO_LINUX; // Set = put image into shared memory for linux
CLR_LVDATA_FROM_LINUX; // Clear = tell linux that DSP is ready for new LV data
SET_LVDATA_TO_LINUX; // Set = put LV char data into shared memory for linux
// clear all possible EDMA
EDMA3_0_Regs->SHADOW[1].ICR = 0xFFFFFFFF;
// Add your init code here
BIOS_start(); /* does not return */
return(0);
}
long timecount= 0;
int whichled = 0;
// This SWI is Posted after each set of new data from the F28335
void RobotControl(void) {
int newOPTITRACKpose = 0;
int i = 0;
if (0==(timecount%1000)) {
switch(whichled) {
case 0:
SETREDLED;
CLRBLUELED;
CLRGREENLED;
whichled = 1;
break;
case 1:
CLRREDLED;
SETBLUELED;
CLRGREENLED;
whichled = 2;
break;
case 2:
CLRREDLED;
CLRBLUELED;
SETGREENLED;
whichled = 0;
break;
default:
whichled = 0;
break;
}
}
if (GET_OPTITRACKDATA_FROM_LINUX) {
if (new_optitrack == 0) {
for (i=0;i<OPTITRACKDATASIZE;i++) {
Optitrackdata[i] = ptrshrdmem->Optitrackdata[i];
}
new_optitrack = 1;
}
CLR_OPTITRACKDATA_FROM_LINUX;
}
if (new_optitrack == 1) {
OPTITRACKps = UpdateOptitrackStates(ROBOTps, &newOPTITRACKpose);
new_optitrack = 0;
}
// using 400deg/s gyro
gyro = adcA3*3.0/4096.0;
if (settlegyro < SETTLETIME) {
settlegyro++;
if (settlegyro < (SETTLETIME/3)) {
// do nothing
} else if (settlegyro < (2*SETTLETIME/3)) {
gyro_zero = gyro_zero + gyro/(SETTLETIME/3);
} else {
gyro_drift += (((gyro-gyro_zero) + old_gyro)*.0005)/(SETTLETIME/3);
old_gyro = gyro-gyro_zero;
}
if(settlegyro%500 == 0) {
LCDPrintfLine(1,"Cal Gyro -- %.1fSecs", (float)(SETTLETIME - settlegyro)/1000.0 );
LCDPrintfLine(2,"");
}
enc1old = enc1;
enc2old = enc2;
newOPTITRACKpose = 0;
SetRobotOutputs(0,0,0,0,0,0,0,0,0,0); // 4.4 = 0 || 6.4 = 1 ||
} else {
gyro_angle = gyro_angle - ((gyro-gyro_zero) + old_gyro)*.0005 + gyro_drift;
old_gyro = gyro-gyro_zero;
gyro_radians = (gyro_angle * (PI/180.0)*400.0*gyro4x_gain);
// Kalman filtering
vel1 = (enc1 - enc1old)/(193.0*0.001); // calculate actual velocities
vel2 = (enc2 - enc2old)/(193.0*0.001);
if (fabsf(vel1) > 10.0) vel1 = vel1old; // check for encoder roll-over should never happen
if (fabsf(vel2) > 10.0) vel2 = vel2old;
enc1old = enc1; // save past values
enc2old = enc2;
vel1old = vel1;
vel2old = vel2;
// Step 0: update B, u
B[0][0] = cosf(ROBOTps.theta)*0.001;
B[1][0] = sinf(ROBOTps.theta)*0.001;
B[2][1] = 0.001;
u[0][0] = 0.5*(vel1 + vel2); // linear velocity of robot
u[1][0] = (gyro-gyro_zero)*(PI/180.0)*400.0*gyro4x_gain; // angular velocity in rad/s (negative for right hand angle)
// Step 1: predict the state and estimate covariance
Matrix3x2_Mult(B, u, Bu); // Bu = B*u
Matrix3x1_Add(x_pred, Bu, x_pred, 1.0, 1.0); // x_pred = x_pred(old) + Bu
Matrix3x3_Add(P_pred, Q, P_pred, 1.0, 1.0); // P_pred = P_pred(old) + Q
// Step 2: if there is a new measurement, then update the state
if (1 == newOPTITRACKpose) {
newOPTITRACKpose = 0;
z[0][0] = OPTITRACKps.x; // take in the LADAR measurement
z[1][0] = OPTITRACKps.y;
// fix for OptiTrack problem at 180 degrees
if (cosf(ROBOTps.theta) < -0.99) {
z[2][0] = ROBOTps.theta;
}
else {
z[2][0] = OPTITRACKps.theta;
}
// Step 2a: calculate the innovation/measurement residual, ytilde
Matrix3x1_Add(z, x_pred, ytilde, 1.0, -1.0); // ytilde = z-x_pred
// Step 2b: calculate innovation covariance, S
Matrix3x3_Add(P_pred, R, S, 1.0, 1.0); // S = P_pred + R
// Step 2c: calculate the optimal Kalman gain, K
Matrix3x3_Invert(S, S_inv);
Matrix3x3_Mult(P_pred, S_inv, K); // K = P_pred*(S^-1)
// Step 2d: update the state estimate x_pred = x_pred(old) + K*ytilde
Matrix3x1_Mult(K, ytilde, temp_3x1);
Matrix3x1_Add(x_pred, temp_3x1, x_pred, 1.0, 1.0);
// Step 2e: update the covariance estimate P_pred = (I-K)*P_pred(old)
Matrix3x3_Add(eye3, K, temp_3x3, 1.0, -1.0);
Matrix3x3_Mult(temp_3x3, P_pred, P_pred);
} // end of correction step
// set ROBOTps to the updated and corrected Kalman values.
ROBOTps.x = x_pred[0][0];
ROBOTps.y = x_pred[1][0];
ROBOTps.theta = x_pred[2][0];
if((first_time == 0) && (timecount>200)) {
first_time = 1;
Semaphore_post(SEM_startAstar);
}
// uses xy code to step through an array of positions
// if( xy_control(&vref, &turn, 1.0, ROBOTps.x, ROBOTps.y, robotdest[statePos].x, robotdest[statePos].y, ROBOTps.theta, 0.25, 0.5))
// { statePos = (statePos+1)%robotdestSize; }
if (newLADARdata == 1) {
newLADARdata = 0;
for (i=0;i<228;i++) {
LADARdistance[i] = newLADARdistance[i];
LADARangle[i] = newLADARangle[i];
LADARdataX[i] = newLADARdataX[i];
LADARdataY[i] = newLADARdataY[i];
}
for (i=0;i<57;i++) {
if ( (obs[i].flag == 0) && (turn<0.2) ) { // 29, 114, 200 72 157
if ((LADARdataX[29]-obs[i].x)*(LADARdataX[29]-obs[i].x) + (LADARdataY[29]-obs[i].y)*(LADARdataY[29]-obs[i].y) < obstacle_threshold){
obs[i].tally++;
if (obs[i].tally > 5){
if (obs[i].c==0){
map[obs[i].idx2] = 'x';
map[obs[i].idx3] = 'x';
}
else if (obs[i].c==10){
map[obs[i].idx1] = 'x';
map[obs[i].idx2] = 'x';
}
else{
map[obs[i].idx1] = 'x';
map[obs[i].idx2] = 'x';
map[obs[i].idx3] = 'x';
}
mapflag = 1;
obs[i].flag = 1;
}
}
if ((LADARdataX[114]-obs[i].x)*(LADARdataX[114]-obs[i].x) + (LADARdataY[114]-obs[i].y)*(LADARdataY[114]-obs[i].y) < obstacle_threshold){
obs[i].tally++;
if (obs[i].tally > 5){
if (obs[i].c==0){
map[obs[i].idx2] = 'x';
map[obs[i].idx3] = 'x';
}
else if (obs[i].c==10){
map[obs[i].idx1] = 'x';
map[obs[i].idx2] = 'x';
}
else{
map[obs[i].idx1] = 'x';
map[obs[i].idx2] = 'x';
map[obs[i].idx3] = 'x';
}
mapflag = 1;
obs[i].flag = 1;
}
}
if ((LADARdataX[200]-obs[i].x)*(LADARdataX[200]-obs[i].x) + (LADARdataY[200]-obs[i].y)*(LADARdataY[200]-obs[i].y) < obstacle_threshold){
obs[i].tally++;
if (obs[i].tally > 5){
if (obs[i].c==0){
map[obs[i].idx2] = 'x';
map[obs[i].idx3] = 'x';
}
else if (obs[i].c==10){
map[obs[i].idx1] = 'x';
map[obs[i].idx2] = 'x';
}
else{
map[obs[i].idx1] = 'x';
map[obs[i].idx2] = 'x';
map[obs[i].idx3] = 'x';
}
mapflag = 1;
obs[i].flag = 1;
}
}
if ((LADARdataX[72]-obs[i].x)*(LADARdataX[72]-obs[i].x) + (LADARdataY[72]-obs[i].y)*(LADARdataY[72]-obs[i].y) < obstacle_threshold){
obs[i].tally++;
if (obs[i].tally > 5){
if (obs[i].c==0){
map[obs[i].idx2] = 'x';
map[obs[i].idx3] = 'x';
}
else if (obs[i].c==10){
map[obs[i].idx1] = 'x';
map[obs[i].idx2] = 'x';
}
else{
map[obs[i].idx1] = 'x';
map[obs[i].idx2] = 'x';
map[obs[i].idx3] = 'x';
}
mapflag = 1;
...
This file has been truncated, please download it to see its full contents.
Comments