DOGSharkMotions
Simple service controlling the shark tail, mouth, and LED peripherals


InitDOGSharkMotions
        Save away priority of service
Set FinInc to 0
Set position of servo to FinInc of FinDuty arry using set pulse width
        Tail should start off not wagging, in neutral position
        Set module level state variable CurrentState as StoppingWag
        Set wag timer in case we need cycles to get back to neutral position
        Start with mouth closed
End of InitDOGSharkMotions


RunDOGSharkMotions
Takes ES_Event, returns ES_Event

Switch based on CurrentState
	CurrentState is Waiting2Wag
			Switch based on EventType of ThisEvent
				Case is ES_PAIRD
					Set CurrentState to Wagging
					Call IncrementFinInc function
					Start WAG_TIMER to TAIL_STEP_PRD of 40 ms
					End of case    
			End of switch

	CurrentState is Wagging
		Switch based on EventType of ThisEvent
			Case is ES_TIMEOUT
				Call IncrementFinInc function
				Start WAG_TIMER to TAIL_STEP_PRD of 40 ms
				End of case        
			Case is ES_UNPAIRED
				Call IncrementFinInc function
				Start WAG_TIMER to TAIL_STEP_PRD of 40 ms
				Change CurrentState to StoppingWag
				End of case 
		End of switch 

	CurrentState is StoppingWag
		Switch based on EventType of ThisEvent
			Case is ES_TIMEOUT
				If FinInc not equal to zero
					Call IncrementFinInc function
					Start WAG_TIMER to TAIL_STEP_PRD of 40ms
				Else if FinInc equal to zero
					Change CurrentState to Waiting2Wag
				End of case    
		End of switch
	End of switch (CurrentState)
End of RunDOGSharkMotions

**********Public Functions******************************

OpenSharkMouth
Takes nothing, returns nothing

Set PWM pulse for mouth servo to open mouth value 
End of OpenSharkMouth



CloseSharkMouth
Takes nothing, returns nothing

Set PWM pulse for mouth servo to closed mouth value 
End of CloseSharkMouth


**********Private Functions******************************

IncFinPosition
	If FinInc is less than or equal to MaxFinDutyIndex
			Increment FinInc by 1
	Else
			Set FinInc equal to zero
	Endif 
	Set tail servo pwm pulse width to the FinInc index of FinDuty array
End of IncFinPosition