'╔═══════════════════════════════════════════════════════════╗
'║     Name: T-Join_Early_07.txt           Date: 20 Dec 2024 ║
'║   Author: Leonardo                                        ║
'║    Scope: New geared reduction, DC motor, Hall encoder    ║
'╠═══════════════════════════════════════════════════════════╣
'║Hardware: L9110s-H_MOSFET 2 motor drivers                  ║
'║          JGY_370_Motor_reduction                          ║
'║          MT6710 only analog                               ║
'╟───────────────────────────────────────────────────────────╢
'║    Note: First run                      Date: 15 Dec 2024 ║
'║                                                           ║
'╚═══════════════════════════════════════════════════════════╝
Option Speed 9
Load images\Geared_motor_reduction.jpg
'Beep "2500 200, 1500 200"
TTS SelectVoice "ENG"
'TTS SelectVoice "ZHO"
TTS SetVolume 100      '100
'TTS SetSpeed -1
TTS Speak "Run"

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

' ╔════════════════════════════════════════════╗ █▀▀▀ █   █▀▀█ ▀▀█▀▀
' ║ SLOT INITIALIZATIONS & VARIABLE            ║ ▀▀▀█ █   █  █   █
' ╚════════════════════════════════════════════╝ ▄▄▄█ █▄▄ █▄▄█   █
Numeric Slot_MotorA_A     =  1
Numeric Slot_MotorA_B     =  2
Numeric Slot_AnalogAngle  =  6
Numeric Slot_LedR         =  7
Numeric Slot_LedG         =  8
Numeric Slot_LedB         =  9
Numeric Slot_SignalScope  = 13

Numeric Slot_Dest1        = 101
Numeric Slot_Pos1         = 102

Numeric Slot_MasterStatus = 908

' ╔═══════════════════════════════════╗ █▀▀▄ █  █ ▀▀█▀▀ ▀▀█▀▀ █▀▀█ █▄ █ ▄▀▀▀
' ║ BUTTONS                           ║ █▀▀█ █  █   █     █   █  █ █ ▀█ ▀▀▀▄
' ╚═══════════════════════════════════╝ █▄▄█ █▄▄█   █     █   █▄▄█ █  █ ▄▄▄▀
Button  1 Label HOME
Button HOME Color PaleTurquoise

Button  2 Label CCW
Button  3 Label CW
Button  4 Label ALT
Button  8 Label GoTo5
Button  9 Label GoTo100
Button 10 Label GoTo200
Button 11 Label GoTo355
Button 12 Label PIDTest


'Button 8 Label InputPosition
'Button InputPosition Text "Input Position"
'Button InputPosition Color GreenYellow

'Button 6 Label EtnTest
'Button EtnTest Text "Etn Test"
'Button EtnTest Color Gold
'
'Button 7 Label DisableMotor
'Button DisableMotor Text "DISABLE MOTOR"
'Button DisableMotor Color PaleTurquoise
'
'Button 8 Label EnableMotor
'Button EnableMotor Text "ENABLE MOTOR"
'Button EnableMotor Color PaleTurquoise

'Button 9 Label RedOn
'Button RedOn Color Red
'Button 10 Label GreenOn
'Button GreenOn Color Green
'Button 11 Label YellowOn
'Button YellowOn Color Yellow
'Button 12 Label BlueOn
'Button BlueOn Color MediumBlue
'Button 13 Label WhiteOn
'Button GreenOn Color Green
'Button 14 Label RgbOff
'Button RgbOff Color Black


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

DisableAllButtons

' ╔════════════════════════════════════════════╗ █   █▀▀█ █▀▀█ █▀▀▄
' ║ LOAD APPS                                  ║ █   █  █ █▄▄█ █  █
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄█ █  █ █▄▄█
Load Theremino_HAL.exe
Load Theremino_SlotViewer.exe
Load Theremino_SignalScope.exe
Load Theremino_Cobot.exe

' ╔═════════════════════════════════╗   █▀▀▀▀  ▀▀█▀▀  █▀▀▀█  █▀▀▀█  ▀▀█▀▀
' ║  EXECUTION START FROM HERE      ║   ▀▀▀▀█    █    █▀▀▀█  █▀█▀▀    █
' ╚═════════════════════════════════╝   ▄▄▄▄█    █    █   █  █  ▀▄    █
YellowOn
Load Hide
Controls ClearTextBox
ALT
YellowOn
EnableAllButtons
Slot(Slot_SignalScope) = 1000
Slot 0 = Recognize
GoTo5
PIDTest

Loop
Stop

'Label EventTimer
'	If Right(DecodeCommandSlot(0), 5) = "ERROR"
'		Print DecodeCommandSlot(0)
'		TTS Speak "Master error"
'		ALT
'		End
'	EndIf
'Return

' ╔════════════════════════════════════════════╗ █   █▀▀█ █▀▀█ █▀▀█
' ║ LOOP                                       ║ █   █  █ █  █ █▄▄█
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄█ █▄▄█ █
Label Loop
	ExecutePID
	Slot(Slot_Pos1) = Slot(Slot_AnalogAngle)
	Wait Seconds 0.01
Goto Loop

' ╔════════════════════════════════════════════╗
' ║ STATUS MASTER QUERY                        ║
' ╚════════════════════════════════════════════╝
Label StatusMaster
	String Master
	If Slot(Slot_MasterStatus) <> 1000
		Master = "OFF"
		Print "MASTER OFF  "
		Wait Seconds 0.2
	Else
		Master = "ON"
		'Print "MASTER ON  "
	EndIf
Return

' ╔════════════════════════════════════════════╗
' ║ 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
	Numeric WCR1
	For WCR1 = 1 To Infinity
		Wait Seconds 0.05
		If SlotText(Slot_ResponsesFromCobot) = "Ready"
			Exit
		EndIf
	Next
Return

' ╔════════════════════════════════════════════╗ █▀▀▀ ▀▀█▀▀ █▀▀█ █▀▀█
' ║ STOP EVENT (when pressing stop button)     ║ ▀▀▀█   █   █  █ █▄▄█
' ╚════════════════════════════════════════════╝ ▄▄▄█   █   █▄▄█ █
Label EventStop
	ALT
	RedOn
	EnableAllButtons
	GoTo5
	Window Sizable
	Slot(Slot_SignalScope) = 0
End

' ╔════════════════════════════════════════════╗ █▀▀ █   █▀▀█ █▀▀▀ █▀▀▀
' ║ CLOSE ALL      (when pressing X on form)   ║ █   █   █  █ ▀▀▀█ █▀▀
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄ █▄▄█ ▄▄▄█ █▄▄▄
Label CloseAll
	ALT
	RedOn
	GoTo5
Return

' =============================================================
'  SEND COMMAND (default timeout = 0.5 Sec)
' =============================================================
Label SendCommand			'(Destination Slot)(Command String)
	Numeric SC1
	String  SC2
	Numeric SC3 = 0.7 '<-- The timeout in seconds was 0.5
	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

Label ClearCommand
	SlotText(Slot_ExternalCommands) = ""
Return

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

Label ExecCobot
	SendCommand(Slot_CommandsToCobot)("DisableRepeat")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	SendCommand(Slot_CommandsToCobot)("StartExecution")
Return

Label StopCobot
	SendCommand(Slot_CommandsToCobot)("StopExecution")
Return

Label ArrowUpCobot
	SendCommand(Slot_CommandsToCobot)("ArrowUp")
Return

Label ArrowDownCobot
	SendCommand(Slot_CommandsToCobot)("ArrowDown")
Return

Label ExecuteCobotFromSelectedLine
	SendCommand(Slot_CommandsToCobot)("StartExecution")
Return

Label CorrectSelectedPosition
	If Master = "OFF"
		Return
	EndIf
	SendCommand(Slot_CommandsToCobot)("CorrectSelectedPosition")
Return

Label DeleteSelectedLine
	SendCommand(Slot_CommandsToCobot)("DeleteSelectedLine")
Return

Label AddNewPosition
	SendCommand(Slot_CommandsToCobot)("AddNewPosition")
Return

Label EnableRepeat
	SendCommand(Slot_CommandsToCobot)("EnableRepeat")
Return

Label DisableRepeat
	SendCommand(Slot_CommandsToCobot)("DisableRepeat")
Return








' ╔════════════╗ █▀▀█ █▀▀▀ ▀▀█▀▀ █ █▀▀█ █▄ █ █▀▀▀
' ║  Actions   ║ █▄▄█ █      █   █ █  █ █ ▀█ ▀▀▀█
' ╚════════════╝ █  █ █▄▄▄   █   █ █▄▄█ █  █ ▄▄▄█
Label CCW                       'down
	ALT
	Slot(Slot_MotorA_A)    = 0
	Slot(Slot_MotorA_B)    = 10000
Return

Label CW                       'up
	ALT
	Slot(Slot_MotorA_A)    = 1000
	Slot(Slot_MotorA_B)    = 0
Return

Label ALT
	Slot(Slot_MotorA_A)    = 1000
	Slot(Slot_MotorA_B)    = 1000
	Wait Seconds 0.1
	Slot(Slot_MotorA_A)    = 0
	Slot(Slot_MotorA_B)    = 0
	Wait Seconds 0.5
Return

Label GoTo5
	Slot(Slot_Dest1) = 5
Return

Label GoTo100
	Slot(Slot_Dest1) = 100
Return

Label GoTo200
	Slot(Slot_Dest1) = 200
Return

Label GoTo355
	Slot(Slot_Dest1) = 355
Return

' ╔════════════════════════════════════════════╗
' ║ HOME SET                                   ║
' ╚════════════════════════════════════════════╝
Label HOME
	Numeric Ho1
	Numeric SH1
	For Ho1 = 1 To Infinity
		SH1 = Slot(Slot_AnalogAngle)
		Slot(Slot_MotorA_A)    = 0
		Slot(Slot_MotorA_B)    = 100
		If SH1 <= 15
			Exit
		EndIf
		Wait Seconds 0.1
	Next
	ALT
Return

' ╔════════════╗ █▀▀█ █▀▀▀ ▀▀█▀▀ █ █▀▀█ █▄ █ █▀▀▀
' ║  Actions   ║ █▄▄█ █      █   █ █  █ █ ▀█ ▀▀▀█
' ╚════════════╝ █  █ █▄▄▄   █   █ █▄▄█ █  █ ▄▄▄█
Label PIDTest
	If Master = "OFF"
		Return
	EndIf
	TTS SpeakWait "Pid test"
	SendCommand(Slot_CommandsToCobot)("EnableRepeat")
	SendCommand(Slot_CommandsToCobot)("LoadSequence T-PID_Test.txt")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	SendCommand(Slot_CommandsToCobot)("StartExecution")
Return

Label NailsEN
	If Master = "OFF"
		Return
	EndIf
	TTS SpeakWait "Goto Home."
	SendCommand(Slot_CommandsToCobot)("DisableRepeat")
	SendCommand(Slot_CommandsToCobot)("LoadSequence TWeld_NailsEN.txt")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	'SendCommand(Slot_CommandsToCobot)("StartExecution")
Return





' ╔═══════════════════════════╗ █▀▀█ █ █▀▀▄
' ║  PID                      ║ █▄▄█ █ █  █
' ╚═══════════════════════════╝ █    █ █▄▄█
Variable Numeric PidKP = 70
Variable Numeric PidKI = 10
Variable Numeric PidKD = 10

'70 50 10 for 6V
'70 10 10 for 10

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

Numeric Drive1

Label ExecutePID
	' --------------------------------- Loop time
	PidNewTime = ElapsedTime
	PidLoopTime = PidNewTime - PidOldTime
	PidOldTime = PidNewTime
	' --------------------------------- Error
	PidErr = Slot(Slot_Dest1) - Slot(Slot_Pos1)
	' --------------------------------- P
	PidP = PidKP * PidErr
	' --------------------------------- I
	If Abs(PidErr) < 15000 And PidErr > 0.01
		PidI = PidI + PidErr * PidLoopTime
	Else
		PidI = 0
	EndIf
	' --------------------------------- D (based on Err)
	PidD = (PidErr - PidOldD) / PidLoopTime
	PidOldD = PidErr
	' --------------------------------- Output
	Drive1 = PidP + PidKI * PidI + PidKD * PidD
	
	If Drive1 > 0
		Slot(Slot_MotorA_A) = 0
		Slot(Slot_MotorA_B) = Abs(Drive1)
	Else
		Slot(Slot_MotorA_B) = 0
		Slot(Slot_MotorA_A) = Abs(Drive1)
	EndIf
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

' ╔════════════════════════════════════════════╗
' ║ PRINT COORDINATES                          ║
' ╚════════════════════════════════════════════╝
Label PrintValues
	Controls OpenTextBox
	s1 =       "Dest.=" + Format(Slot(Slot_MotorA_B), "0.000")
	Print CRLF + s1
Return

' ╔════════════════════════════════════════════╗ ▀▀█▀▀ █▀▀█ █ █▀▀█ █
' ║ T R I A L S                                ║   █   █▄▄▀ █ █▄▄█ █
' ╚════════════════════════════════════════════╝   █   █ ▀▄ █ █  █ █▄▄

' ╔════════════════════════════════════════════╗ █▀▀▀ █  █ █▀█
' ║  S U B R O U T I N E   A R E A             ║ ▀▀▀█ █  █ █▀▀█
' ╚════════════════════════════════════════════╝ ▄▄▄█ █▄▄█ █▄▄█




'BOOKMARKS 374 

'EDITEDLINES 58 1 56 1 57 2 348 3 117 4 189 5 200 6 374 9 373 9 378 9 375 10 

'WATCH ttsvoices
'WATCH S1
