' ╔═════════════════════════════════════════════╗
' ║ TMOT TESTER - Variable Speed                ║
' ╟─────────────────────────────────────────────╢
' ║ Version 2                                   ║
' ╚═════════════════════════════════════════════╝

' -------------------------------------------------
'  UNCOMMENT THIS LINE IF USING THE PROTOTYPE "A"
' -------------------------------------------------
'PROTOTYPE = "A"
' -------------------------------------------------

' ------------------------ MOTOR PARAMETERS
Variable String PROTOTYPE
If PROTOTYPE = "A"
	MaxPWM = 530
	MaxRPM = 2000
Else
	MaxPWM = 400
	MaxRPM = 3800
EndIf

' ------------------------ BUTTONS and KEYS
Button 1 Text 4000 RPM
Button 2 Text 3000 RPM
Button 3 Text 2000 RPM
Button 4 Text 1000 RPM

Button 6 Text -4000 RPM
Button 7 Text -3000 RPM
Button 8 Text -2000 RPM
Button 9 Text -1000 RPM

Button 17 Text +100 RPM
Button 18 Text -100 RPM

Button 20 Text +10 RPM
Button 21 Text -10 RPM

Button 23 Text +1 RPM
Button 24 Text -1 RPM

Button 26 Text StopMotor

Key Up   ReadKeys
Key Down ReadKeys

' ----------------------------- APPS
'Load "Theremino_RS485.exe"
'Load "Theremino_SlotViewer.exe"
'Load "Theremino_SignalScope.exe"
''Load "Theremino_Hal.exe"
'Load "C:\Program Files (x86)\Pico Technology\PicoScope6\PicoScope.exe"

' ----------------------------- START
Option Speed 9
WorkingModeVelocity
Gosub Loop
Stop

' ----------------------------- STOP
Label EventStop
	StopMotor
End

Label StopMotor
	RPM = 0
	For v1 = 1 To 200
		UpdateMotorValues
		If Abs(RpmSmoothed) < 0.01
			Exit
		EndIf
		Wait Seconds 0.01
	Next
	Slot 162 = 0
	Slot 166 = 0
Return

' ----------------------------- WORKING MODE VELOCITY
Label WorkingModeVelocity
	Slot(150) = 1 ' Write-Protection ON
	Wait Seconds 0.05
	Slot(150) = 0 ' Write-Protection OFF
	Wait Seconds 0.05
	Slot(151) = 0 ' Working mode = Nothing
	Wait Seconds 0.05
	Slot(151) = 2 ' Working mode = Velocity
	Wait Seconds 0.05
	Slot(150) = 1 ' Write-Protection ON
	Wait Seconds 0.05
Return

' ----------------------------- BUTTON EXECUTION
Label 4000 RPM
	RPM = 4000
Return

Label 3000 RPM
	RPM = 3000
Return

Label 2000 RPM
	RPM = 2000
Return

Label 1000 RPM
	RPM = 1000
Return

Label -4000 RPM
	RPM = -4000
Return

Label -3000 RPM
	RPM = -3000
Return

Label -2000 RPM
	RPM = -2000
Return

Label -1000 RPM
	RPM = -1000
Return

Label +100 RPM
	RPM = RPM + 100
Return

Label -100 RPM
	RPM = RPM - 100
Return

Label +10 RPM
	RPM = RPM + 10
Return

Label -10 RPM
	RPM = RPM - 10
Return

Label +1 RPM
	RPM = RPM + 1
Return

Label -1 RPM
	RPM = RPM - 1
Return

' ----------------------------- KEYS
Label ReadKeys
	Variable Numeric JogSpeed = 100
	Select True
		'Case Key("SHIFT")
		'	JogSpeed = JogSpeed * 0.1
		Case Key("CTRL")
			JogSpeed = JogSpeed * 0.1
		Case Key("ALT")
			JogSpeed = JogSpeed * 0.01
	EndSelect
	If Key("UP")
		RPM = Int(RPM + JogSpeed)
		Print RPM
	EndIf
	If Key("DOWN")
		RPM = Int(RPM - JogSpeed)
		Print RPM
	EndIf
Return


' ╔════════════════════════════════════════════╗
' ║ LOOP and MOTOR CONTROL                     ║
' ╚════════════════════════════════════════════╝
Label Loop
	UpdateMotorValues
	Wait Seconds 0.01
Goto Loop

' ----------------------------- MOTOR CONTROL
Variable Numeric RPM
Variable Numeric RpmSmoothed
Variable Numeric MaxPWM
Variable Numeric MaxRPM
Variable Numeric VelocityLimit

Label UpdateMotorValues
	' ------------------------- update GoalPosition
	Slot(158) = Slot(110)
	' -------------------------
	RPM = Min(RPM, MaxRPM)
	RPM = Max(RPM, -MaxRPM)
	RpmSmoothed = FilterLP(RPM, 1, 8)
	' --------------------------------- speed protection
	VelocityLimit = (Slot(118) + 200) * 1.1
	If Abs(RpmSmoothed) > VelocityLimit
		RpmSmoothed = Sign(RPM) * VelocityLimit
		RPM = Int(RpmSmoothed)
	EndIf
	' ------------------------- drive the motor
	If Abs(RpmSmoothed) > 0.5
		Slot 162 = MaxPWM - Slot(120)/8
		Slot 166 = RpmSmoothed
		Print RPM
	Else
		Slot 162 = 0
		Slot 166 = 0
	EndIf
Return


