Position.c

void InitPositioning(POS_StartColor_t startColor)

Store startColor as the teamColor

If startColor is Green

Set CurHeading to 170

Set CurPosition to 4

Else

Set CurHeading to 350

Set CurPosition to 0

Set stateOfPosition to HOME

Set initial gear lash as both motors set forward

char Pos_GoToStart(void)

If stateOfPosition Not Equal to HOME

Return False

Using data structures, set finalHead to orientation to first target

Set curExecPlan to: MR, TA, TL, MS, OR, FN

Set curExecData to: 14, CW, False, DistanceFromLine, finalHead, 0

Set stateOfPosition to LOAD_PROC

Set curAction to zero

return TRUE

char Pos_GoToHome(void)

If stateOfPosition is not READY

return False

Set desiredNeighborhood and finalHeading based off teamColor

Use data structures to determine neighborhood of curPosition, firstOrient

If curNeighborhood is desNeighborhood

Set interHead based off CurPosition

Set curExecPlan to: OR, MS, OR, MS, FN

Set curExecData to: firstOrient, 5, interHead, 18, 0

Else

CreateExecutePlanToNeigh(desNeighborhood, True);

Append to curExecPlan: OR, MS, FN

Append to curExecData: finalHead, 26.16, 0

Set CurPosition to zero

Set curAction to zero

Set stateOfPosition to LOAD_PROC

return True

char Pos_ResetCurrentPos(int desTarget)

If stateOfPosition is not READY

return False

Use data structures to determine desPos given desTarget

If desPos != CurPosition

return False

If desPos == 0 || desPos == 3 || desPos == 4 || desPos == 7

Set curExecPlan to: OR, MS, OR, TA, TL

Set curExecData using data structures based of desPos

Else

Set curExecPlan to: OR, TA, TL, OR

Set curExecData using data structures based of desPos

Append to curExecPlan: MS, OR, FN

Append to curExecData using data structures based of desTarget and desPos

Set curAction to zero

Set stateOfPosition to LOAD_PROC

return True

char Pos_GoToTarget(int desTarget)

If stateOfPosition is not READY

return False

Use data structures to determine desPos, finalHead, desNeigh, curNeigh

If desPos is CurPosition

Set curExecPlan to: OR, FN

Set curExecData to: finalHead, 0

Else

Use data structures to see if a shortcut path is available

If shortcut is available

Set curExecPlan to: OR, MS, OR, FN

Set curExecData using data structures based off CurPosition

Else

Use data structures to determine if robot should stop at neighborhood via point

If robot should stop at via point

CreateExecutePlanToNeigh(desNeigh, True);

Append to curExecPlan: OR

Append to curExecData using data structures based off desPos

Else

CreateExecutePlanToNeigh(desNeigh, False);

Append to curExecPlan: MS, OR, FN

Append to curExecPlan: distanceFromLine, finalHead, 0

Set CurPosition to desPosition

Set curAction to zero

Set stateOfPosition to LOAD_PROC

char CreateExecutePlanToNeigh(char desNeigh, char stopAtTapeEnd)

Set curExecPlan using data structures based of desNeigh and curPosition

Set curExecData using data structures based of desNeigh and curPosition

Return the length of the execution plan

POS_State_t Pos_getStatus (SPI_Status_t SPIstatus)

isDelay = (SPIstatus == DELAY)

switch (stateOfPosition)

case READY:

Return STOPPED

case LOAD_PROC:

If CurExecPlan[curAction] is FN

stateOfPosition = READY

Else

Reset flags and variables to prepare for executing a new procedure

stateOfPosition = EXEC_PROC

Return POS_EXECUTING

case EXEC_PROC:

Call procedure function depending on value of curExecPlan[curAction]

If procedure is a success

Increment curAction

stateOfPosition = LOAD_PROC

If procedure fails

stateOfPosition = FAIL_PROC

Return POS_EXECUTING

case FAIL_PROC:

Return POS_ERROR

Procedure Orient:

Step 1: Determine degree of rotation based of curHeading and desHeading

Step 2: Fix Gear Lash

Step 3: Motors_turnStop()

Step 4: When motors stop, set new gear lash state, and return SUCCESS

Procedure TrackLine:

Step 1: Fix Gear Lash

Step 2: Move forward with motor speeds based off state of front three tape sensors

Step 3: If input from front three tape sensors changes, go back to step 2

Step 4: If front three tape sensors all detect tape, must be at T, return SUCCESS

If all three tape sensors fail to read the black tape

Execute search procedure to turn back and forth to find tape and realign with it

If fail to find line, return FAIL

Procedure TapeAlign:

Step 1: Fix Gear Lash

Step 2: Motors_moveStraight(fast)

Step 3: When front three tape sensors detect anything, Motors_moveStraight(slow)

Step 4: When middle tape sensor senses tape, stop motors

Step 5: Turn until front three tape sensors are aligned with tape, return SUCCESS

Procedure MoveForwardRolling:

Step 1: Fix Gear Lash

Step 2: Reset encoder counts, Motors_moveStraight(fast)

Step 3: When distance traveled equals desDistance, return SUCCESS

Procedure MoveForwardRolling:

Step 1: Fix Gear Lash

Step 2: Reset encoder counts, Motors_moveStraightStop(fast)

Step 3: When motors come to a stop, return SUCCESS