Back

Code for the first assignment

/*.....................................................................................
Finding the top of the ramp
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 leftB = 12; /* Bumper ports */
int rightB = 8;

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 offRight = -1; /* Flags */
int offLeft = 1;
int leftTurn = -2;
int rightTurn = 2;
int seekRamp = 100;
int goRamp = 10;
int endRamp = 21;
int bumpedLeft = -3;
int bumpedRight = 3;

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

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 limit = 10; /* Limits after which a significant color change is reported */
int turnLimit = 50;

int compensation = 2; /* Compensations for different sensor sensitivities */
int turnCompensation = 9;

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

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

leftTracker = analog(leftS); /* Gets sensor readings */
rightTracker = analog(rightS)+compensation;
leftTurner = analog(turnLeftS);
rightTurner = analog(turnRightS)+turnCompensation;

if (leftTurner>200 && rightTurner>200) /* Ramp was found by seeing */
return goRamp;
else if (turnCounter>3) /* Too many turns. It must be the ramp */
return seekRamp;
else

if (leftTurner >= rightTurner+turnLimit) /* Reports a turn was found */
return leftTurn;
else if (rightTurner >= leftTurner+turnLimit)
return rightTurn;
else

if (leftTracker >= rightTracker+limit) /* Reports line was touched */
return offRight;
else if (rightTracker >= leftTracker+limit)
return offLeft;
else
return OK;
} /* End forward motion */

/*.....................................................
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);
}

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

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

if (leftBumper == HIT) /* Reports bump */
return bumpedLeft;
else if (rightBumper == HIT)
return bumpedRight;
else if (displacements > 80) /* Reports found top of ramp */
return endRamp;
else
return OK;
} /* End climb ramp */

/*.....................................................
Main
.....................................................*/
void main()
{
int flag;

printf("Ready :)\n");
while (1)
{
while (!start_button());
flag = OK;
beep();

while (!stop_button())
{
while (flag == OK) flag=MoveForward(); /* Moves a little and reads status of sensors */

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 == goRamp) || (flag == seekRamp) ) /* Ramp was found */
break;
flag = OK;
}

printf("Ramp found!!\n"); /* Aligning for ramp */
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; /* Resetting values */
lastTurn = NONE;
turnCounter = 0;

while (1) /* Robot climbs ramp */
{
while (flag == OK) flag=ClimbRamp(); /* Moves a little and reads sensors */

if (flag == bumpedLeft) /* Corrects trajectory if bumped */
{AdjustRight();flag = OK;}
if (flag == bumpedRight)
{AdjustLeft();flag = OK;}
if (flag == endRamp) /* Stops moving here */
break;
}

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

Back

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