Servo Timer Service Pseudo Code Using the Gen2.

x Event Framework
*************************************************************************************
Pseudo-code for the Servo Timer Service module
Data private to the module: MyPriority, diff, servoparam,
increment, tickcount, TICS_PER_MS, SERVO_RANGE, SERVO_START,
SERVO_END, ONE_SEC

InitServoTimeService
Takes a priority number, returns True.
Initialize the MyPriority variable with the passed in parameter.
Post Event ES_Init to ServoTimeService queue (this service)
End of InitServoTimeService

PostServoTimeService
Takes an event, returns a Boolean value
Calls ES_PostToService on the passed in event with MyPriority
Returns the result of ES_PostToService
End of PostServoTimeService

RunServoTimeService
Takes an event (ThisEvent), returns an event
Initialize ReturnEvent as ES_NO_EVENT

If ThisEvent is ES_INIT:
Set diff (increment normalized to tick length) to 0
Set increment to 0
Set servoparam to 0
Set tickcount to 60
Set servoparam to SERVO_END – diff*SERVO_RANGE
Call PWMOperate_SetPulseWidthOnChannel on
servoparam*TICS_PER_MS, channel 4
Endif

If ThisEvent is ES_TIMEOUT:
Cast increment to a float, set diff to increment/tickcount
Set servoparam to SERVO_END – diff*SERVO_RANGE
Call PWMOperate_SetPulseWidthOnChannel on
servoparam*TICS_PER_MS, channel 4
If increment is equal to tickcount:
Set increment to 0
Post ES_SERVOTIMEREND event to all services
Else:
Increment the value of increment by 1
Initialize timer SERVO_TIME_TIMER to ONE_SEC
Endif
Endif

If ThisEvent is ES_SERVOSTOP:
Stop timer SERVO_TIME_TIMER
Endif

If ThisEvent is ES_SERVORESET:
Set increment to 0
Stop timer SERVO_TIME_TIMER
Set diff to 0
Set servoparam to SERVO_END – diff*SERVO_RANGE
Call PWMOperate_SetPulseWidthOnChannel on
servoparam*TICS_PER_MS, channel 4
Endif

If ThisEvent is ES_ServoStart:
Initialize timer SERVO_TIME_TIMER to ONE_SEC
Endif

Return ReturnEvent
End of RunServoTimeService