Module functions

InitThrustControl
DigitalThrust(HighOrLow, WhichThrustFan)
ApplyControl
EvaluateThrust

Module variables

LeftControl
RightControl
FwdBack
LeftRight

Begin InitMotionService
	Init PWM
	Call InitThrustControl
End InitMotionService

Begin RunMotionService
	Switch on the type of event received
		If type is ES_NEW_THRUST_CMD
			Set FwdBack to event parameter hi byte
			Set LeftRight to event parameter lo byte
			EvaluateThrust(FwdBack, LeftRight)
		End if type is ES_NEW_THRUST_CMD
	End switch on the type of event received
End RunMotionService

Begin EvaluateThrust
	Convert FwdBack and LeftRight to signed double values
	Scale SignedFwdBack and SignedLeftRight to a percent value between -100 and + 100
	Set left control and right control equal to the PercentFwdBack value
	Calculate a ScaledLeftRight value according to linearization:
		If magnitude of percent fwd back is 0, multiply ScaledLeftRight by 1
		If magnitude of percent fwd back is 100, multiply ScaledLeftRight by 0.5
		Otherwise, use linear interpolation to determine the scalar factor
	If PercentFwdBack is less than a Backup value threshhold
		Multiply ScaledLeftRight by negative 1 to correct for backing up behavior
	End if PercentFwdBack is less than a Backup value threshhold
	Subtract ScaledLeftRight from LeftControl
	Add ScaledLeftRight to RightControl
	Call ApplyControl for L_THRUST, LeftControl
	Call ApplyControl for R_THRUST, RightControl
End EvaluateThrust

Begin ApplyControl(WhichMotor, HowMuch)
	If HowMuch is 0
		SetDigitalThrust to Digital LO for WhichMotor
		SetDutyCycle to PWM_DC_LO for WhichMotor
	else if HowMuch is > 0 (forward thrust)
		SetDigitalThrust to Digital LO for WhichMotor
		SetDutyCycle to HowMuch for WhichMotor
	else (backward thrust)
		SetDigitalThrust to Digital HI for WhichMotor
		Create variable FlippedDutyCycle = MAX_DUTY - Magnitude(HowMuch)
		SetDutyCycle to FlippedDutyCycle for WhichMotor
End ApplyControl