Tail Service

# 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.7*TICS_PER_MS)) # Full CW

Create_constant POS_0 as ((uint16_t)(2.25*TICS_PER_MS)) # Full CCW

Create_constant MAX_POS as POS_0 # Oscillation max

Create_constant MIN_POS as POS_180 # Oscillation min

# 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_TAIL_CHAN as 3

# 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

Declare isLooping as bool

# Converts a position in angle to ticks

Function AngleToServoTailPos(uint8_t angle):

Return POS_0 + (POS_180 - POS_0) * angle / 180

Endfunction

# Moves the servo by one step and stops it if it reaches the end

# Or flip it if it is looping

Function TakeMoveTailStep:

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

If isLooping:

Set AimPosition to MIN_POS

Set moveDirection to DIRECTION_CW

Else:

Set isMoving to false

Endif

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

If isLooping:

Set AimPosition to MAX_POS

Set moveDirection to DIRECTION_CCW

Else:

Set isMoving to false

Endif

Endif

Endif

Endfunction

Function InitTailService:

# Servo variables init

Instantiate initAngle to 90

Set speed to 85

Set timeStep to 1 + BASE_TIME_STEP * (255-speed) / speed

Set isLooping to false

Set CurrentPosition to AngleToServoHourGlassPos(initAngle)

Set AimPosition to AngleToServoHourGlassPos(initAngle)

# PWM Init

Call PWMSetup_BasicConfig(5)

Call PWMSetup_SetFreqOnTimer(50, _Timer3_)

Call PWMSetup_AssignChannelToTimer(SERVO_TAIL_CHAN, _Timer3_))

Call PWMOperate_SetPulseWidthOnChannel(AngleToTailPos(initAngle), SERVO_TAIL_CHAN)

Call PWMSetup_MapChannelToOutputPin(SERVO_TAIL_CHAN, HOUGLASS_SERVO_PIN))

# Post the initial transition event

Post_to TailService ES_INIT

Endfunction

Function RunTailService(event):

If type of event is ES_TIMEOUT and parameter is TailTimer and isMoving:

Call TakeMoveTailStep()

If isMoving:

Start_timer TailTimer with duration timeStep # restart for next step

Endif

ElseIf type of event is ES_TAIL_SET_SPEED:

Set speed to parameter of event

Set timeStep to 1 + BASE_TIME_STEP * (255-speed) / speed

ElseIf type of event is ES_TAIL_START_OSCILLATING and not isLooping:

Set speed to parameter of event

Set timeStep to 1 + BASE_TIME_STEP * (255-speed) / speed

Set moveDirection to DIRECTION_CCW

Set AImPosition to AX_POS

Set isLooping to true

Set isMoving to true

Start_timer TailTimer with duration timeStep

ElseIf type of event is ES_TAIL_MOVE_TO:

If parameter of event <= 180:

Instantiate newPosition to AngleToTailPos(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 TailTimer with duration timeStep

Set isMoving to true

Endif

Endif

Endfunction