/****************************************************************************
Position.c
ME 218B Project Hats Off
Team Firefox
March 7, 2010
Description
This module is
responsible for navigating the robot between points on the
board.
****************************************************************************/
/*----------------------------- Include Files
-----------------------------*/
#include <stdio.h>
#include <termio.h>
#include <hidef.h>
#include <mc9s12e128.h>
#include <bitdefs.h>
#include <S12E128bits.h>
#include
"S12eVec.h"
#include
"ME218_E128.h"
#include
"myTimer.h"
#include
"Motors.h"
#include
"TapeSensors.h"
#include
"SPI.h"
#include
"Position.h"
/*----------------------------- Module Defines
----------------------------*/
typedef enum{ SUB_RUNNING=0, SUB_DONE=1, SUB_ERROR=2} SubProcedure_State_t;
typedef enum{ OR, //orient with a direction
TL, //track tape till hit intersection 0=don't
stop at end. 1= stop
MS, //move and stop at a given
distance
MR, //move but keep rolling at a
given distance
TA, //hit tape at
an angle, and align with it. 0=CCW 1=CW
FN //finished action sequence
}
Actions_t;
typedef enum{ EXECUTING=0, SUCCESS=1, FAIL=2} Procedure_State_t;
typedef enum{ READY, HOME, EXEC_PROC, LOAD_PROC, FAIL_PROC} InternalState_t;
/*---------------------------- Module Functions
---------------------------*/
Procedure_State_t Orient(int desHeading); //OR
Procedure_State_t TrackLine(char stopAtEnd, char delay); //TL
Procedure_State_t MoveForwardStop(float distance, char delay); //MS
Procedure_State_t MoveForwardRolling(float distance, char delay); //MR
Procedure_State_t ToTapeAlign(MOTOR_TurnDir_t dir, char delay); //TA
SubProcedure_State_t FixGearLash(char M1_Forward, char M0_Forward);
char CreateExecutePlanToNeigh(char desNeigh, char stopAtTapeEnd);
void printProcedure(Actions_t proc);
int round(float number);
/*---------------------------- Module Calibrations
------------------------*/
/*AddressBook: [Neighborhood, Heading from Neighborhood point,
Distance (inches)]
Note: Neighborhood = (0=A, 1=B, 2=C, 3=D)*/
const static int DistanceFromLine
= 12;
const static int AddressBook[8][3] = { { 0, 0, DistanceFromLine},//0
{ 0, 90, DistanceFromLine},//1
{ 1, 90, DistanceFromLine},//2
{ 1, 180, DistanceFromLine},//3
{ 2, 180, DistanceFromLine},//4
{ 2, 270, DistanceFromLine},//5
{ 3, 270, DistanceFromLine},//6
{ 3, 0, DistanceFromLine}};//7
//Target Info: [Position, Final Heading] for a given target
const static int TargetInfo[16][2] = { { 0, 315}, //0
{ 0, 38}, //1
{ 1, 50}, //2
{ 1, 130}, //3
{ 2, 65}, //4
{ 2, 130}, //5
{ 3, 142}, //6
{ 3, 218}, //7
{ 4, 135}, //8
{ 4, 216}, //9
{ 5, 228}, //10
{ 5, 308}, //11
{ 6, 241}, //12
{ 6, 307}, //13
{ 7, 320}, //14
{ 7, 40}};//15
/*PathToPos (getting to a neighborhood spot from a given
position)
Note: path array is data0, data1, data3, etc
....
Where Type 1: OR, MR, TA, TL (hit tape at angle)
Type 2: OR, MR, TL (hit tape from straight on)
Type 0: will use shortcut instead
Note: first ORient is just assumed, not included in data
structures
A B C D */
const static char PathFromPosType[8][4] = { { 0, 2, 1, 1}, //0
{ 1, 1, 1, 1}, //1
{ 1, 1, 1, 1}, //2
{ 2, 0, 1, 1}, //3
{ 1, 1, 0, 2}, //4
{ 1, 1, 1, 1}, //5
{ 1, 1, 1, 1}, //6
{ 1, 1, 2, 0} }; //7
// A B C
D
const static char PathFromPosData[8][4][2] = {{{0, 0}, { 12, 0}, {47, CCW}, {47, CW}}, //0
{{1, CW}, {1, CCW}, {52, CCW}, {52, CW}}, //1
{{1, CW}, {1, CCW}, {52, CCW}, {52, CW}}, //2
{{12, 0}, {0, 0}, {47, CCW}, {47, CW}}, //3
{{47, CCW}, {47, CW}, {0, 0}, {12, 0}}, //4
{{52, CCW}, {52, CW}, {1, CW}, {1, CCW}}, //5
{{52, CCW}, {52, CW}, {1, CW}, {1, CCW}}, //6
{{47, CCW}, {47, CW}, {12, 0}, {0, 0}}}; //7
// A B C D
const static int PathFromPosFirstOrient[8][4] = { {180, 180, 233, 233}, //0
{207, 207, 249, 249}, //1
{333, 333, 291, 291}, //2
{ 0, 0, 307, 307}, //3
{53, 53, 0, 0}, //4
{69, 69,
27, 27}, //5
{111, 111, 153, 153}, //6
{127, 127, 180, 180}};//7
// OR MS
const static float ShortcutPathFromPosToPos[8][2] = { { 135, 18.38477631}, //0
{ 315, 0}, //1
{ 225, 0}, //2
{ 45, 18.38477631}, //3
{ 315, 18.38477631}, //4
{ 135, 0}, //5
{ 45, 0}, //6
{ 225, 18.38477631}}; //7
/*---------------------------- Module Variables
---------------------------*/
//internal state variables
static int CurHeading
= 0; //due North
static char CurPosition
= 0; //one of 8 positions around the playing field
static char M0_LashForward, M1_LashForward; //used to keep track
of gear lash state
static char curAction
= 0; //used to keep track of procedure order
static char curStep
= 0; //used by procedures to keep track of current state
static char curGearLashStep
= 0; //used by the gear
last sub-procedure
static Actions_t
CurExecPlan[10] = {FN}; //set of procedures
to be executed
static float CurExecData[10] = {0}; //set of data
corresponding to the procedures
static Procedure_State_t
proc_state;
static SubProcedure_State_t
subProcState;
static InternalState_t
stateOfPosition;
static POS_StartColor_t
teamColor;
//debug variables
static char lastStep
= 0;
static char lastGearLashStep
= 0;
/*------------------------------ Module Code
------------------------------*/
//Initialization function (to be called only once at start of
code)
void InitPositioning(POS_StartColor_t startColor) {
teamColor =
startColor;
if (startColor == POS_GREEN) {
CurHeading =
170; //South
east
CurPosition =
4;
}
else {
CurHeading =
350; //North
west
CurPosition =
0;
}
stateOfPosition =
HOME;
//set initial
gear lash state (make sure to place the robot correctly)
M0_LashForward =
TRUE;
M1_LashForward =
TRUE;
}
//This function will setup the current execution plan so that
the robot will move
//to the first target, 0 or 8, depending on the team its on (only works if robot
//is at home position and not already moving somewhere)
char Pos_GoToStart(void) {
if
(stateOfPosition
== HOME) {
CurExecPlan[0] = MR; //Move forward 14 inches to clear the tape sensors of the
CurExecData[0] = 14; //starting box tape
CurExecPlan[1] = TA; //Align with the tape
CurExecData[1] = CW; //Turn clock-wise when aligning with the tape
CurExecPlan[2] = TL; //Track the
tape to the end of the T
CurExecData[2] = FALSE; //Do not stop at the end of the T
CurExecPlan[3] = MS; //Move a specific distance off the tape and stop
CurExecData[3] = DistanceFromLine;
//orient
towards the first target
if (CurPosition == 4) {
CurExecPlan[4] = OR;
CurExecData[4] = TargetInfo[8][1];
}
else {
CurExecPlan[4] = OR;
CurExecData[4] = TargetInfo[0][1];
}
CurExecPlan[5] = FN; //end of routine
CurExecData[5] = 0;
//set state
to beging executing plan
stateOfPosition =
LOAD_PROC;
curAction =
0;
return TRUE;
}
else return FALSE;
}
//This function will setup the current execution plan so that
the robot will
//navigate back to the starting box (will only work if the robot
is at a
//posiiton and not moving)
char Pos_GoToHome(void) {
const static distanceToMoveOffTape
= 17; //inches
const static distanceToMoveToBase
= 19; //inches
char
desNeigh, curNeigh, nextStep;
int
finalHead, interHead;
if (stateOfPosition != READY) {
return FALSE;
}
if (teamColor == POS_RED) {
desNeigh = 1; //home position is near B
finalHead = 135; //SouthEast
}
else {
desNeigh = 3; //home position is near D
finalHead = 315; //NorthWest
}
//check if already
in the neighborhood of home
curNeigh = (char)AddressBook[CurPosition][0];
if (curNeigh == desNeigh) { //need
to handle special
switch (CurPosition) {
case 2:
interHead =
180;
break;
case 3:
interHead =
90;
break;
case 6:
interHead =
0;
break;
case 7:
interHead =
270;
break;
}
nextStep =
0;
CurExecPlan[nextStep] = OR; //orient towards wall
CurExecData[nextStep] = AddressBook[CurPosition][1];
nextStep++;
CurExecPlan[nextStep] = MS; //move a specific
distance into home base
CurExecData[nextStep] = distanceToMoveOffTape
- 1 - DistanceFromLine;
nextStep++;
CurExecPlan[nextStep] = OR; //orient towards home
base
CurExecData[nextStep] = interHead;
nextStep++;
CurExecPlan[nextStep] = MS; //move a specific
distance into home base
CurExecData[nextStep] = distanceToMoveToBase-1;
nextStep++;
CurExecPlan[nextStep] = FN; //end
CurExecData[nextStep] = 0;
}
else { //need to move across the table
nextStep =
CreateExecutePlanToNeigh(desNeigh, TRUE);
CurExecPlan[nextStep] = OR; //orient towards home
base
CurExecData[nextStep] = finalHead;
nextStep++;
CurExecPlan[nextStep] = MS; //move a specific
distance into home base
CurExecData[nextStep] = 26.16;
nextStep++;
CurExecPlan[nextStep] = FN; //end
CurExecData[nextStep] = 0;
}
CurPosition =
0; //optimistic, assume we make it to the final destination
curAction =
0;
stateOfPosition =
LOAD_PROC;
return
TRUE;
}
//This function allows the robot to use tape to re-establish it's current position
//by setting up an execution plan the will take the robot to
tape, then return him
//to the position he currently thinks he is at
char Pos_ResetCurrentPos(int curTarget) {
int
nextStep;
// OR
MS OR TA
TL OR
const static int ResetCurrentPos[8][6] = { { 225, 25, 135, CCW, 0, 0}, //0
{ 0, 0,
207, CW, 1, 90}, //1
{ 0, 0,
333, CCW, 1, 90}, //2
{ 315, 25, 45, CW, 0, 0}, //3
{ 45, 25, 315, CCW, 0, 0}, //4
{ 0, 0, 27, CW,
1,
270}, //5
{ 0, 0,
153, CCW, 1, 270}, //6
{ 135, 25, 225, CW, 0, 0}}; //7
if
(stateOfPosition
!= READY) {
return FALSE;
}
if
(TargetInfo[curTarget][0] != CurPosition) {
return FALSE;
}
//Create
Execution Plan:
//decide if
path needs to go through neighborhood via point
if
((CurPosition == 0) || (CurPosition == 3) || (CurPosition == 4) || (CurPosition == 7)) {
nextStep =
0;
CurExecPlan[nextStep] = OR;
CurExecData[nextStep] = ResetCurrentPos[CurPosition][0]; //CurPosition tells us Row, [0] = column
nextStep++;
CurExecPlan[nextStep] = MS;
CurExecData[nextStep] = ResetCurrentPos[CurPosition][1];
nextStep++;
CurExecPlan[nextStep] = OR;
CurExecData[nextStep] = ResetCurrentPos[CurPosition][2];
nextStep++;
CurExecPlan[nextStep] = TA;
CurExecData[nextStep] = ResetCurrentPos[CurPosition][3];
nextStep++;
CurExecPlan[nextStep] = TL;
CurExecData[nextStep] = ResetCurrentPos[CurPosition][4];
}
else{
nextStep =
0;
CurExecPlan[nextStep] = OR;
CurExecData[nextStep] = ResetCurrentPos[CurPosition][2];
nextStep++;
CurExecPlan[nextStep] = TA;
CurExecData[nextStep] = ResetCurrentPos[CurPosition][3];
nextStep++;
CurExecPlan[nextStep] = TL;
CurExecData[nextStep] = ResetCurrentPos[CurPosition][4];
nextStep++;
CurExecPlan[nextStep] = OR;
CurExecData[nextStep] = ResetCurrentPos[CurPosition][5];
}
nextStep++;
CurExecPlan[nextStep] = MS; //orient to face
target
CurExecData[nextStep] = AddressBook[CurPosition][2];
nextStep++;
CurExecPlan[nextStep] = OR; //orient to face
target
CurExecData[nextStep] = TargetInfo[curTarget][1];
nextStep++;
CurExecPlan[nextStep] = FN; //end
CurExecData[nextStep] = 0;
curAction =
0;
stateOfPosition =
LOAD_PROC;
return
TRUE;
}
//This function sets up an execution plan that will take the
robot from where
//he is currently at to a desired target (will only work if the
robot is stopped
//at a position)
char Pos_GoToTarget(int desTarget) {
char
desNeigh, curNeigh, desPos, nextStep;
int
finalHead, orientOffNeigh;
float
shortCutDis;
if
(stateOfPosition
!= READY) {
return FALSE;
}
//use data
structures to get info about path planning
desPos = (char)TargetInfo[desTarget][0];
finalHead =
TargetInfo[desTarget][1];
desNeigh = (char)AddressBook[desPos][0];
curNeigh = (char)AddressBook[CurPosition][0];
//Create
Execution Plan:
//first check
if already in desired position (just need to re-orient)
if
(desPos == CurPosition) {
//already in
position, just reorient
nextStep =
0;
}
else
{ //second decide
if path needs to go through neighborhood via point
shortCutDis =
ShortcutPathFromPosToPos[CurPosition][1];
if ((shortCutDis > 0.1) && (desNeigh == curNeigh)) { //use shortcut
nextStep =
0;
CurExecPlan[nextStep] = OR; //orient to face new
position
CurExecData[nextStep] = ShortcutPathFromPosToPos[CurPosition][0];
nextStep++;
CurExecPlan[nextStep] = MS; //move a specific
distance
CurExecData[nextStep] = ShortcutPathFromPosToPos[CurPosition][1];
nextStep++;
}
else { //go through via point
orientOffNeigh =
AddressBook[desPos][1];
if ((orientOffNeigh == 0) || (orientOffNeigh
== 180)) {
//want
to roll off end of tape
nextStep =
CreateExecutePlanToNeigh(desNeigh, FALSE);
}
else {
//have to
stop at end of tape
nextStep =
CreateExecutePlanToNeigh(desNeigh, TRUE);
CurExecPlan[nextStep] = OR; //orient off
neighborhood point
CurExecData[nextStep] = AddressBook[desPos][1];
nextStep++;
}
CurExecPlan[nextStep] = MS; //move a specific
distance
CurExecData[nextStep] = AddressBook[desPos][2];
nextStep++;
}
}
CurExecPlan[nextStep] = OR; //orient to face
target
CurExecData[nextStep] = finalHead;
nextStep++;
CurExecPlan[nextStep] = FN; //end
CurExecData[nextStep] = 0;
CurPosition =
desPos; //optimistic, assume robot arrives at desired position
curAction =
0;
stateOfPosition =
LOAD_PROC;
return
TRUE;
}
//Private function that creates the first part of an execution plan
that takes the
//robot from his current position to a desired neighborhood via
point with the option
//of having the robot roll right
through the T in the tape at the end of the routine
//This function returns the next open position in the execution
plan array
char CreateExecutePlanToNeigh(char desNeigh, char stopAtTapeEnd) {
char
typeMan, data0, data1, nextStep;
int
initHead;
typeMan =
PathFromPosType[CurPosition][desNeigh];
initHead =
PathFromPosFirstOrient[CurPosition][desNeigh];
data0 =
PathFromPosData[CurPosition][desNeigh][0];
data1 =
PathFromPosData[CurPosition][desNeigh][1];
CurExecPlan[0] = OR;
CurExecData[0] = initHead;
switch
(typeMan) {
case 1: //Type 1: OR, MR, TA,
TL
CurExecPlan[1] = MR;
CurExecPlan[2] = TA;
CurExecPlan[3] = TL;
CurExecData[1] = data0;
CurExecData[2] = data1;
CurExecData[3] = stopAtTapeEnd;
nextStep =
4;
break;
case 2: //Type 2: OR, MR, TL
CurExecPlan[1] = MR;
CurExecPlan[2] = TL;
CurExecData[1] = data0;
CurExecData[2] = stopAtTapeEnd;
nextStep =
3;
break;
}
return
nextStep;
}
//This function returns the current state of the Position
module.
//This funciton needs to be called continuously because this
function is in charge
//of carrying out the execution plan
POS_State_t
Pos_getStatus (SPI_Status_t SPIstatus) {
char
isDelay;
isDelay = (SPIstatus == SPI_DELAY);
switch
(stateOfPosition) {
case READY:
return POS_STOPPED;
break;
case LOAD_PROC:
if
(isDelay && (Motors_getState() == MOTORS_STOPPED)) {
//being told to delay and motors are currently stopped,
so hold of loading next procedue
return POS_EXECUTING;
}
if
(CurExecPlan[curAction] == FN) {
stateOfPosition =
READY;
}
else
{
curStep =
0; //reset step for procedure
curGearLashStep =
0; //reset for gear lash purposes
lastStep = -1;
stateOfPosition =
EXEC_PROC;
(void)printf("Executing: ");
printProcedure(CurExecPlan[curAction]);
(void)printf(" with data,
%f. Current heading: %d, Current Position: %d\r\n", CurExecData[curAction], CurHeading,
CurPosition);
}
return
POS_EXECUTING;
break;
case EXEC_PROC:
switch (CurExecPlan[curAction]) {
case OR: //Orient - does NOT
respond to delay
proc_state =
Orient((int)round(CurExecData[curAction]));
break;
case TL: //Track Line
proc_state =
TrackLine((char)(round(CurExecData[curAction])), isDelay);
break;
case MS: //Move Forward and Stop
proc_state =
MoveForwardStop(CurExecData[curAction], isDelay);
break;
case MR: //Move Forward and Keep Rolling
proc_state = MoveForwardRolling(CurExecData[curAction], isDelay);
break;
case TA: //To Tape Aligning from some arbitrary angle
proc_state =
ToTapeAlign((MOTOR_TurnDir_t)round(CurExecData[curAction]), isDelay);
break;
default:
(void)printf("ERROR!!");
break;
}
if (proc_state == SUCCESS){
curAction++;
stateOfPosition =
LOAD_PROC;
}
if (proc_state == FAIL) {
stateOfPosition =
FAIL_PROC;
}
return POS_EXECUTING;
break;
case FAIL_PROC:
return POS_ERROR;
break;
}
}
//used to simplify debugging
void printProcedure(Actions_t proc) {
switch (proc) {
case OR: (void)printf("OR");
break;
case TL: (void)printf("TL");
break;
case MS: (void)printf("MS");
break;
case MR: (void)printf("MR");
break;
case TA: (void)printf("TA");
break;
case FN: (void)printf("FN");
break;
}
}
//used for debugging purposes, to test path planning code
void debug_setCurPosition(int newCurPos) {
stateOfPosition =
READY;
if (newCurPos < 0) newCurPos = 0;
if (newCurPos > 7) newCurPos = 7;
CurPosition = (char)newCurPos;
}
//used to help debug path planning code
void debug_printCurExecPlan(void) {
int
i=0;
(void)printf("CurExecPlan:
\r\n");
do {
(void)printf("(%d)
", i+1);
printProcedure(CurExecPlan[i]);
(void)printf(" - (%f)\r\n", CurExecData[i]);
i++;
} while (CurExecPlan[i] != FN && i<10);
(void)printf("\r\n");
}
/*----------------------------- Procedures
-----------------------------*/
//Orient the robot from his current orientation to any arbitrary
orientation
//where 0=Due North and 180 = Due South
Procedure_State_t Orient(int desHeading) {
//ASSUMES:
bot is currently stopped
//At End of
Procedure: bot is stopped
//NOTE:
motor 0 runs right wheel
static const DegreeTurnSpd
= 25;
static const GearLashTurnSpd
= 10;
static int degrees, diff; //positive values
mean CW turn
static char M1_UP, M0_UP;
static
MOTOR_TurnDir_t dir;
switch(curStep) {
case 0: //initialization,
calc degrees to turn
curGearLashStep = 0; //may need gear lash sub-procedure
if (CurHeading == desHeading) return SUCCESS;
//determine
number of degrees to turn, and in which direction
if (desHeading > CurHeading) {
diff =
desHeading - CurHeading;
if (diff < 180) {
dir =
CW;
degrees =
diff;
}
else {
dir =
CCW;
degrees =
360 - diff;
}
}
else { //CurHeading > desHeading
diff =
CurHeading - desHeading;
if (diff < 180) {
dir =
CCW;
degrees =
diff;
}
else {
dir =
CW;
degrees =
360 - diff;
}
}
if (dir == CW) {
M1_UP =
TRUE;
M0_UP =
FALSE;
}
else { //CCW
M1_UP =
FALSE;
M0_UP =
TRUE;
}
curStep++;
return EXECUTING;
break;
case 1: //fix gear lash
subProcState = FixGearLash(M1_UP, M0_UP);
if (subProcState == SUB_DONE) {
//lash gone
curStep = 11;
}
if (subProcState == SUB_ERROR) {
return FAIL;
}
return EXECUTING;
break;
case 11: //command specific
degree turn
Motors_turnStop(dir, DegreeTurnSpd, degrees);
curStep++;
return EXECUTING;
break;
case 12:
if (Motors_getState() ==
MOTORS_STOPPED) {
CurHeading =
desHeading; //update current heading
//update gear lash state (stopping the turn flipped the
gear lashes)
if (dir == CW) {//CW turn (with a CCW stop)
M0_LashForward =
TRUE;
M1_LashForward =
FALSE;
}
else {
M0_LashForward =
FALSE;
M1_LashForward =
TRUE;
}
return SUCCESS;
}
else return
EXECUTING;
break;
}
}
//Have the robot track the black line he is currently near by
adjusting the speeds
//of the wheels to maintain the front of the robot over the line
//There is quite a bit of code written here to handle situations
where the robot no
//longer can sense the black line on any of the front three tape
sensors
Procedure_State_t TrackLine(char stopAtEnd, char isDelay) {
//ASSUMES:
nothing
//At End of
Procedure: bot may or may not be stopped (depending on input variable)
//NOTE:
motor 0 runs right wheel
static const int AlignedSpeed
= 65; //55;
static const int LilOffSpeed
= 61; //51;
static const int WayOffSpeed
= 25; //25;
static const int FinishSpeed
= 20; //20
static const int LostSearchTurnSpd
= 15; //doesn't work at 25
static const int LostSearchDegree
= 65;
static const int LostAlignSpeed
= 25;
static const float DistanceFrontCenter
= 4.5; //inches
static
TAPE_TrackStatus_t curTrackState, lastTrackState;
static
MOTOR_TurnDir_t lostGuessDir = CW;
switch(curStep) {
case 0: //if bot is stopped,
fix gear lash before continuing
lostGuessDir = CW;
if (Motors_getState() ==
MOTORS_STOPPED) {
curGearLashStep = 0;
curStep++;
}
else curStep
= 10; //start tracking
return EXECUTING;
break;
case 1: //fix gear lash for
going forward
subProcState = FixGearLash(TRUE, TRUE);
if (subProcState == SUB_DONE) curStep = 10; //gear lash gone
if (subProcState == SUB_ERROR) return FAIL;
return EXECUTING;
break;
case 10: //figure out which
speed to run at
curTrackState = Tape_getTrackStatus();
//check
if should be delaying
if (isDelay) {
Motors_moveStraight(0); //stop motors
return EXECUTING;
}
switch(curTrackState) {
case INTERSECTION:
if (stopAtEnd) curStep = 30;
else curStep
= 60;
break;
case ALIGNED:
Motors_moveStraight(AlignedSpeed);
curStep++;
break;
case LIL_LEFT:
Motors_moveDiff(AlignedSpeed, LilOffSpeed);
lostGuessDir = CW;
curStep++;
break;
case WAY_LEFT:
Motors_moveDiff(AlignedSpeed, WayOffSpeed);
lostGuessDir = CW;
curStep++;
break;
case LIL_RIGHT:
Motors_moveDiff(LilOffSpeed, AlignedSpeed);
lostGuessDir = CCW;
curStep++;
break;
case WAY_RIGHT:
Motors_moveDiff(WayOffSpeed, AlignedSpeed);
lostGuessDir = CCW;
curStep++;
break;
case LOST:
Motors_moveStraight(0); //stop motors
M0_LashForward =
FALSE; //update gear
lash state
M1_LashForward =
FALSE;
curStep = 20;
break;
case ERROR: return FAIL; break;
}
lastTrackState = curTrackState;
return EXECUTING;
break;
case 11: //driving with some
sort of track status = [Way_Left, Aligned, Way_Right]
curTrackState = Tape_getTrackStatus();
//check
if should be delaying
if (isDelay) {
Motors_moveStraight(0); //stop motors
curStep =
10;
return EXECUTING;
}
if (lastTrackState != curTrackState) { //something changed, need to adjust speeds
curStep = 10;
}
return EXECUTING;
break;
case 20: //lost, need to find line
to track after confirming wheels have stopped moving
if (Motors_getState() ==
MOTORS_STOPPED) {
//fix gear lash before turning to search for line
curGearLashStep = 0;
curStep++;
}
return EXECUTING;
break;
case 21: //fix gear lashing
before starting turn
if (lostGuessDir == CW) subProcState = FixGearLash(TRUE, FALSE);
else subProcState
= FixGearLash(FALSE, TRUE);
if (subProcState == SUB_DONE) { //gear lash gone
//start turning in lostGuessDir dir
Motors_resetPosition();
Motors_turn(lostGuessDir, LostSearchTurnSpd);
curStep++;
}
if (subProcState == SUB_ERROR) return FAIL;
return EXECUTING;
break;
case 22: //currently searching
in guessLostDir
if (Motors_getDegreesTurned()
> LostSearchDegree) { //didn't find line
//start turning other direction looking for line
//can avoid fixing gear lash here, it will self correct
when both wheels switch direction
Motors_resetPosition();
if (lostGuessDir == CW ) lostGuessDir = CCW;
else lostGuessDir
= CW;
Motors_turn(lostGuessDir, LostSearchTurnSpd);
curStep++;
}
if (Tape_getTrackStatus() ==
ALIGNED) { //found line!
Motors_moveStraight(0); //stop motors
//update gear lash status
if (lostGuessDir == CW) {//CW turn (with a CCW stop)
M0_LashForward =
TRUE;
M1_LashForward =
FALSE;
}
else {
M0_LashForward =
FALSE;
M1_LashForward =
TRUE;
}
curStep = 40; //need to align with
line
}
return EXECUTING;
break;
case 23: //currently turning
in guessLostDir
if (Motors_getDegreesTurned()
> 2*LostSearchDegree) { //didn't find line
//didn't find the line, stop motors, give up, error out
Motors_moveStraight(0);
//update gear lash status
if (lostGuessDir == CW) {//CW turn (with a CCW stop)
M0_LashForward =
TRUE;
M1_LashForward =
FALSE;
}
else {
M0_LashForward =
FALSE;
M1_LashForward =
TRUE;
}
curStep=50;
}
if (Tape_getTrackStatus() ==
ALIGNED) { //found line!
Motors_moveStraight(0); //stop motors
//update gear lash status
if (lostGuessDir == CW) {//CW turn (with a CCW stop)
M0_LashForward =
TRUE;
M1_LashForward =
FALSE;
}
else {
M0_LashForward =
FALSE;
M1_LashForward =
TRUE;
}
curStep = 40; //need to align with
line
}
return EXECUTING;
break;
case 60://reached intersection in NO STOP mode, drive forward
until one of the off center tapes read
if (Tape_isBlack(CENTER_LEFT) || Tape_isBlack(CENTER_RIGHT)) {
return
SUCCESS;
}
return EXECUTING;
break;
case 30: //reached
intersection in STOP mode, drive forward until one of the off center tapes read
Motors_moveStraight(FinishSpeed);
curStep++;
return EXECUTING;
break;
case 31: //driving past
intersection, wait till either of the off center tapes read
if (Tape_isBlack(CENTER_LEFT)) {
Motors_moveDiff(0,FinishSpeed);
curStep = 32;
}
if (Tape_isBlack(CENTER_RIGHT)) {
Motors_moveDiff(FinishSpeed, 0);
curStep = 33;
}
return EXECUTING;
break;
case 32: //waiting on right
side to reach black tape
if (Tape_isBlack(CENTER_RIGHT)) {
Motors_moveStraight(0);
curStep = 34;
}
return EXECUTING;
break;
case 33: //waiting on left
side to reach black tape
if (Tape_isBlack(CENTER_LEFT)) {
Motors_moveStraight(0);
curStep = 34;
}
return EXECUTING;
break;
case 34: //waiting for bot to
stop, before returning success
if (Motors_getState() ==
MOTORS_STOPPED) {
//update status of gear lashing
M0_LashForward =
FALSE;
M1_LashForward =
FALSE;
return SUCCESS;
}
return EXECUTING;
break;
case 40: //found line with front
tape after being lost, waiting for motors to stop
if (Motors_getState() ==
MOTORS_STOPPED) {
//fix gear lash before turning to search for line
curGearLashStep = 0;
curStep++;
}
return EXECUTING;
break;
case 41: //fix gear lash
before going forward
subProcState = FixGearLash(TRUE, TRUE);
if (subProcState == SUB_DONE) { //gear lash gone
//move forward 4 in which will bring middle tape up to
the line
Motors_moveStraightStop(LostAlignSpeed, DistanceFrontCenter);
M0_LashForward =
FALSE;
M1_LashForward =
FALSE;
curStep++;
}
if (subProcState == SUB_ERROR) return FAIL;
return EXECUTING;
break;
case 42: // center over top of
line, need to wait for motor to stop, then spin to align front
if (Motors_getState() ==
MOTORS_STOPPED) {
//fix gear lash before turning to search for line
curGearLashStep = 0;
curStep++;
}
return EXECUTING;
break;
case 43: //fix gear lashing
before starting turn
if (lostGuessDir == CW) subProcState = FixGearLash(TRUE, FALSE);
else subProcState
= FixGearLash(FALSE, TRUE);
if (subProcState == SUB_DONE) { //gear lash gone
//start turning opposite to lostGuessDir dir
Motors_resetPosition();
if (lostGuessDir == CW) Motors_turn(CCW, LostSearchTurnSpd);
else Motors_turn(CW, LostSearchTurnSpd);
curStep++;
}
if (subProcState == SUB_ERROR) return FAIL;
return EXECUTING;
break;
case 44: //currently searching
opposite to guessLostDir
if (Motors_getDegreesTurned()
> LostSearchDegree) { //didn't find line
//start turning other direction looking for line
//no need to fix gear lash here, motors will self correct
gear lash when switching directions
Motors_resetPosition();
Motors_turn(lostGuessDir, LostSearchTurnSpd);
curStep++;
}
if (Tape_getTrackStatus() ==
ALIGNED) { //found line!
Motors_moveStraight(0); //stop motors
//update gear lash status
if (lostGuessDir == CCW) {//CCW turn because going OPPOSITE to lostGuessDir (with a
CCW stop)
M0_LashForward =
TRUE;
M1_LashForward =
FALSE;
}
else {
M0_LashForward =
FALSE;
M1_LashForward =
TRUE;
}
curStep = 46; // wait for stop,
then start tracking again
}
return EXECUTING;
break;
case 45: //currently turning
in guessLostDir direction looking for line
if (Motors_getDegreesTurned()
> 2*LostSearchDegree) { //didn't find line
//didn't find the line, stop motors, give up, error out
Motors_moveStraight(0);
//update gear lash status
if (lostGuessDir == CW) { //CW turn (with a CCW stop)
M0_LashForward =
TRUE;
M1_LashForward =
FALSE;
}
else {
M0_LashForward =
FALSE;
M1_LashForward =
TRUE;
}
curStep = 50;
}
if (Tape_getTrackStatus() ==
ALIGNED) { //found line!
Motors_moveStraight(0); //stop
motors
if (lostGuessDir == CW) { //CW
turn (with a CCW stop)
M0_LashForward =
TRUE;
M1_LashForward =
FALSE;
}
else {
M0_LashForward =
FALSE;
M1_LashForward =
TRUE;
}
curStep = 46; //wait for motors to
stop, then start over
}
return EXECUTING;
break;
case 46:
if (Motors_getState() ==
MOTORS_STOPPED) {
curGearLashStep = 0;
curStep = 1; //fix gear lash and
start procedure again
}
return EXECUTING;
break;
case 50: //waiting to stop,
before erroring out
if (Motors_getState() ==
MOTORS_STOPPED) {
return FAIL;
}
return EXECUTING;
break;
}
}
//Have the robot come to a complete
stop after traveling a given distance
Procedure_State_t MoveForwardStop(float DesDistance, char isDelay) {
//Assumes
nothing (robot could be moving or not
//At End of
Procedure: robot will be stopped
//NOTE:
motor 0 runs right wheel
static const OpenFieldSpeed
= 65;
static char gotDelayed = FALSE;
static float delayedDis = 0;
static float localDesDistance;
switch(curStep) {
case 0: //if bot is stopped,
fix gear lash before continuing
gotDelayed = FALSE;
delayedDis = 0;
localDesDistance = DesDistance;
if (Motors_getState() ==
MOTORS_STOPPED) {
curGearLashStep = 0;
curStep++;
}
else curStep
= 2; //initialize
return EXECUTING;
break;
case 1: //fix gear lash for
going forward
subProcState = FixGearLash(TRUE, TRUE);
if (subProcState == SUB_DONE) curStep++; //gear lash gone
if (subProcState == SUB_ERROR) return FAIL;
return EXECUTING;
break;
case 2: //initialize
Motors_moveStraightStop(OpenFieldSpeed, localDesDistance); //encoder counts are reset
curStep++;
return EXECUTING;
break;
case 3: //waiting for bot to
stop
if (isDelay) {
gotDelayed =
TRUE;
Motors_moveStraight(0); //stop motors
delayedDis =
Motors_getNetDistance();
return EXECUTING;
}
if (gotDelayed) {
gotDelayed =
FALSE;
localDesDistance =
localDesDistance -
delayedDis;
Motors_moveStraightStop(OpenFieldSpeed, localDesDistance);
return EXECUTING;
}
if (Motors_getState() ==
MOTORS_STOPPED) {
M0_LashForward =
FALSE;
M1_LashForward =
FALSE;
return SUCCESS;
}
return EXECUTING;
break;
default:
return FAIL;
break;
}
}
//Have the robot move forward a given distance,
but do not come to a stop
Procedure_State_t MoveForwardRolling(float DesDistance, char isDelay) {
//Assumes
nothing (robot may or may not be moving)
//END: bot
is moving
//NOTE:
motor 0 runs right wheel
static const OpenFieldSpeed
= 65;
static char gotDelayed = FALSE;
static float delayedDis = 0;
static float localDesDistance;
switch(curStep) {
case 0: //if bot is stopped,
fix gear lash before continuing
gotDelayed =
FALSE;
delayedDis =
0;
localDesDistance =
DesDistance;
if (Motors_getState() ==
MOTORS_STOPPED) {
curGearLashStep = 0;
curStep++;
}
else curStep
= 2; //initialize
return EXECUTING;
break;
case 1: //fix gear lash for
going forward
subProcState = FixGearLash(TRUE, TRUE);
if (subProcState == SUB_DONE) curStep++; //gear lash gone
if (subProcState == SUB_ERROR) return FAIL;
return EXECUTING;
break;
case 2: //initialize
Motors_resetPosition();
Motors_moveStraight(OpenFieldSpeed);
curStep++;
return EXECUTING;
break;
case 3: //waiting for bot to
reach desired distance
if (isDelay) {
gotDelayed =
TRUE;
Motors_moveStraight(0);
return EXECUTING;
}
if (gotDelayed) {
gotDelayed =
FALSE;
Motors_moveStraight(OpenFieldSpeed);
return EXECUTING;
}
if (Motors_getNetDistance()
> DesDistance) {
return SUCCESS;
}
return EXECUTING;
break;
default:
return FAIL;
break;
}
}
//Drive forward until tape is detected, then
align to the tape by turning
//in a given direction
Procedure_State_t ToTapeAlign(MOTOR_TurnDir_t dir, char isDelay) {
//ASSUMES:
bot is stopped, and facing either E or W
//END: bot
will be stopped, new orientation
//NOTE:
motor 0 runs right wheel
static const OpenFieldSpeed
= 65;
static const NearingLineSpeed
= 40;
static const FastTurnSpeed
= 30;
static const SlowTurnSpeed
= 10;
static char gotDelayed;
switch(curStep) {
case 0: //if bot is stopped,
fix gear lash before continuing
if (Motors_getState() ==
MOTORS_STOPPED) {
curGearLashStep = 0;
curStep++;
}
else curStep
= 2; //initialize
return EXECUTING;
break;
case 1: //fix gear lash
before going forward
subProcState = FixGearLash(TRUE, TRUE);
if (subProcState == SUB_DONE) curStep++; //gear lash gone
if (subProcState == SUB_ERROR) return FAIL;
return EXECUTING;
break;
case 2: //initialize
Motors_moveStraight(OpenFieldSpeed);
curStep++;
return EXECUTING;
break;
case 3: //driving forward at
open field speed
if (isDelay) {
gotDelayed =
TRUE;
Motors_moveStraight(0);
return EXECUTING;
}
if (gotDelayed) {
gotDelayed =
FALSE;
Motors_moveStraight(OpenFieldSpeed);
return EXECUTING;
}
if (Tape_getTrackStatus() !=
LOST) { //front
tape sensors read something, slow down
Motors_moveStraight(NearingLineSpeed); //slow motors
curStep++;
}
return EXECUTING;
break;
case 4://driving forward at
slow speed
if (isDelay) {
gotDelayed =
TRUE;
Motors_moveStraight(0);
return EXECUTING;
}
if (gotDelayed) {
gotDelayed =
FALSE;
Motors_moveStraight(OpenFieldSpeed);
return EXECUTING;
}
if (Tape_isBlack(CENTER)) {
Motors_moveStraight(0); //stop motors
M0_LashForward =
FALSE; //update gear
lash state
M1_LashForward =
FALSE;
curStep++;
}
return EXECUTING;
break;
case 5: //stopping, wait for
bot to stop completely
if (Motors_getState() ==
MOTORS_STOPPED) {
curStep++;
curGearLashStep = 0; //prepare for fixing gear lash
}
return EXECUTING;
break;
case 6: //fix gear lashing
if (dir == CW) subProcState = FixGearLash(TRUE, FALSE);
else subProcState
= FixGearLash(FALSE, TRUE);
if (subProcState == SUB_DONE) { //gear lash gone
curStep++;
Motors_turn(dir, FastTurnSpeed);
}
if (subProcState == SUB_ERROR) return FAIL;
return EXECUTING;
break;
case 7: //turning in place looking
for black on any of the front tape sensors
if (Tape_getTrackStatus() !=
LOST) {
curStep++;
Motors_turn(dir, SlowTurnSpeed);
}
return EXECUTING;
break;
case 8: //turning in place
looking for black on front center tape
if (Tape_getTrackStatus() ==
ALIGNED) {
Motors_moveStraight(0); //stop
motors
if (dir == CW) { //CW
turn (with a CCW stop)
M0_LashForward =
TRUE;
M1_LashForward =
FALSE;
}
else {
M0_LashForward =
FALSE;
M1_LashForward =
TRUE;
}
curStep = 20;
}
return EXECUTING;
break;
case 20:
if (Motors_getState() ==
MOTORS_STOPPED) {
//update CurHeading
if (CurHeading > 180) { //bot was facing west at start of this procedure
if (dir == CW) CurHeading = 0;
else CurHeading
= 180;
}
else {
if (dir == CW) CurHeading = 180;
else CurHeading
= 0;
}
return SUCCESS;
}
return EXECUTING;
break;
default:
return ERROR;
break;
}
}
/*----------------------------- SubProcedures
-----------------------------*/
//Compare desired wheel rotation with current gear lash state
and turn motors
//accordingly to prevent gear lash from affecting the next move
SubProcedure_State_t FixGearLash(char M1_Forward, char M0_Forward) {
//ASSUMES:
bot is currently stopped
//End: bot
is stopped
//NOTE:
motor 0 runs right wheel
static const NumTicksGearLash
= 20;
static const GearLashTurnSpd
= 10;
static int spd0, spd1;
/*if (curGearLashStep != lastGearLashStep)
(void)printf("CurGearLashStep: %d\r\n", curStep);
lastGearLashStep
= curGearLashStep;*/
switch(curGearLashStep) {
case 0: //initialization,
determine speeds to correct lash
spd0 =
0;
spd1 =
0;
Motors_resetPosition();
if (M1_Forward) {
if(M1_LashForward) spd1 = 0;
else spd1
= GearLashTurnSpd; //drive forward
through gear lash
}
else { //want M1 backwards
if(M1_LashForward) spd1 = -1*GearLashTurnSpd; //drive backward through gear lash
else spd1
= 0;
}
if (M0_Forward) {
if(M0_LashForward) spd0 = 0;
else spd0
= GearLashTurnSpd; //drive forward
through gear lash
}
else { //want M0 backwards
if(M0_LashForward) spd0 = -1*GearLashTurnSpd; //drive backward through gear lash
else spd0
= 0;
}
if (spd1 != 0) { //watch motor 1 to see when lash has been overcome
curGearLashStep = 11;
Motors_moveDiff(spd1, spd0);
}
else {
if (spd0 != 0) { //watch motor 0 to see when lash has been overcome
curGearLashStep = 12;
Motors_moveDiff(spd1, spd0);
}
else { //good to go!
curGearLashStep = 0;
return SUB_DONE;
}
}
return SUB_RUNNING;
break;
case 11: //watching motor 1
for completed gear lash
if ((Motors_getEncoderCount(1) > NumTicksGearLash) || (Motors_getEncoderCount(1) < -1*NumTicksGearLash)) {
Motors_moveStraight(0); //stop motors, will overshoot
curGearLashStep = 20;
}
return SUB_RUNNING;
break;
case 12: //watching motor 0
for completed gear lash
if ((Motors_getEncoderCount(0) > NumTicksGearLash) || (Motors_getEncoderCount(0) < -1*NumTicksGearLash)) {
//(void)printf("Number of
encoder ticks 0,1: %d, %d\r\n", Motors_getEncoderCount(0),
Motors_getEncoderCount(1));
Motors_moveStraight(0); //stop motors, will overshoot
curGearLashStep = 20;
}
return SUB_RUNNING;
break;
case 20: //stopping motors
if (Motors_getState() ==
MOTORS_STOPPED) {
curGearLashStep = 0;
//update lash state
if (spd1 > 0) M1_LashForward = TRUE;
else M1_LashForward
= FALSE;
if (spd0 > 0) M0_LashForward = TRUE;
else M0_LashForward
= FALSE;
return SUB_DONE;
}
return SUB_RUNNING;
break;
}
}
//round a float to a integer
int round(float number)
{
return (number >= 0) ? (int)(number + 0.5) : (int)(number - 0.5);
}