Here's the code that makes the Nomad operational:
#include <stdio.h>
#include <stdlib.h>
#include "Nclient.h"
#define ROTATION_CONSTANT 0.118597 /* inches/degree (known to 100 ppm) */
#define RIGHT(trans, steer) (trans + (int)((float)steer*ROTATION_CONSTANT))
#define LEFT(trans, steer) (trans - (int)((float)steer*ROTATION_CONSTANT))
#define scout_vm(trans, steer) vm(RIGHT(trans, steer), LEFT(trans, steer), 0)
#define scout_pr(trans, steer) pr(RIGHT(trans, steer), LEFT(trans, steer), 0)
short angle;
short dist;
short foward_power;
int hallwidth2;
int left_doors,right_doors,door_flag,ldoor_flag,rdoor_flag,uld,user_doors,not;
char user_side;
int i,counter,x,y,a;
int current_quad;
int dist_quad;
char current_direction;
char command[10];
int dr = 0;
short c;
int quad=0;
int current_index=0;
int instructions[1000];
int matrix[12][4];
int length[12][4];
int where[1000];
int sequence[20];
int inches;
int hallwidth=0;
int adjl=25;
int adjr=-25;
int dummy;
int next_room=0;
/****************************************************************************/
void turn_front_sonar_on(void)
{
int sn_order[16] = {0, 2, 15, 1, 14, 3, 13, 4, 12, 5, 11, 6, 10, 7, 9, 8}; /*
Remainder are irrelevant */
/* turn on 5 front sonars and set them to fire in certain order */
conf_sn(15, sn_order);
conf_tm(16);
}
/*******************************************************************************
***/
short shortest_front_sonar(long *state, short min_dist)
{
short i, shortest, shortest_sonar;
shortest = 255;
shortest_sonar = 0;
i = 17;
do {
if (state[i] < shortest) {
shortest = (short)state[i];
shortest_sonar = i-16;
}
i++;
} while (i < 33);
if (shortest > min_dist)
shortest_sonar = 0;
dist = shortest;
return(shortest_sonar);
}
/*******************************************************************************
***/
short get_dist(long *state, short sonar)
{
short distance;
distance = (short)state[sonar+17];
return(distance);
}
/*******************************************************************************
**/
void avoid_obsticle(){
if((get_dist(State,0) < 35) || (get_dist(State,1) < 30) || (get_dist(State,15)
< 30)|| (get_dist(State,2) < 20) || (get_dist(State,14) < 20) ||
(get_dist(State,4) < 10) || (get_dist(State,12) < 20)){
printf("\nobsticle");
foward_power = 15;
if((abs( get_dist(State,1) - get_dist(State,15)) > 3)){
if(get_dist(State,1) > get_dist(State,15)){
angle = angle + 50;
}
if(get_dist(State,15) > get_dist(State,1)){
angle = angle - 50;
}
}
else{
printf("\nsame angle");
angle = -100;
if((abs(get_dist(State,2) - get_dist(State,14)) > 5)){
if(get_dist(State,2) > get_dist(State,14)){
angle = angle + 50;
}
if(get_dist(State,14) > get_dist(State,2)){
angle = angle - 50;
}
}
}
if(((abs(get_dist(State,2) - get_dist(State,14)) < 5)) &&
(get_dist(State,0) < 5) || (get_dist(State,0) < 8)){
foward_power = 1;
printf("\nSTOP! %d ", abs( get_dist(State,2) - get_dist(State,14))
);
/*angle=0;*/
}
}
if((get_dist(State,3) <= 20) && (get_dist(State,13) > 20)){
angle = angle - 10;
}
if((get_dist(State,13) <= 20) && (get_dist(State,3) > 20)){
angle = angle + 10;
}
if((get_dist(State,4) <= 20) && (get_dist(State,12) > 20)){
angle = angle - 10;
}
if((get_dist(State,14) <= 20) && (get_dist(State,4) > 20)){
angle = angle + 10;
}
if((get_dist(State,1) <= 20) && (get_dist(State,15) > 20)){
angle = angle - 10;
}
if((get_dist(State,3) <= 11) && (get_dist(State,13) > 11)){
angle = angle - 50;
}
if((get_dist(State,13) <= 11) && (get_dist(State,3) > 11)){
angle = angle + 50;
}
if((get_dist(State,4) <= 11) && (get_dist(State,12) > 11)){
angle = angle - 50;
}
if((get_dist(State,14) <= 11) && (get_dist(State,4) > 11)){
angle = angle + 50;
}
if((get_dist(State,1) <= 11) && (get_dist(State,15) > 11)){
angle = angle - 50;
}
}
/*******************************************************************************
**/
int door()
{
int r,n,count;
count=0;
n=0;
while(n<20000)
{
n++;
if ( (get_dist(State,4)>70)||(get_dist(State,12)>70) )
{
count=count+1;
}
}
printf("\ncount=%d",count);
if ((count>19995)&&((ldoor_flag==0)||(rdoor_flag==0))){
door_flag=1;
printf("\ndoor!%d",door_flag);
if( (get_dist(State,4)>70) &&(ldoor_flag==0) ){
left_doors=left_doors+1;
}
if( (get_dist(State,12)>70)&&(rdoor_flag==0) )
{
right_doors=right_doors+1;
}
if (get_dist(State,4)>70){
ldoor_flag=1;
}
if( get_dist(State,12)>70){
rdoor_flag=1;
}
r=1;
}
else
{
r=0;
not=not+1;
}
if((count<10)&&(not>250))
{
door_flag=0;
if( get_dist(State,4)<50){
ldoor_flag=0;
}
if (get_dist(State,12)<50){
rdoor_flag=0;
}
not=0;
}
printf("\nr=%d",r);
return(r);
}
/**********************************************************************************/
/**********************************************************************/
void move_down_the_hall(){
foward_power = 35;
angle = ((get_dist(State,3) - get_dist(State,13)) + (get_dist(State,2) -
get_dist(State,14)));
angle=angle*3;
if ((get_dist(State,4) > hallwidth+20) || (get_dist(State,12) > hallwidth+20) ||
(get_dist(State,3) > hallwidth+20) || (get_dist(State,13) > hallwidth+20) ||
(get_dist(State,2) > hallwidth+20) || (get_dist(State,14) > hallwidth+20)){
angle = 0;
/*
if(get_dist(State,4)<(hallwidth/2)-2){
angle=adjr;
}
if(get_dist(State,12)<(hallwidth/2)-2){
angle=adjl;
}
*/
/*
if( (get_dist(State,4)>(hallwidth/2)+2)&&(get_dist(State,4)<(hallwidth))
){
angle=adjl;
}
if( (get_dist(State,12)>(hallwidth/2)+2)&&(get_dist(State,12)<(hallwidth))
){
angle=adjr;
}
*/
}
if((get_dist(State,1) <= hallwidth/2) && (get_dist(State,15) > hallwidth/2+3)){
angle = adjr;
}
if((get_dist(State,15) <= hallwidth/2) && (get_dist(State,1) > hallwidth/2+3)){
angle = adjl;
}
/*
if((get_dist(State,4) <= 7) && (get_dist(State,12) > 7)){
angle = adjr;
}
*/
if((get_dist(State,15) <= 7) && (get_dist(State,1) > 7)){
angle = adjl;
}
if((get_dist(State,1) <= 7) && (get_dist(State,15) > 7)){
angle = adjr;
}
if(((abs(get_dist(State,2) - get_dist(State,14)) < 5)) &&
(get_dist(State,0) < 5) || (get_dist(State,0) < 8)){
foward_power = 0;
printf("\nSTOP! %d ", abs( get_dist(State,2) - get_dist(State,14))
);
/*angle=0;*/
}
system("clear");
printf("\nangle= %d ",angle);
printf("\nright side= %d ",get_dist(State,12));
printf("\nleft side= %d ",get_dist(State,4));
}
/*************************< get input >********************************/
void get_input()
{int j;
printf("\nWhat Quadrant and direction are you in now?->");
scanf("%d%c",¤t_quad,¤t_direction);
printf("\nWhat Sequence of Quadrants do you want to go to? enter a 0 at end of
sequence->");
j=0;
scanf("%d",&dist_quad);
while(dist_quad != 0){
sequence[j]=dist_quad;
scanf("%d",&dist_quad);
j++;
}
printf("\n\n%c",current_direction);
printf("\n");
}
/*****************************************************************/
int get_input2()
{
printf("\nWhere do you want to go?>");
scanf("%d%c",&user_doors,&user_side);
if(user_side =='r')
return(1);
else
return(0);
}
/************************< bumped >************************************/
void bumped(){
int i;
if(State[33] != 0){
printf("\nBumped!\n");
for (i=1; i<10; i++){
scout_vm(-100,50);
printf("\nBacking...");
}
}
}
/********************************< hall width >***************************/
int hall_width(){
int left,right,total;
left = get_dist(State,4);
right = get_dist(State,12);
total=left+right;
return(total);
}
/*******************************************************************/
void last_avoid()
{
if((get_dist(State,3) <= 7) && (get_dist(State,13) > 7)){
angle = -25;
}
if((get_dist(State,13) <= 7) && (get_dist(State,3) > 7)){
angle = 25;
}
if((get_dist(State,4) <= 7) && (get_dist(State,12) > 7)){
angle = -25;
}
if((get_dist(State,14) <= 7) && (get_dist(State,4) > 7)){
angle = 25;
}
if((get_dist(State,1) <= 7) && (get_dist(State,15) > 7)){
angle = -25;
}
}
/**************< Turn Left >******************/
void turn_left()
{
int front,back,left,right;
/*
scout_pr(0,910);
sleep(4);
scout_vm(0,0);
ws(1,1,0,0);
*/
scout_vm(0,0);
ws(1,1,1,0);
front=get_dist(State,0);
back=get_dist(State,8);
right=get_dist(State,12);
left=get_dist(State,4);
scout_vm(0,800);
sleep(.1);
ws(1,1,0,0);
/*
while(
(get_dist(State,0)!=left)||(get_dist(State,4)||back)&&(get_dist(State,8)!=right)
||(get_dist(State,12)!=front) ){
scout_vm(0,50);
}
scout_vm(0,800);
sleep(.2);
*/
scout_pr(0,800);
sleep(1);
ws(1,1,0,0);
scout_vm(0,0);
ws(1,1,0,0);
}
/*************< Turn Right >***************/
void turn_right()
{
int front,back,left,right;
/*
scout_pr(0,-910);
sleep(4);
scout_vm(0,0);
ws(1,1,0,0);
*/
scout_vm(0,0);
ws(1,1,1,0);
front=get_dist(State,0);
back=get_dist(State,8);
right=get_dist(State,12);
left=get_dist(State,4);
scout_vm(0,-500);
sleep(.1);
ws(1,1,0,0);
/*
while(
(get_dist(State,4)!=front)||(get_dist(State,12)||back)&&(get_dist(State,0)!=righ
t)||(get_dist(State,8)!=left) ){
scout_vm(0,-50);
}
scout_vm(0,-500);
sleep(.2);
ws(1,1,0,0);
*/
scout_pr(0,-800);
sleep(1);
scout_vm(0,0);
ws(1,1,0,0);
}
/******************************< Get to Quad >***************************/
void go_to_quad(int quads)
{
quads=quads*quad;
sleep(1);
while (get_dist(State,0) > quads+(hallwidth)/2 ){
printf("\nfront=%d",get_dist(State,0));
printf("\nhallwidth=%d",hallwidth);
move_down_the_hall();
scout_vm(foward_power,angle);
/* avoid_obsticle2(); */
bumped();
}
scout_vm(0,0);
ws(1,1,0,0);
}
/*****************************test for one door********************/
int just_one_door(int start){
int c,ii;
c=0;
for(ii=0;ii<4;ii++){
if (matrix[start][ii] == 0 ){
c++;
}
}
if (c==3){
return(1);}
else{
return(0);
}
}
/********************* Ken next ********************************/
void next_directionk( int start, int end){
int i,j,k,direction,dw,ken,seven;
int nextquad,countzeros,notzeros;
countzeros=0;
ken=0;
notzeros=0;
nextquad=start;
direction=-1;
if( (end==7) ) { seven=0;}
else{seven=1;}
/*******************************/
if (start!=end){
for(i=0;i<4;i++){
system("clear") ;
if(matrix[start][i] == 0){
countzeros++;
}
else{
notzeros=i;
}
if (countzeros == 3){
nextquad = matrix[start][notzeros];
direction = notzeros;
}
}//end for loop
}
if (start==end){
direction=9;
}
else{
if(countzeros!=3){
for(i=0; i<4 ;i++){
if (matrix[start][i] == end){
nextquad = matrix[start][i];
direction = i;
break;
}
else if(start > end){
if ((matrix[start][i] <= nextquad) &&
(end<=matrix[start][i]+seven)&&
(matrix[start][i]>0)&&
(!just_one_door(matrix[start][i])) ){
nextquad = matrix[start][i];
direction = i;
}
}
else if(start < end){
if ((matrix[start][i] >= nextquad)&&
(matrix[start][i]>0) &&
(matrix[start][i]<100) ){
nextquad = matrix[start][i];
direction = i;
}
}
}
}
/*******************************/
}
if (direction==(-1)){
if(start< end){
for(k=0; k<4 ;k++){
if ((matrix[start][k] == nextquad-1)){
nextquad = matrix[start][k];
direction = k;
}
}
}
if(start> end){
for(k=0; k<4 ;k++){
if ((matrix[start][k] == nextquad+1)){
nextquad = matrix[start][k];
direction = k;
}
}
}
}
if (direction>(-1)){
instructions[current_index] = direction;
where[current_index]=nextquad;
current_index++;
}
if (direction!=9){
next_directionk(nextquad,end);
}
}//final end
/**********************************readfile*************************/
void readfile(char *onefile){
FILE *fp;
char *fname;
int info=0;
int x,y;
/*strcpy(fname,onefile);*/
if((fp=fopen(onefile,"r")) == NULL){
printf("\n\nncant open file");
exit(1);
}
else{
printf("\n\n open file\n");
for (x=1; x<13; x++)
{
for (y=0; y<4; y++)
{
fscanf(fp,"%d",&info);
matrix[x][y] = info;
printf(" %d",info);
}
printf("\n");
}
fclose(fp);
}
}
/*******************************************************/
void readfile2(char *onefile){
FILE *fp;
char *fname;
int info=0;
int x,y;
if((fp=fopen(onefile,"r")) == NULL){
printf("cannot open file");
exit(1);
}
else{
for (x=1; x<13; x++){
for (y=0; y<4; y++){
fscanf(fp,"%d",&info);
length[x][y] = info;
}
}
fclose(fp);
}
}
/********************< Face correct direction >*********************/
void face_new_direction(int n) // n = destination direction
{
int rt,lt,a;
rt=0;
lt=0;
printf("\ndistination direction=%d ",n);
printf("\ncurrent direction %c",current_direction);
if( n==0){
if (current_direction=='s'){lt=2;}
if (current_direction=='e'){lt=1;}
if (current_direction=='w'){rt=1;}
current_direction='n';
}
if( n==1){
if (current_direction=='n'){rt=2;}
if (current_direction=='e'){rt=1;}
if (current_direction=='w'){lt=1;}
current_direction='s';
}
if( n==2){
if (current_direction=='n'){rt=1;}
if (current_direction=='s'){lt=1;}
if (current_direction=='w'){rt=2;}
current_direction='e';
}
if( n==3){
if (current_direction=='n'){lt=1;}
if (current_direction=='s'){rt=1;}
if (current_direction=='e'){lt=2;}
current_direction='w';
}
for (a=0; a<rt; a++){turn_right();}
for (a=0; a<lt; a++){turn_left();}
}
/************************< Get around >******************/
void get_around(int direction , int n)
{
face_new_direction(direction);
go_to_quad(length[where[n]][direction]);
}
/*******************************************************************************
*******/
void test()
{
short shortest_front;
shortest_front = shortest_front_sonar(State, 300);
printf("shortest sensor= %d\n",shortest_front-1);
angle=(shortest_front-9)*22.5;
/*gs();*/
}
/********************************* main ****************************/
main(void){
door_flag = 0;
ldoor_flag = 0;
rdoor_flag = 0;
left_doors = 0;
right_doors = 0;
not = 0;
connect_robot(1, MODEL_SCOUT2, "/dev/ttyS0", 38400);
turn_front_sonar_on();
/************************/
for (x=0;x<30;x++){
instructions[x]=0;
where[x]=0;
}
gs();
sleep(1);
scout_vm(0,0);
ws(1,1,0,0);
hallwidth = hall_width();
hallwidth2=hallwidth;
quad=hallwidth+18;
/*******************************************************/
while (strcmp(command,"quit")){
system("clear");
printf("\nCOMMAND>");
scanf("%s",command);
if (!strcmp(command,"turn_right")){
turn_right();
}
else if (!strcmp(command,"turn_left")){
turn_left();
}
else if (!strcmp(command,"go_foward")){
system("clear");
printf("\nFor how long? (in seconds)");
scanf("%d",&inches);
scout_vm(35,0);
sleep(inches);
scout_vm(0,0);
ws(1,1,0,0);
}
else if (!strcmp(command,"go_backward")){
system("clear");
printf("\nHow long? (in secons)");
scanf("%d",&inches);
scout_vm(-35,0);
sleep(inches);
scout_vm(0,0);
ws(1,1,0,0);
}
else if (!strcmp(command,"do_doors")){
a=get_input2();
dr=0;
while (dr < user_doors){
system("clear");
if(a)
dr=right_doors;
else
dr=left_doors;
scout_vm(foward_power,angle);
move_down_the_hall();
avoid_obsticle();
if(State[33] != 0){
printf("\nBumped!\n");
for (i=1; i<10; i++)
{
scout_vm(-100,0);
printf("\nBacking...");
}
}
door();
printf("\nLeft doors=%d ",left_doors);
printf("\nright doors=%d ",right_doors);
printf("\nleft door flag=%d ",ldoor_flag," %d",rdoor_flag);
printf("\nright door flag=%d ",rdoor_flag);
}
}
else if (!strcmp(command,"fallow")){
gs();
/* test();*/
while (1)
{
if (angle>180){
angle=-(360-angle);
}
scout_vm(-35,angle*5);
test();
}
}
else if (!strcmp(command,"run")){
gs();
/* test();*/
while (1)
{
if (angle>180){
angle=-(360-angle);
}
scout_vm(35,angle*5);
test();
}
}
else if (!strcmp(command,"do_maze")){
readfile("map.txt");
readfile2("length.txt");
get_input();
while (sequence[next_room] != 0){
dist_quad = sequence[next_room];
next_room++;
where[0]=current_quad;
next_directionk(current_quad,dist_quad);
printf("\n sequance[next room] =%d curent dir= %c",
sequence[next_room],current_direction);
printf("\n current quad = %d current dest = %d", current_quad,dist_quad);
for (counter=0;counter<current_index-1;counter++){
printf("current index=%d",current_index);
get_around(instructions[counter],counter);
}
current_quad=dist_quad;
current_index=0;
for (x=0;x<30;x++){
instructions[x]=0;
where[x]=0;
}
not = 0;
}
}
scout_vm(0,0);
}
return(0);
}
University of Texas at El Paso
Last update: 03 May 1999.