Data private to the module: CurrentState, FieldStrengths[], firstSwitchType, OnWire, pGain,

dGain

RunLineFollowingSM(CurrentEvent)

Declare/Set MakeTransition to false

Declare/Set NextState to CurrentState
Declare/Set EntryEventKind to ES_ENTRY
Declare/Set ReturnEvent to CurrentEvent

Switch on CurrentState:
Case DRIVING_FORWARD:
Set ReturnEvent, CurrentEvent to return value of DuringDrivingForward()
If CurrentEvent is not ES_NO_EVENT
Switch on CurrentEvent:
Case EV_LINE_HIT:
Set NextState to PID_CONTROL
Set MakeTransition to true
Set ReturnEvent to ES_NO_EVENT
Break
End Switch
End If
Break
Case PID_CONTROL:
Set ReturnEvent, CurrentEvent to return value of DuringPIDControl()
If CurrentEvent is not ES_NO_EVENT
Switch on CurrentEvent
Case EV_SWITCH_HIT:
Set FirstSwitchType to CurrentEvent parameter
Set NextState to SQUARE_UP
Set MakeTransition to true
Set ReturnEvent type to ES_NO_EVENT
Break
End Switch
End if
Break
End if
If MakeTransition is true:
Set CurrentEvent type to ES_EXIT
RunLineFollowingSM(CurrentEvent)
Set CurrentState to NextState
RunLineFollowingSM(EntryEventKind)
End if
Return ReturnEvent
End of function

StartLineFollowingSM(CurrentEvent)
If CurrentEvent type is not ES_ENTRY_HISTORY:
If OnWire:
Set CurrentState to PID_CONTROL
Else:
Set CurrentState to ENTRY_STATE
End if
End if
RunLineFollowingSM(CurrentEvent)
End of function

QueryLineFollowingSM(void)
Return CurrentState
End of function

Check4Wire(void)
Declare/Set InductorValuesLeft and InductorValuesRight to empty int arrays
Declare/Set ArrayPosition

Read and update FieldStrengths

Calculate InductorValuesRight/Left from FieldStrength readings
Increment ArrayPosition
Calculate Average Inductor Values from InductorValueArrays

Calculate FieldError as the difference between InductorValueLeft and InductorValueRight

Calculate RequestedDuty from FieldError and PID gains
Call Clamp function with parameters RequestedDuty, -21, 21
Cal SetDuty function with parameters RequestedDuty, RightMotor
End of function

Clamp(value, lowerLimit, upperLimit)

If value is above upperLimit:
Value = upperLimit
Else if value is below lowerLimit:
Value = lowerLimit
End if
End of function

DuringDrivingForward(Event)
Declare/Set ReturnEvent as Event
If Event type is ES_ENTRY or ES_ENTRY_HISTORY:
Call DriveForward function with FORWARD_DUTY parameter
Else if Event type is ES_EXIT:
Call SetDuty function with parameters 50, LeftMotor
Call SetDuty function with parameters 20, RightMotor
Start TURNING_TOWARDS_WIRE_TIMER timer
Else:
Do nothing
End if
Return ReturnEvent
End of function

DuringPIDControl(Event)
Declare/Set ReturnEvent as Event
If Event type is ES_ENTRY or ES_ENTRY_HISTORY:
Do Nothing
Else if Event type is ES_EXIT:
Call StopMotors function
Else:
If Event type is ES_TIMEOUT:
If Event parameter is TURNING_TOWARDS_WIRE_TIMER:
Call DriveForward function with parameter OFFSET_FORWARD_SPEED
Call ControlLaw function
Start CONTROL_LAW_TIMER timer
Else if Event parameter is CONTROL_LAW_TIMER:
Call ControlLaw function
Start CONTROL_LAW_TIMER timer
End if
End if
End if
Return ReturnEvent
End of function

DuringSquareUp(Event)
Set/Declare ReturnEvent to Event
If Event type is ES_ENTRY or ES_ENTRY_HISTORY:
If FirstSwitchType is true:
Call SetDuty function with parameters 0, RIGHT_MOTOR
Call SetDuty function with parameters SQUARE_UP_SPEED, LEFT_MOTOR
Else:
Call SetDuty function with parameters SQUARE_UP_SPEED, RIGHT_MOTOR
Call SetDuty function with parameters 0, LEFT_MOTOR
End if
Else if Event type is ES_EXIT:
Call StopMotors function
Else:
 Do nothing
End if
Return ReturnEvent
End of function