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

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

' ------------------------ MOTOR PARAMETERS
Variable String PROTOTYPE
If PROTOTYPE = "A"
	MaxPWM = 530
	MaxRPM = 2000
Else
	MaxPWM = 300   ' 400
	MaxRPM = 4000
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 11 Text StopMotor
Button StopMotor Color Gold

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



Key Up   ReadKeys
Key Down ReadKeys

' ----------------------------- APPS
Load "Theremino_RS485.exe"
Load "Theremino_SlotViewer.exe"
Load "Theremino_SignalScope1.exe"
Load "Theremino_SignalScope2.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.1
			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

Label UpdateMotorValues
	' ------------------------- update GoalPosition
	Slot(158) = Slot(110)
	' -------------------------
	RPM = Min(RPM, MaxRPM)
	RPM = Max(RPM, -MaxRPM)
	RpmSmoothed = FilterLP(RPM, 1, 8)
	
	
	ExecutePID
	
	'RpmSmoothed = RpmSmoothed - RPMdecrease
	RpmSmoothed = RpmSmoothed / (1 + RPMdecreaseSmoothed)
	
	' ------------------------- drive the motor
	If Abs(RpmSmoothed) > 0.5
		Slot 162 = 0
		Slot 166 = RpmSmoothed
	Else
		Slot 162 = 0
		Slot 166 = 0
	EndIf
	
	Print RPM + PadLeft(Str(RpmSmoothed), 8)
Return


' ╔════════════════════════════════════════════╗
' ║ PID                                        ║
' ╚════════════════════════════════════════════╝
Variable Numeric PidKP = 0.05  '3.0
Variable Numeric PidKI = 0     '5.0
Variable Numeric PidKD = -0.0002

Variable Numeric PidP
Variable Numeric PidI
Variable Numeric PidD
Variable Numeric PidErr
Variable Numeric PidNewTime
Variable Numeric PidOldTime
Variable Numeric PidLoopTime
Variable Numeric PidOldD

Variable Numeric RPMdecrease
Variable Numeric RPMdecreaseSmoothed
'Variable Numeric PwmSmoothed
Variable Numeric MotorCurrent

Label ExecutePID
	MotorCurrent = Slot(120)
	' --------------------------------- Loop time
	PidNewTime = ElapsedTime
	PidLoopTime = PidNewTime - PidOldTime
	PidOldTime = PidNewTime
	' --------------------------------- Error
	PidErr = MotorCurrent - 300
	PidErr = Max(PidErr, 0)
	' --------------------------------- P
	PidP = PidKP * PidErr
	' --------------------------------- I
	If Abs(PidErr) < 15000 And PidErr > 1
		PidI = PidI + PidErr * PidLoopTime
	Else
		PidI = 0
	EndIf
	' --------------------------------- D (based on Current)
	'PidD = (PidOldD - MotorCurrent) / PidLoopTime
	'PidOldD = MotorCurrent
	' --------------------------------- D (based on Err)
	PidD = (PidErr - PidOldD) / PidLoopTime
	PidOldD = PidErr
	' --------------------------------- Output
	RPMdecrease = PidP + PidKI * PidI + PidKD * PidD
	' --------------------------------- Limit the speed
	RPMdecrease = Min(RPMdecrease, 4000)
	RPMdecrease = Max(RPMdecrease, 0)
	RPMdecreaseSmoothed = FilterLP(RPMdecrease, 1, 2)
Return

'


