'╔═══════════════════════════════════════════════════════════╗
'║     Name: Winch_03.txt                  Date: 24 Set 2022 ║
'║   Author: Leonardo                                        ║
'║    Scope: Test all the functionality of winch             ║
'╠═══════════════════════════════════════════════════════════╣
'║Hardware: Standard FeeTech STS3215                         ║
'╟───────────────────────────────────────────────────────────╢
'║    Note: FeeTech at 7.5V                                  ║
'║                                                           ║
'╚═══════════════════════════════════════════════════════════╝
Option Speed 9
Beep "2500 20, 1500 100"
Load images\Winch.jpg

' ╔════════════════════════════════════════════╗
' ║ SLOT TEXT for COMMANDS and RESPONSES       ║
' ╚════════════════════════════════════════════╝
Variable Numeric Slot_CommandsToMotors   = 50
Variable Numeric Slot_CommandsToCobot    = 51
Variable Numeric Slot_ResponsesFromCobot = 52
Variable Numeric Slot_ExternalCommands   = 53

' ╔════════════════════════════════════════════╗ █   █ █▀▀█ █▀▀▀█
' ║ VARIABLE INITIALIZATIONS & ASSIGNEMENTS    ║ █   █ █▄▄█ █▄▄▄█
' ╚════════════════════════════════════════════╝  ▀▄▀  █  █ █  ▀▄
Variable Numeric Slot_EncoderData  =  1
Variable Numeric Slot_EncoderBtn   = 10

Variable Numeric Slot_Analog_X     =  4
Variable Numeric Slot_Analog_Y     =  5

Variable Numeric Slot_LedR         =  7
Variable Numeric Slot_LedG         =  8
Variable Numeric Slot_LedB         =  9

Variable Numeric Slot_M1           = 101

Variable Numeric Slot_SignalScope  = 13
Variable Numeric Slot_RS485_FPS    = 56

' ╔════════════════════════════════════════════╗ █▀▀▄ █  █ ▀▀█▀▀
' ║ BUTTONS                                    ║ █▀▀█ █  █   █
' ╚════════════════════════════════════════════╝ █▄▄█ █▄▄█   █
Button 1 Label ExecCobot
Button ExecCobot  Text "Exec Cobot"
Button ExecCobot  Color GreenYellow

Button 2 Label StopCobot
Button StopCobot  Text "Stop Cobot"
Button StopCobot  Color Yellow

Button 4 Label SetCenterHere
Button SetCenterHere Text "Set Center Here"
Button SetCenterHere Color Orange

Button 5 Label CalibrateToZero
Button CalibrateToZero  Text "Calibrate to Zero"
Button CalibrateToZero  Color Orange

'Button 6 Label CalibrateMotor1To180
'Button CalibrateMotor1To180  Text "Example: Calibrate" + CRLF + "Motor1 To 180"
'Button CalibrateMotor1To180  Color Orange

Button  7 Label CurrentMeter

Button  8 Label "MotorTemp"
Button MotorTemp Color Salmon

Button 10 Label Knob
Button Knob Text "Knob"
Button Knob Color Aquamarine

Button 15 Label CloseAll
Button CloseAll  Text "CLOSE ALL"
Button CloseAll  Color Red


' ╔════════════════════════════════════════════╗ █   █▀▀█ █▀▀█ █▀▀▄
' ║ LOAD APPS                                  ║ █   █  █ █▄▄█ █  █
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄█ █  █ █▄▄█

Load Theremino_Motors.exe
WaitApplicationsStarted
' -------------------------------------
'  MOTORS must be active before Cobot
' -------------------------------------
Load Theremino_HAL.exe
Load Theremino_SlotViewer.exe
Load Theremino_SlotViewer_M1.exe
Load Theremino_SlotViewer_Strings.exe
Load Theremino_SignalScope.exe
Load Theremino_Cobot.exe
WaitApplicationsStarted
Load Hide

' ------------------------- GOTO HOME - Command to Cobot
'WaitCobotReady
'SendCommand(Slot_CommandsToCobot)(GotoHome)


' ╔════════════════════════════════════════════╗ █   █▀▀█ █▀▀█ █▀▀█
' ║ LOOP                                       ║ █   █  █ █  █ █▄▄█
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄█ █▄▄█ █
Label Loop
	CurrentMeter
Goto Loop
Stop

' ╔════════════════════════════════════════════╗
' ║ ENABLE/DISABLE BUTTONS                     ║
' ╚════════════════════════════════════════════╝
Label EnableAllButtons
	For v1 = 1 To 16
		Button v1 Enabled
	Next
Return

Label DisableAllButtons
	For v1 = 1 To 16
		Button v1 Disabled
	Next
Return


' ╔════════════════════════════════════════════╗
' ║  WAIT COBOT READY                          ║
' ╚════════════════════════════════════════════╝
Label WaitCobotReady
	Variable Numeric WCR1
	For WCR1 = 1 To Infinity
		Wait Seconds 0.05
		If SlotText(Slot_ResponsesFromCobot) = "Ready"
			Exit
		EndIf
	Next
Return

' ╔════════════════════════════════════════════╗
' ║  WAIT APPS STARTED                         ║
' ╚════════════════════════════════════════════╝
Label WaitApplicationsStarted
	Slot(Slot_RS485_FPS) = 0
	Wait Slot(Slot_RS485_FPS) > 10
	Wait Seconds 0.1
Return



' ╔════════════════════════════════════════════╗ █▀▀▀ ▀▀█▀▀ █▀▀█ █▀▀█
' ║ STOP EVENT (when pressing stop button)     ║ ▀▀▀█   █   █  █ █▄▄█
' ╚════════════════════════════════════════════╝ ▄▄▄█   █   █▄▄█ █

Label EventStop
	StopCobotAndGotoHome
End

Label StopCobotAndGotoHome
	StopCobot
	WaitMotorsNotMoving
	GotoSafePosition
Return

Label GotoSafePosition
	SendCommand(Slot_CommandsToCobot)(GotoHome)
	WaitMotorsNotMoving
Return

Label WaitMotorsNotMoving
	Variable Numeric WMN1
	Variable Numeric WMN2
	Wait Seconds 0.1
	For WMN1 = 0 To Infinity
		WMN2 = Slot(Slot_M1 + 6)
		'WMN2 = WMN2 + Slot(Slot_M2 + 6)
		'WMN2 = WMN2 + Slot(Slot_M3 + 6)
		If WMN2 = 0
			Exit For
		EndIf
	Next
Return

' ╔════════════════════════════════════════════╗ █▀▀ █   █▀▀█ █▀▀▀ █▀▀▀
' ║ CLOSE ALL      (when pressing X on form)   ║ █   █   █  █ ▀▀▀█ █▀▀
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄ █▄▄█ ▄▄▄█ █▄▄▄
Label CloseAll
	StopCobotAndGotoHome
	Load CloseAll
End


' ╔═════════════════════════╗  █▀▀▀ █▀▀█ █▄ ▄█   █▀▀  █▀▀▀ █▀▀█ █▀█
' ║  COMMANDS FROM COBOT    ║  █    █  █ █ ▀ █   █▀   █    █  █ █▀▀█
' ╚═════════════════════════╝  █▄▄▄ █▄▄█ █   █ ▄ █  ▄ █▄▄▄ █▄▄█ █▄▄█

Label Event_ExternalCommands
	' ------------------------------------------------------ Do not print lines containing ''
	Print " Exec: " + CommandText + CRLF
	' ------------------------------------------------------
	Select CommandText
			' ---------------------------------------------- ACTIONS
		Case "Beep"
			Beep 880-100
			ClearCommand
		Case "Pause"
			Wait Seconds GetSeparatedString(CommandText, 1)
			ClearCommand
		'Case "StartPump"
		'	StartPump
		'	ClearCommand
		'Case "StopPump"
		'	StopPump
		'	ClearCommand
		CaseElse
			Controls OpenTextBox
			Print CRLF + " Unrecognized command: " + CommandText + CRLF
			Beep
			ClearCommand
			Stop								'
	EndSelect
Return

Label ClearCommand
	SlotText(Slot_ExternalCommands) = ""
Return


' =============================================================
'  SEND COMMAND (default timeout = 0.5 Sec)
' =============================================================
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


' =============================================================
'  COMMANDS to Cobot
' =============================================================
Label ExecCobot
	SendCommand(Slot_CommandsToCobot)(EnableRepeat)
	SendCommand(Slot_CommandsToCobot)(SelectLine 1)
	SendCommand(Slot_CommandsToCobot)(StartExecution)
Return

Label StopCobot
	SendCommand(Slot_CommandsToCobot)(StopExecution)
Return

' =============================================================
'  CALIBRATION COMMANDS to Motors and Cobot
' =============================================================
Label SetCenterHere
	SendCommand(Slot_CommandsToMotors)("ResetAllCalibrationValues")
	Wait Seconds 0.5
	SendCommand(Slot_CommandsToMotors)("ResetAllMultiturnValues")
	Wait Seconds 0.5
	SendCommand(Slot_CommandsToCobot) ("SetAllMotorsCenterHere")
	Wait Seconds 0.5
	SendCommand(Slot_CommandsToCobot) ("CopyPositionsToDestinations")
Return

Label CalibrateToZero
	SendCommand(Slot_CommandsToMotors)("Calibrate Motor 1 To 0")
	SendCommand(Slot_CommandsToMotors)("Calibrate Motor 2 To 0")
	SendCommand(Slot_CommandsToMotors)("Calibrate Motor 3 To 0")
	SendCommand(Slot_CommandsToMotors)("Calibrate Motor 4 To 0")
	SendCommand(Slot_CommandsToMotors)("Calibrate Motor 5 To 0")
	'SendCommand(Slot_CommandsToMotors)("Calibrate Motor 6 To 0")
	Wait Seconds 0.5
	SendCommand(Slot_CommandsToCobot) ("CopyPositionsToDestinations")
Return

Label CalibrateMotor1To180
	SendCommand(Slot_CommandsToMotors)("Calibrate Motor 1 To 180")
	SendCommand(Slot_CommandsToCobot)("CopyPositionsToDestinations")
Return


' ╔════════════════════════════════════════════╗ █▀▀▀ █  █ █▀█
' ║  S U B R O U T I N E   A R E A             ║ ▀▀▀█ █  █ █▀▀█
' ╚════════════════════════════════════════════╝ ▄▄▄█ █▄▄█ █▄▄█
Label CurrentMeter
	Variable Numeric mA
	mA = Slot(108) + Slot(208) + Slot (308) ' <-- Motors slots
	mA = FilterLP(mA, 0.1, 1) '<-- Filter 0.5 seconds / 1 pole
	Button CurrentMeter Text Str(mA, "0 mA")
	'Slot 999 = mA
Return

Label Knob
	If Granularity <> 0
		Granularity = 0
	Else
		BlueOn
		DisableAllButtons
		Button Knob Enabled
		v73 = Slot(Slot_EncoderData)
		Granularity = 1
		For v1 = 1 To Infinity
			Slot(101) = (v73 - Slot(Slot_EncoderData)) * Granularity
			SetGranularity
			If Granularity = 0
				Exit
			EndIf
		Next
		EnableAllButtons
	EndIf
Return

Variable Numeric Granularity = 0
Label SetGranularity
	Variable Numeric SG1
	Variable Numeric SG2 = Slot(Slot_EncoderBtn)
	If SG1 <= 500 And SG2 > 500
		If Granularity = 1
			Button Knob Color Flashing6 LightCyan Aqua
			Granularity = 0.1
		Else
			Button Knob Color Flashing6 LightCyan Green
			Granularity = 1
		EndIf
	EndIf
	SG1 = SG2
Return

' ╔════════════════════════════════════════════╗ █   █▀▀ █▀▄
' ║ LED COLORS                                 ║ █   █▀  █  █
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄ █▄▄█
Label RedOn
	Slot Slot_LedR = 1000
	Slot Slot_LedG = 0
	Slot Slot_LedB = 0
Return

Label MagentaOn
	Slot Slot_LedR = 1000
	Slot Slot_LedG = 0
	Slot Slot_LedB = 1000
Return

Label YellowOn
	Slot Slot_LedR = 900
	Slot Slot_LedG = 400
	Slot Slot_LedB = 0
Return

Label GreenOn
	Slot Slot_LedR = 0
	Slot Slot_LedG = 1000
	Slot Slot_LedB = 0
Return

Label BlueOn
	Slot Slot_LedR = 0
	Slot Slot_LedG = 0
	Slot Slot_LedB = 1000
Return

Label WhiteOn
	Slot Slot_LedR = 1000
	Slot Slot_LedG = 1000
	Slot Slot_LedB = 1000
Return

Label RgbOff
	Slot Slot_LedR = 0
	Slot Slot_LedG = 0
	Slot Slot_LedB = 0
Return

Label RedBlink
	RedOn
	WhiteOn
Return


'WATCH Granularity
