This service is responsible for controlling a servo. It permits setting the speed of the servo and querying it to move to a given position. This service was duplicated to both control the hourglass servo and the dispenser servo but for the sake of clarity, it was grouped as one here.
# Constants
# TICS_PER_MS assumes a 20MHz PBClk /8 = 2.5MHz clock rate
Create_constant TICS_PER_MS as 2500
Create_constant POS_180 as ((uint16_t)(0.25*TICS_PER_MS)) # Full CW
Create_constant POS_0 as ((uint16_t)(2.25*TICS_PER_MS)) # Full CCW
# These are related to how fast we move. full range of motion in 100 steps
Create_constant TICKS_PER_STEP as ((POS_0-POS_180)/100)
Create_constant DIRECTION_CW as true
Create_constant DIRECTION_CCW as false
Create_constant BASE_TIME_STEP as 10
# Associate timer channels with function
Create_constant SERVO_CHAN as 1
# Variables
Declare MyPriority as uint8_t
Declare CurrentPosition as uint16_t
Declare timeStep as uint16_t
Instantiate isMoving as false
Instantiate moveDirection as DIRECTION_CCW
Declare speed as uint8_t
Declare AimPosition as uint16_t
# Converts a position in angle to ticks
Function AngleToServoPos(uint8_t angle):
Return POS_0 + (POS_180 - POS_0) * angle / 180
Endfunction
# Moves the servo bt one step and stops it if it reaches the end
Function TakeMoveServoStep:
If DIRECTION_CCW is moveDirection:
Set CurrentPosition to CurrentPosition + TICKS_PER_STEP
If AimPosition >= CurrentPosition: # below the CCW limit?
Call PWMOperate_SetPulseWidthOnChannel(CurrentPosition, SERVO_CHAN)
Else: # no, so restore position & stop
Set CurrentPosition to CurrentPosition - TICKS_PER_STEP
Set isMoving to false
Endif
ElseIf DIRECTION_CW is moveDirection:
Set CurrentPosition to CurrentPosition - TICKS_PER_STEP
If AimPosition <= CurrentPosition: # below the CW limit?
Call PWMOperate_SetPulseWidthOnChannel(CurrentPosition, SERVO_CHAN)
Else: # no, so restore position & stop
Set CurrentPosition to CurrentPosition + TICKS_PER_STEP
Set isMoving to false
Endif
Endif
Endfunction
Function InitServoService:
# Servo variables init
Instantiate initAngle to 90
Set speed to 255
Set CurrentPosition to AngleToServoPos(initAngle)
Set AimPosition to AngleToServoPos(initAngle)
# PWM Init
Call PWMSetup_BasicConfig(5)
Call PWMSetup_SetFreqOnTimer(50, _Timer3_)
Call PWMSetup_AssignChannelToTimer(SERVO_CHAN, _Timer3_))
Call PWMOperate_SetPulseWidthOnChannel(AngleToServoPos(initAngle), SERVO_CHAN)
Call PWMSetup_MapChannelToOutputPin(SERVO_CHAN, SERVO_PIN))
# Post the initial transition event
Post_to ServoService ES_INIT
Endfunction
Function RunServoService(event):
If type of event is ES_TIMEOUT and parameter is ServoTimer and isMoving:
Call TakeMoveServoStep()
If isMoving:
Start_timer ServoTimer with duration timeStep # restart for next step
Endif
ElseIf type of event is ES_SET_SPEED:
Set speed to parameter of event
ElseIf type of event is ES_MOVE_TO:
If parameter of event <= 180:
Instantiate newPosition to AngleToServoPos(parameter of event)
If newPosition >= CurrentPosition:
Set moveDirection to DIRECTION_CCW
Else:
Set moveDirection to DIRECTION_CW
Endif
Set timeStep to 1 + BASE_TIME_STEP * (255-speed) / speed
Set AimPosition to newPosition
Start_timer ServoTimer with duration timeStep
Set isMoving to true
Endif
Endif
Endfunction