Robotics Project:

Steering Controller

Ross Makulec
ME 5286: Robotics
March 11, 2009
Following is the guidance controller code for the UMN Safetruck project. The guidance
controller first calculates the section of the path closest to the current position of the truck. It
then calculates the angle between the front of the truck and the next point on the path and
steers the wheels of the truck toward that point. When the truck comes within a certain
distance of that point it then steers toward the next point. This system is currently not sufficient
to follow the curve of the MnROAD course. Improving the program by having the truck
continuously look a certain distance ahead and steer based on that point should improve
accuracy. Accuracy is measured by the lateral distance between the truck and the path,
calculated as the perpendicular distance from the gps to the path.

/*************************************************************************
* SAFETRUCK Project
* University of Minnesota
*
* FILE: GuidanceController.c
* AUTHOR: Alec Gorjestani
* CREATION: 5/10/1998
*
* REVISION: 5/26/1999 - Ported to Neutrino.
*
* REVISION: 11/1999 - Modified for Robotics course
* - Mike Sergi
*
*
* PURPOSE: This module is the guidance controller for the truck
* simulation. It calculates the desired steering angle and sends it via
* shared memory to the SteerController.c thread.
*
*************************************************************************/

#include "NEUTRINO.h"
#include "SpatialDB.h"
#include "nto_ipc.h"
#include "nto_timer.h"
#include "GuidanceController.h"
#include "trimble.h"
#include "nto_ipc.h"
#include "nto_timer.h"
#include "SpatialDB.h"
#include "mnroad/SDBAttributes.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define GUIDANCE_CONTR_RATE 5.0 /* Hz */

#define PI 3.14159265359
#define A2A 5.49

/* Define any global variables here */

/* Put your function prototypes here */

void GuidanceController(void);
double distance(double *point1, double *point2);
double perp_dist(double *point1, double *point2, double *curr_pos);
QueryIndex getGDB(void);
/*************************************************************************
*
* Main
*
************************************************************************/

int main() {

GuidanceController();

return(1);
}

/*************************************************************************
*
* GuidanceController
*
* This task determines the desired steering angle and sends it to the
* Velocity controller using the nto_ipc API.
*
************************************************************************/

void GuidanceController(void)
{
int steer_ipc, speed_ipc, truck_init_ipc, gps_ipc;
gps *gps_data;
double DelayTime, steer, *speed;
double curr_pos[2], des_pos[2], p1[2], p2[2];
double heading, dist, min_dist=1000.0, des_ang;
int i, j, pos_count;
QueryIndex index;
IndexIterator iter, iter_mindist;
struct LaneCenter *lc = NULL;

index = getGDB();
iter = index.head;

/* This is the gps shared memory ipc object */

if (( gps_ipc = nto_ipc_init(GPS_NAME, GPS_SIZE, WAIT, NULL, NULL)) <= 0) {
fprintf(stderr,"(GuidanceController): ERROR -- unable to open ipc-
%s\n", GPS_NAME);
return;
} //if

/* This is the steer command shared memory ipc object */

if (( steer_ipc = nto_ipc_init(STEER_IPCNAME, STEER_SIZE, NO_WAIT, NULL,
NULL)) <= 0) {
fprintf(stderr,"(GuidanceController): ERROR -- unable to open ipc-
%s\n", STEER_IPCNAME);
return;
} //if

/* This is the speed shared memory ipc object */

if (( speed_ipc = nto_ipc_init(SPEED_IPCNAME, SPEED_SIZE,
NO_WAIT, NULL, NULL)) <= 0) {
fprintf(stderr, "(GuidanceController): ERROR -- unable to open
ipc- %s\n", SPEED_IPCNAME);
return;
} //if
/* The memory space for speed is allocated here,
Other processes allocate memory for the gps and steer IPC's*/
if ((speed = (double *)malloc(SPEED_SIZE)) == NULL) {
 fprintf(stderr,"(GuidanceController): ERROR -- unable to alocate
memory for speed!!\n");
return;
} //if
memset( speed, 0x00, SPEED_SIZE);

/* Allocate memory for structures */

gps_data = (gps *)malloc(sizeof(gps));

printf("(GuidanceController): IPC and data structures initialized...\n");

*speed = 0.0;
printf("(GuidanceController): Setting speed to 0 while simulation
initializes..\n");
nto_ipc_write(speed_ipc, (void *)speed, SPEED_SIZE);
delay(8000); // Pause for 7 seconds to allow for other processes to
initialize
printf("(GuidanceController): Beginning guidance control loop.\n");

/* Get gps data from gps ipc */

// nto_ipc_read(gps_ipc, (void *)gps_data, GPS_SIZE);
if( nto_ipc_read(gps_ipc, (void *)gps_data, GPS_SIZE) == 0)
{ printf("(GuidanceController):Error reading from gps!\n"); }

heading = gps_data->heading;
curr_pos[0] = gps_data->cartesian_x+A2A*sin(heading);
curr_pos[1] = gps_data->cartesian_y+A2A*cos(heading);

printf("\nx = %lf y = %lf heading = %lf\n", curr_pos[0],

curr_pos[1], heading);

for(i=0;i<index.Length;i++)
{
for(j=0;j<*(iter->NumPoints)-1;j++)
{
p1[0] = (iter->Points)[j].x;
p1[1] = (iter->Points)[j].y;
p2[0] = (iter->Points)[j+1].x;
p2[1] = (iter->Points)[j+1].y;

dist = perp_dist(p1, p2, curr_pos);

if(dist<min_dist){
pos_count = j+1;
iter_mindist = iter;
min_dist = dist;
}
}
iter = iter->next;
}

iter = iter_mindist;

des_pos[0] = (iter->Points)[pos_count].x;
des_pos[1] = (iter->Points)[pos_count].y;

printf("\ndes_x = %lf des_y = %lf\n", des_pos[0], des_pos[1]);

DelayTime = ( 1.0 / GUIDANCE_CONTR_RATE )*1000; // msec

 for (;;) {

/* Get gps data from gps ipc */

// nto_ipc_read(gps_ipc, (void *)gps_data, GPS_SIZE);
if( nto_ipc_read(gps_ipc, (void *)gps_data, GPS_SIZE) == 0){
printf("(GuidanceController):Error reading from gps!\n");
}

heading = gps_data->heading;
curr_pos[0] = gps_data->cartesian_x + A2A*sin(heading);
curr_pos[1] = gps_data->cartesian_y + A2A*cos(heading);

printf("\n\ncurrent position: %lf %lf", curr_pos[0], curr_pos[1]);

if(distance(curr_pos, des_pos)<1) {
pos_count ++;
if(pos_count == *(iter->NumPoints)){
iter = iter->next;
pos_count = 0;
}
des_pos[0] = (iter->Points)[pos_count].x;
des_pos[1] = (iter->Points)[pos_count].y;
}
printf("\ndesired position: %lf %lf", des_pos[0], des_pos[1]);

des_ang = -atan2(des_pos[0]-curr_pos[0], des_pos[1]-curr_pos[1]);

printf("\ndes_ang = %lf", des_ang);

printf("\nheading = %lf", heading);

steer = des_ang - heading;

printf("\nsteer = %lf", steer);

lc = (struct LaneCenter*)(iter->Attributes);

//*speed = (lc->speed)/2.2369363;

/**************************************************************************/
/* Insert your guidance controller code here.. You are going to want */
/* to extract your current position and heading from the gps structure */
/* and then use this information to find what steering angle you need */
/* in order to coverge to the desired path. Set the variable "steer" */
/* equal to your desired steering angle (in radians, ccw from north). */
/* This value will be written to the steering shared memory object below */
/**************************************************************************/
//steer = 0;
*speed = 10.0;
printf("\n%lf @ %lf rad", *speed, steer);
/************************************************************************
**/
/* You will also want to determine the speed limit and an approriate
speed */
/* and send it to the speed shared memory object. The speed controller
*/
/* will adjust the vehicle speed to that speed. Currently the speed is
being */
/* set to 10.0 m/s */
/************************************************************************
**/
 nto_ipc_write(speed_ipc, (void *)speed, SPEED_SIZE);
nto_ipc_write(steer_ipc, &steer, STEER_SIZE);
delay((int)(DelayTime));
} /* end for(;;) */
}

/**********************************************************************/

QueryIndex getGDB(void)
{
int i,j,k;
/* query values */
char SDB_name[] = "TEST_client\0";
int SDB_coid=0;
int query_size;

/* query structure */
UserQuery uq;
QueryIndex index;
IndexIterator iter;

double RIGHT,LEFT,TOP,BOTTOM;
struct LaneCenter *lc = NULL;

double temp;
FILE *out_file=NULL;

/* You must call this function before using the database! */

InitializeIndex(&index);

printf("opening com to database, '%s'\n",DATABASE_ID);

/* initialize the ipc with the database */

/* The SDB_name var (defined above) can be any name you like */
SDB_coid = sdb_ipc_open(SDB_name,DATABASE_ID);
if( SDB_coid == -1 )
{
printf("error\n");
return index;
}

/* This will set the query to only lane centers */

strcpy(uq.query,"LaneCenter()\0");
/* These are the max and min x,y coordinates that bound MnRoad */
RIGHT = 821010.041;
BOTTOM = 349861.605;
LEFT = 824582.237;
TOP = 353102.538;
/* Set up your query polygon. There are 5 points because you must */
/* close the polygon (in this case, a square) */
uq.num_pts = 5;
uq.pts[0].x = RIGHT; uq.pts[0].y = BOTTOM;
uq.pts[1].x = RIGHT; uq.pts[1].y = TOP;
uq.pts[2].x = LEFT; uq.pts[2].y = TOP;
uq.pts[3].x = LEFT; uq.pts[3].y = BOTTOM;
uq.pts[4].x = uq.pts[0].x; uq.pts[4].y = uq.pts[0].y;

printf("sending query\n");
query_size = sdb_ipc_query(SDB_coid,&uq,&index);

printf("%d objects\n",index.Length);
 /* This sorts the values returned */
SortInt(&index,LaneCenter_id,ASCENDING);

return(index);
}

double distance(double *point1, double *point2)

{
double dist;
dist = (point1[0]-point2[0])*(point1[0]-point2[0])+(point1[1]-
point2[1])*(point1[1]-point2[1]);
dist = sqrt(dist);
return(dist);
}

double perp_dist(double *point1, double *point2, double *curr_pos)

{
double A,B,C, dist;

A = point2[1] - point1[1];
B = point1[0] - point2[0];
C = point2[0]*point1[1]-point1[0]*point2[1];

dist = abs(A*curr_pos[0]+B*curr_pos[1]+C)/sqrt(A*A+B*B);
return(dist);
}