' ╔═════════════════════════════════════════════╗
' ║ EGD1 - Production - V2                      ║
' ╚═════════════════════════════════════════════╝

' ------------------------------------- MOTOR PARAMETERS
MaxTORQUE = 200	'  200
MaxACC =   1000	' 1000
MaxRPM =   1000	' 1000

PidKP  =   50	' 50
PidKI  =   50	' 50
PidKD  =   50	'  0
DeadBand =  1	'  5

ZeroSpeedCurrent = 0
StartSensorMode = 0

Kreduction = 41

' ------------------------------------- SLOTS
Variable Numeric s_ActualPos   = 110
Variable Numeric s_IdSensor    = 127
Variable Numeric s_WriteProt   = 150
Variable Numeric s_WorkingMode = 151
Variable Numeric s_GoalPos     = 158
Variable Numeric s_GoalTorque  = 162
Variable Numeric s_GoalAcc     = 164
Variable Numeric s_GoalSpeed   = 166
Variable Numeric s_PidP        = 168
Variable Numeric s_PidI        = 169
Variable Numeric s_PidD        = 170
Variable Numeric s_DeadBand    = 171
Variable Numeric s_ZeroCurrent = 172
'Variable Numeric s_MagSens_WorkMode  = 188

' ------------------------------------- BUTTONS and KEYS
Button 1 Label ChangeSpeed
Button ChangeSpeed Color Yellow

Button 3 Label Turn1
Button Turn1 Text "1 giro"
Button Turn1 Color GreenYellow
Button 4 Label Turn10
Button Turn10 Text "10 giri"
Button Turn10 Color GreenYellow
Button 6 Label Goto-Zero
Button Goto-Zero Text "Torna a zero"
Button Goto-Zero Color Gold

Button 17 Label EnableConnection
Button EnableConnection Text "Connetti"
Button EnableConnection Color GreenYellow
Button 18 Label DisableConnection
Button DisableConnection Text "Disconnetti"
Button DisableConnection Color Gold

Button 20 Label WriteFirmware
Button WriteFirmware Text "Scrivi il firmware"
Button WriteFirmware Color Yellow9
Button 22 Label SendReset
Button SendReset Text "Invia reset"
Button SendReset Color Yellow9

Button 24 Label TestMagSensB
Button TestMagSensB Text "Test MagSensB"
Button TestMagSensB Color Yellow9

Button 38 Label FpsValue
Button 39 Label VoltValue
Button 40 Label TempValue
Button 42 Label Mag1Value
Button 43 Label IdSensValue

Button 33 Label SerValue
Button 34 Label DevValue
Button 35 Label FirmValue
Button 36 Label BootValue


Key Up   ReadKeys
Key Down ReadKeys

' ------------------------------------- APPS
Load "Theremino_Motors.exe"
Load "Theremino_SlotViewer.exe"
'Load "Theremino_Hal.exe"


' ------------------------------------- START
Option Speed 9
ShowSpeed
WaitGoodCommunication
' ------------------------------------- LOOP
Gosub Loop
Stop

Label TestCommunication
	If Slot(56) < 10
		WaitGoodCommunication
	EndIf
Return

Label WaitGoodCommunication
	For v1 = 1 To Infinity
		If Slot(56) > 50
			v2 = v2 + 1
		Else
			v2 = 0
		EndIf
		If v2 > 50
			Beep 1660 40
			InitAllParams
			Controls SetBackColor Gray7
			Exit
		EndIf
		Print " >>>>>>> ERRORE DI COMUNICAZIONE <<<<<<<"
		Controls SetBackColor OrangeRed
		Wait Seconds 0.01
	Next
Return

Label InitAllParams
	Slot s_GoalTorque = 0
	Slot s_GoalAcc = 0
	Slot s_GoalSpeed = 0
	Slot s_PidP = 0
	Slot s_PidI = 0
	Slot s_PidD = 0
	Slot s_DeadBand = 0
	Slot s_ZeroCurrent = 0
	Position = Slot(s_ActualPos)
	Destination = Position
	Slot(s_GoalPos) = Destination
	Slot s_GoalTorque = MaxTORQUE
	Slot s_GoalAcc = MaxACC
	Slot s_GoalSpeed = MaxRPM
	Slot s_PidP = PidKP
	Slot s_PidI = PidKI
	Slot s_PidD = PidKD
	Slot s_DeadBand = DeadBand
	Slot s_ZeroCurrent = ZeroSpeedCurrent
	Wait Seconds 0.1
	SetWorkingMode(1) ' 1 = Position
Return

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

Label StopMotor
	Destination = Position
	UpdateMotorValues
	SetWorkingMode(0)
Return

' ------------- SET WORKING MODE 1=Position 2=Speed
Label SetWorkingMode
	Variable Numeric SWM1
	Slot(s_WriteProt) = 1   ' Write-Protection ON
	Wait Seconds 0.05
	Slot(s_WriteProt) = 0   ' Write-Protection OFF
	Wait Seconds 0.05
	Slot(s_WorkingMode) = 0 ' Working mode = 0
	Wait Seconds 0.05
	Slot(s_WorkingMode) = SWM1 ' Working mode
	Wait Seconds 0.05
	Slot(s_WriteProt) = 1   ' Write-Protection ON
Return


' ------------------------------------- BUTTON EXECUTION
Label WorkingMode
	Variable Numeric WKM1
	WKM1 = Slot(s_WorkingMode) + 1
	WKM1 = Mod(WKM1 , 2)
	SetWorkingMode(WKM1)
Return

Label ChangeSpeed
	Select Slot(s_GoalSpeed)
		Case 500
			Slot(s_GoalSpeed) = 1000
		Case 1000
			Slot(s_GoalSpeed) = 1500
		Case 1500
			Slot(s_GoalSpeed) = 2000
		CaseElse
			Slot(s_GoalSpeed) = 500
	EndSelect
	ShowSpeed
	MaxRPM = Slot(s_GoalSpeed)
Return

Label ShowSpeed
	Button ChangeSpeed Text "RPM = " + Slot(s_GoalSpeed)
Return

' ------------------------------------- MOVE BUTTONS
Label Turn1
	Destination = Destination + 360 * Kreduction
Return
Label Turn10
	Destination = Destination + 3600 * Kreduction
Return

Label Goto-Zero
	Destination = 0
Return

' ------------------------------------- COMMENDS TO MOTORS APP
Label EnableConnection
	SendCommand(Slot_Commands)("EnableConnection")
Return
Label DisableConnection
	SendCommand(Slot_Commands)("DisableConnection")
Return

Label WriteFirmware
	SendCommand(Slot_Commands)("WriteFirmware")
Return
Label SendReset
	SendCommand(Slot_Commands)("SendReset")
Return

' =============================================================
'  SEND COMMAND (default timeout = 0.5 Sec)
' =============================================================
Variable Numeric Slot_Commands = 50

Label SendCommand
	Variable Numeric SC1       '<-- Parameter Destinat.-Slot
	Variable String  SC2       '<-- Parameter Command-String
	Variable Numeric SC3 = 0.5 '<-- The timeout in seconds
	Variable Numeric SC4
	SlotText SC1 = SC2
	For SC4 = 0.1 To SC3 Step 0.1
		If SlotText(SC1) = ""
			Exit
		EndIf
		Wait Seconds 0.1
	Next
Return


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

Label PrintValues
	s1 = "Destination: " + Format(Destination, "0.00") + CRLF
	s1 = s1 + "   Position: " + Format(Position, "0.00")
	Print s1
	
	Button FpsValue Text "FPS  " + Slot(56)
	Button VoltValue Text "Volt  " + Str(Slot(122), "0.0")
	Button TempValue Text "Temp  " + Slot(124) + " °C"
	
	Button Mag1Value Text "MagSensA  " + Slot(130)
	Button IdSensValue Text "ID Sens  " + Int(Slot(s_IdSensor)/40) + "%"
	
	Button SerValue Text "Serial " + Slot(100)
	Button DevValue Text "Device " + Slot(103) + "." + Slot(104)
	Button FirmValue Text "Firmware " + Slot(105) + "." + Slot(106)
	Button BootValue Text "Bootloader " + Slot(107)	
Return

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


' ------------------------------------- MOTOR CONTROL
Variable Numeric MaxTORQUE
Variable Numeric MaxACC
Variable Numeric MaxRPM
Variable Numeric Position
Variable Numeric Destination
Variable Numeric PidKP
Variable Numeric PidKI
Variable Numeric PidKD
Variable Numeric DeadBand
Variable Numeric ZeroSpeedCurrent
Variable Numeric StartSensorMode
Variable Numeric Kreduction

Label UpdateMotorValues
	Slot(s_GoalPos) = Destination
	Position = Slot(s_ActualPos)
Return

Label MoveBy
	Variable Numeric MB1
	Destination = Destination + MB1
	UpdateMotorValues
Return


'BOOKMARKS 

'EDITEDLINES -1 1 76 8 -1 4 -1 3 -1 4 -1 5 281 6 280 7 -1 8 -1 9 -1 10 

