Data private to the module: MyPriority, CurrentState, PWM0_100_A, PWM0_100_B

InitMotorService(Priority)
Declare ThisEvent
Declare/Set MyPriority to Priority
Enable PWM pins for motors
Call StopMotors function
Set ThisEvent type to ES_INIT
Return the return value of ES_PostToService with parameters MyPriotiry, ThisEvent
End of function

PostMotorService(ThisEvent)
Return the return value of ES_PostToService with parameters MyPriotiry, ThisEvent
End of function

RunMotorService(ThisEvent)
Declare ReturnEvent
Set ReturnEvent type to ES_NO_EVENT
Return ReturnEvent
End of function

DriveForward(Duty)
Call SetDirection function with parameters true, true
Call SetDirection function with parameters true, false
Call SetDuty function with parameters Duty, true
Call SetDuty function with parameters Duty*SPEED_RATIO_RL, false
End of function

DriveBackward(Duty)
Call SetDirection function with parameters false, true
Call SetDirection function with parameters false, false
Call SetDuty function with parameters Duty, true
Call SetDuty function with parameters Duty*SPEED_RATIO_RL, false
End of function

RotateRight(Duty)
Call SetDirection function with parameters true, false
Call SetDirection function with parameters false, true
Call SetDuty function with parameters Duty, true
Call SetDuty function with parameters Duty*SPEED_RATIO_RL, false
End of function

RotateLeft(Duty)
Call SetDirection function with parameters false, false
Call SetDirection function with parameters true, true
Call SetDuty function with parameters Duty, true
Call SetDuty function with parameters Duty*SPEED_RATIO_RL, false
End of function

StopMotors(void)
Call SetDirection function with parameters true, true
Call SetDirection function with parameters true, false
Call SetDuty function with parameters NO_SPEED, true
Call SetDuty function with parameters NO_SPEED, false
End of function

SetDirction(Forward, RightMotor)
If Forward is true:
If RightMotor is true:
Set polarity of left motor
Set motor direction
Else:
Set polarity of right motor
Clear motor direction
End if
Else:
If RightMotor is true:
Clear polarity of left motor
Clear motor direction
Else:
Set polarity of right motor
Set motor direction
End if
End if
End of function

SetDuty(Duty, RightMotor)
Declare/Read Load
Declare/Set DesiredHightTime based on Load and Duty values
Declare/Set NewCMP based on Load and DesiredHighTime values
If RightMotor is true:
If Duty equals NO_SPEED:
Set PWM A duty to 0
Set PWM0_100_A to 1
Else if Duty equals 100:
Set PWM A duty to 100
 Set PWM0_100_A to 1
Else if PWM0_100_A equals 1:
Set PWM A to GenA_Normal
Set PWM0_100_A to 0
End if
Set PWM of A to NewCMP
Else:
If Duty equals NO_SPEED:
Set PWM B duty to 0
Set PWM0_100_B to 1
Else if Duty equals 100:
Set PWM B duty to 1
Set PWM0_100_B to 1
Else if PWM0_100_A equals 1:
Set PWM B to GenB_Normal
Set PWM0_100_B to 0
End if
Set PWM of B to NewCMP