Mobile robot - C code for part 3

Code for final project

/*......................................................................
Final project - Recognizing commands
Mobile robot - Spring 1999
Alejandro J. Trejo
......................................................................*/

int leftMotor = 0; /* Motor ports */
int rightMotor = 2;

int leftS = 2; /* Photoresistor ports */
int rightS = 0;
int turnLeftS = 6;
int turnRightS = 4;

int topS = 1; /* Infrared ports */
int bottomS = 5;

int leftB = 12; /* Bumper ports */
int rightB = 8;
int centerB = 10;

int OK = 0; /* Constants */
int YES = 1;
int NO = 0;
int NONE = 0;
int HIT = 1;
int NOHIT = 0;
int left = -7;
int right = 7;

int flag; /* Flags */
int offRight = -1;
int offLeft = 1;
int leftTurn = -2;
int rightTurn = 2;
int seekRamp = 100;
int goRamp = 10;
int peak = 21;
int bumpedLeft = -3;
int bumpedRight = 3;
int bumpedCenter = 30;

float forward = 0.05; /* Time allowed for displacements */
float reverse = 1.0;
float adjusting = 0.2;
float turn = 1.0;
float adjustRamp = 0.5;
float turnAround = 2.0;

int leftForward = -25; /* Motor speeds */
int rightForward = -25;

int leftForwardFast = -40;
int rightForwardFast = -40;

int leftBackward = 50;
int rightBackward = 50;

int leftUphill = -80;
int rightUphill = -80;

int leftTracker; /* Sensor readings */
int rightTracker;
int leftTurner;
int rightTurner;
int leftBumper;
int rightBumper;
int centerBumper;
int topScanner;
int bottomScanner;

int limit = 10; /* Limits */
int turnLimit = 60;
int maxBumps = 2;

int compensation = 2; /* Compensations */
int turnCompensation = 9;

int turnCounter = 0; /* Global variables */
int displacements = 0;
int leftBumps = 0;
int rightBumps = 0;
int lastTurn = NONE;
int peakStop = NO;

/*.....................................................
Forward motion
.....................................................*/
int MoveForward()
{
motor(leftMotor,leftForward); /* Starts moving here */
motor(rightMotor,rightForward);
printf("Forward: %d:%d %d:%d %d\n",leftTracker,rightTracker, leftTurner, rightTurner, displacements);
sleep(forward);

leftTracker = analog(leftS); /* Gets sensor readings */
rightTracker = analog(rightS)+compensation;
leftTurner = analog(turnLeftS);
rightTurner = analog(turnRightS)+turnCompensation;
leftBumper = digital(leftB);
rightBumper = digital(rightB);
centerBumper = digital(centerB);

if ( (leftTurner>150) && (rightTurner>150) ) /* Robot is moving on ramp */
displacements++;

if (turnCounter>3) /* Reports ramp was found */
return seekRamp;

else if (centerBumper == HIT) /* Reports robot bumped */
return bumpedCenter;
else if (leftBumper == HIT)
return bumpedLeft;
else if (rightBumper == HIT)
return bumpedRight;

else if (leftTurner >= rightTurner+turnLimit) /* Reports a turn was found */
{
if (displacements>20) /* Check if getting off ramp. Otherwise, robot won't get off ramp */
{RawForward(); RawForward(); RawForward(); RawForward(); }
else
return leftTurn;
}
else if (rightTurner >= leftTurner+turnLimit)
{
if (displacements>20) /* Check if getting off ramp... */
{RawForward(); RawForward(); RawForward(); RawForward(); }
else
return rightTurn;
}

else if (leftTracker >= rightTracker+limit) /* Reports line was touched */
{
if ( (displacements>20) && ((leftTurner >= rightTurner+turnLimit) || (rightTurner >= leftTurner+turnLimit)) ) /* Check if getting off ramp... */
{RawForward(); RawForward(); RawForward(); RawForward(); }
else
return offRight;
}
else if (rightTracker >= leftTracker+limit)
{
if ( (displacements>20) && ((leftTurner >= rightTurner+turnLimit) || (rightTurner >= leftTurner+turnLimit)) ) /* Check if getting off ramp... */
{RawForward(); RawForward(); RawForward(); RawForward(); }
else
return offLeft;
}

return OK;
} /* End forward motion */

/*.....................................................
Raw forward motion
.....................................................*/
int RawForward()
{
motor(leftMotor,leftForward);
motor(rightMotor,rightForward);
printf("Getting off ramp...\n");
sleep(forward);
}

/*.....................................................
Adjustment to right
.....................................................*/
void AdjustRight()
{
motor(rightMotor,rightBackward);
motor(leftMotor,leftForwardFast);
printf("Adjusting...\n");
sleep(adjusting);
}

/*.....................................................
Adjustment to left
.....................................................*/
void AdjustLeft()
{
motor(leftMotor,leftBackward);
motor(rightMotor, rightForwardFast);
printf("Adjusting...\n");
sleep(adjusting);
}

/*.....................................................
Right turn
.....................................................*/
void TurnRight()
{
motor(rightMotor,rightBackward);
motor(leftMotor,leftForwardFast);
printf("Turning right...\n");
sleep(turn);
}

/*.....................................................
Left turn
.....................................................*/
void TurnLeft()
{
motor(rightMotor,rightForwardFast);
motor(leftMotor,leftBackward);
printf("Turning left...\n");
sleep(turn);
}

/*.....................................................
Reverse
.....................................................*/
void Reverse()
{
motor(leftMotor,-leftForward);
motor(rightMotor,-rightForward);
printf("Backing...\n");
sleep(reverse);
}

/*.....................................................
Turn around
.....................................................*/
void TurnAround(int direction)
{
Reverse();
Reverse();
if (direction == right)
{
motor(rightMotor,rightBackward);
motor(leftMotor,leftForwardFast);
printf("Turning around...\n");
sleep(turnAround);
}
if (direction == left)
{
motor(rightMotor,rightForwardFast);
motor(leftMotor,leftBackward);
printf("Turning around...\n");
sleep(turnAround);
}
} /* End Turn around */

/*.........................................................
Forward motion uphill on ramp
.........................................................*/
int ForwardRamp()
{
motor(leftMotor,leftUphill); /* Starts moving here */
motor(rightMotor,rightUphill);
printf("Climbing ramp %d %d\n",displacements, peakStop);
sleep(forward);
displacements++;

leftBumper = digital(leftB); /* Gets sensor readings */
rightBumper = digital(rightB);
centerBumper = digital(centerB);

if (leftBumper == HIT) /* Reports bump */
return bumpedLeft;
else if (rightBumper == HIT)
return bumpedRight;
else if (displacements > 80) /* On center of ramp. End fast speed */
return peak;
else
return OK;
} /* End forward ramp */

/*.....................................................
Climbing ramp
.....................................................*/
void ClimbRamp()
{
if ( (lastTurn == leftTurn) && (flag == seekRamp) )
{
motor(rightMotor,rightBackward); /* Robot corrects for ramp on right */
motor(leftMotor,leftForwardFast);
printf("Finding ramp...\n");
sleep(adjustRamp);
}
if ( (lastTurn == rightTurn) && (flag == seekRamp) )
{
motor(rightMotor,rightForwardFast); /* Robot corrects for ramp on left */
motor(leftMotor,leftBackward);
printf("Finding ramp...\n");
sleep(adjustRamp);
}
flag = OK;
lastTurn = NONE;
turnCounter = 0;
displacements=0;

while (1) /* Robot climbs ramp */
{
while (flag == OK) flag=ForwardRamp(); /* Move a little and check if bumped */

if (flag == bumpedLeft) /* Correct trajectory if bumped */
{AdjustRight();flag = OK;}
if (flag == bumpedRight)
{AdjustLeft();flag = OK;}
if (flag == peak)
break;
}
displacements=0;
} /* End climb ramp */

/*.....................................................
Main
.....................................................*/
void main()
{
printf("Ready :)\n");
while (1)
{
while (!start_button());
beep();

while (!stop_button())
{
flag = OK;
while (flag == OK) flag=MoveForward(); /* Move a little and check sensors */

if (flag == bumpedCenter) /* Center bumper hit */
{
alloff();
topScanner = analog(topS); /* Read command */
bottomScanner = analog(bottomS);
printf("Infrared: %d %d\n", topScanner, bottomScanner);
sleep(1.0);
if ( (topScanner>100 && bottomScanner<100) || (topScanner<100 && bottomScanner>100) ) /* Bicolor block */
{ TurnAround(right); flag = OK; }
if (topScanner>100 && bottomScanner>100) /* Black block */
{ TurnAround(right); peakStop = YES; flag = OK; }
if (topScanner<100 && bottomScanner<100) /* White block */
break;
}

if ( (leftTurner>150) && (rightTurner>150) ) /* If on ramp... */
{
if (flag == bumpedLeft) /* Left bumper hit - correct a little*/
{ AdjustRight(); AdjustRight(); flag=OK; }
if (flag == bumpedRight) /* Right bumper hit - correct a little*/
{ AdjustLeft(); AdjustLeft(); flag=OK; }
}

else /* If off ramp... */
{
displacements = 0;
if ( (flag == bumpedLeft) && (leftBumps<maxBumps) ) /* Left bumper hit */
{ Reverse();leftBumps++;flag = OK; }
if (leftBumps == maxBumps) /* --enough bumping on the left! */
{ TurnAround(left);leftBumps=0;flag=OK; }
if ( (flag == bumpedRight) && (rightBumps<maxBumps) ) /* Right bumper hit */
{ Reverse();rightBumps++;flag = OK; }
if (rightBumps == maxBumps) /* --enough bumping on the right! */
{ TurnAround(right);rightBumps=0;flag=OK; }
if (flag == offRight) /* Line hit on left side */
{ AdjustLeft(); turnCounter=0; flag = OK; }
if (flag == offLeft) /* Line hit on right side */
{ AdjustRight(); turnCounter=0; flag = OK; }
if (flag == leftTurn) /* Turn found on left side */
{ TurnLeft(); turnCounter++; lastTurn = leftTurn; flag = OK; }
if (flag == rightTurn) /* Turn found on right side */
{ TurnRight(); turnCounter++; lastTurn = rightTurn; flag = OK; }
if (flag == seekRamp) /* Ramp was found */
{ ClimbRamp(); flag = OK; if (peakStop == YES) break; }
}
flag = OK;
}

flag = OK;
alloff();
beep();
printf("Done! :)\n");
}
}

Back

Alejandro J. Trejo
University of Texas at El Paso
Last update: 09 Apr 1999.