Public DesiredSpeedL As Single
Public CurrentSpeedL As Single
Public CurrentOutputL As Single
Private ek1L As Single
Private ek2L As Single

Public DesiredSpeedR As Single
Public CurrentSpeedR As Single
Public CurrentOutputR As Single
Private ek1R As Single
Private ek2R As Single

Public UserMaxOutput As Single

Private Const Kpro As Single = 0.2
Private Const T As Single = 1.0
Private Const Td As Single = 0.0
Private Const Ti As Single = 4.0

Private b0 As Single
Private b1 As Single
Private b2 As Single
'----------------------------------------------------------------------------
Private Sub DoSpeedControl( _
	ByVal DesiredSpeed As Single, _
	ByVal CurrentSpeed As Single, _
	ByRef CurrentOutput As Single, _
	ByRef ek1 As Single, _
	ByRef ek2 As Single, _
	ByVal MotorMinusPin As Byte, _
	ByVal MotorPWMPin As Byte)

	Dim MinOutput As Single
	Dim MaxOutput As Single
	Dim NewOutput As Single
	Dim SpeedError As Single

	If (DesiredSpeed > 0.0) Or ((DesiredSpeed = 0.0) And (CurrentSpeed > 0.0)) Then
		MinOutput = 0.0
		MaxOutput = UserMaxOutput
	ElseIf (DesiredSpeed < 0.0) Or ((DesiredSpeed = 0.0) And (CurrentSpeed < 0.0)) Then
		MinOutput = -UserMaxOutput
		MaxOutput = 0.0
	End If

	SpeedError = DesiredSpeed - CurrentSpeed

	NewOutput = (b0 * SpeedError) + (b1 * ek1) + (b2 * ek2) + CurrentOutput

	If NewOutput > MaxOutput Then
		NewOutput = MaxOutput
	ElseIf NewOutput < MinOutput Then
		NewOutput = MinOutput
	End If

	If NewOutput < 0.0 Then
		Call PutPin(MotorMinusPin, 1)
		Call PutPinPWM(MotorPWMPin, 1.0 + NewOutput)
	Else
		Call PutPin(MotorMinusPin, 0)
		Call PutPinPWM(MotorPWMPin, NewOutput)
	End If

	CurrentOutput = NewOutput
	ek2 = ek1
	ek1 = SpeedError

End Sub
'----------------------------------------------------------------------------
Sub InitSpeedControl()

	ek1L = 0.0
	ek1R = 0.0
	ek2L = 0.0
	ek2R = 0.0
	b0 = Kpro + (Kpro * (T/Ti)) + (Kpro * (Td/T))
	b1 = -Kpro - (2.0 * Kpro * (Td/T))
	b2 = Kpro * (Td/T)
	UserMaxOutput = 1.0

End Sub
'----------------------------------------------------------------------------
Sub SpeedControl()
	Call DoSpeedControl(DesiredSpeedL,CurrentSpeedL,CurrentOutputL,ek1L,ek2L,LeftMotorMinus,LeftMotorPWM)
	Call DoSpeedControl(DesiredSpeedR,CurrentSpeedR,CurrentOutputR,ek1R,ek2R,RightMotorMinus,RightMotorPWM)
End Sub