' -------------------------------------- Execution speed
Option Speed 9
' -------------------------------------- Speed for keyboard Jog
KeyJog_Speed = 20
' -------------------------------------- Slots
SlotMotor1 = 1
SlotMotor2 = 3
SlotMotorRot1 = 19
SlotMotorRot2 = 21
SlotSwitchStart1 = 11
SlotSwitchEnd1 = 7
SlotSwitchStart2 = 12
SlotSwitchEnd2 = 8
' -------------------------------------- Limits Min and Max
Mot1Min = 0
Mot1Max = 650
Mot2Min = 0
Mot2Max = 650
Mot1RotMin = -90
Mot1RotMax = +90
Mot2RotMin = -90
Mot2RotMax = +90
' -------------------------------------- Buttons
Button 1 Text HOME MOTORS
Button 2 Text Test1
Button 3 Text Test2
Button 5 Text Loop
' -------------------------------------- Init Keys
Gosub InitKeys
' -------------------------------------- Ensure motor stopped
Gosub WaitAllMotors
' -------------------------------------- Restore old motor positions
Gosub RestorePositions
' -------------------------------------- Eventually do HOME
'Gosub "HOME MOTORS"

Stop


' ====================================================================
'  TEST FUNCTIONS
' ====================================================================
Label Test1
	
Return

Label Test2
	
Return

Label Loop
	MoveSlow_Dest1    = 600
	MoveSlow_Dest2    = 10
	MoveSlow_DestRot1 = -30
	MoveSlow_DestRot2 = 30
	MoveSlow_Speed    = 100
	Gosub MoveSlow
	'
	MoveSlow_Dest1    = 600
	MoveSlow_Dest2    = 600
	MoveSlow_DestRot1 = 30
	MoveSlow_DestRot2 = -30
	MoveSlow_Speed    = 100
	Gosub MoveSlow
	'
	MoveSlow_Dest1    = 100
	MoveSlow_Dest2    = 600
	MoveSlow_DestRot1 = 30
	MoveSlow_DestRot2 = -30
	MoveSlow_Speed    = 100
	Gosub MoveSlow
Goto Loop


' ####################################################################
'  END OF USER AREA
' ####################################################################

' ====================================================================
'  VARS
' ====================================================================
Variable Numeric SlotMotor1
Variable Numeric SlotMotor2
Variable Numeric SlotMotorRot1
Variable Numeric SlotMotorRot2
Variable Numeric SlotSwitchStart1
Variable Numeric SlotSwitchEnd1
Variable Numeric SlotSwitchStart2
Variable Numeric SlotSwitchEnd2

Variable Numeric Mot1Min
Variable Numeric Mot1Max
Variable Numeric Mot1RotMin
Variable Numeric Mot1RotMax
Variable Numeric Mot2Min
Variable Numeric Mot2Max
Variable Numeric Mot2RotMin
Variable Numeric Mot2RotMax

' ====================================================================
' ENABLE/DISABLE BUTTONS
' ====================================================================
Label EnableAllButtons
	For v1 = 1 To 32
		Button v1 Enabled
	Next
Return

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

' =====================================================================
'  CLOSING EVENTS
' =====================================================================
Label EventStop
	Gosub SavePositions
	Gosub EnableAllButtons
End

' ====================================================================
'  SAVE and RESTORE POSITIONS
' ====================================================================
Label SavePositions
	v1 = Slot(SlotMotor1)
	v2 = Slot(SlotMotor2)
	v3 = Slot(SlotMotorRot1)
	v4 = Slot(SlotMotorRot2)
	Save Vars
Return

Label RestorePositions
	Load Vars
	If v1 <> 0 And v2 <> 0 And v3 <> 0 And v4 = 0
		Slot(SlotMotor1) = Reset
		Wait Seconds 0.01
		Slot(SlotMotor1) = v1
		Wait Seconds 0.01
		'
		Slot(SlotMotor2) = Reset
		Wait Seconds 0.01
		Slot(SlotMotor2) = v2
		Wait Seconds 0.01
		'
		Slot(SlotMotorRot1) = Reset
		Wait Seconds 0.01
		Slot(SlotMotorRot1) = v3
		Wait Seconds 0.01
		'
		Slot(SlotMotorRot2) = Reset
		Wait Seconds 0.01
		Slot(SlotMotorRot2) = v4
		Wait Seconds 0.01
	EndIf
	Gosub PrintValues
Return

' ====================================================================
'  MOVE SLOW XY
' ====================================================================
Variable Numeric MoveSlow_Dest1 	' destination in mm
Variable Numeric MoveSlow_Dest2 	' destination in mm
Variable Numeric MoveSlow_DestRot1 	' destination in mm
Variable Numeric MoveSlow_DestRot2 	' destination in mm
Variable Numeric MoveSlow_Speed 	' speed in mm/sec

Variable Numeric MSV1
Variable Numeric MSV2
Variable Numeric MSVR1
Variable Numeric MSVR2
Variable Numeric MSV5
Variable Numeric MSV6

Label MoveSlow
	' ----------------------------------- test for out of limit
	Slot(SlotMotor1) = Limit(Slot(SlotMotor1), Mot1Min, Mot1Max)
	Slot(SlotMotor2) = Limit(Slot(SlotMotor2), Mot2Min, Mot2Max)
	Slot(SlotMotorRot1) = Limit(Slot(SlotMotorRot1), Mot1RotMin, Mot1RotMax)
	Slot(SlotMotorRot2) = Limit(Slot(SlotMotorRot2), Mot2RotMin, Mot2RotMax)
	' ----------------------------------- test for very low speed
	If MoveSlow_Speed < 0.1
		MoveSlow_Speed = 1
	EndIf
	' ----------------------------------- calc the movement in mm
	MSV1 = MoveSlow_Dest1 - Slot(SlotMotor1)
	MSV2 = MoveSlow_Dest2 - Slot(SlotMotor2)
	MSVR1 = MoveSlow_DestRot1 - Slot(SlotMotorRot1)
	MSVR2 = MoveSlow_DestRot2 - Slot(SlotMotorRot2)
	
	MSV5 = MSV1 * MSV1
	MSV5 = MSV5 + MSV2 * MSV2
	MSV5 = MSV5 + MSVR1 * MSVR1
	MSV5 = MSV5 + MSVR2 * MSVR2
	MSV5 = Sqrt(MSV5)
	' ----------------------------------- calc the loops count
	MSV6 = MSV5 / 0.01 '<<< loop time
	MSV6 = Int(MSV6 / MoveSlow_Speed)
	' ----------------------------------- too much speed ?
	If MSV6 = 0
		Slot(SlotMotor1) = Limit(MoveSlow_Dest1, Mot1Min, Mot1Max)
		Slot(SlotMotor2) = Limit(MoveSlow_Dest2, Mot2Min, Mot2Max)
		Slot(SlotMotorRot1) = Limit(MoveSlow_DestRot1, Mot1RotMin, Mot1RotMax)
		Slot(SlotMotorRot2) = Limit(MoveSlow_DestRot2, Mot2RotMin, Mot2RotMax)
		Return
	EndIf
	' ----------------------------------- prepare the increments
	MSV1 = (MoveSlow_Dest1 - Slot(SlotMotor1)) / MSV6
	MSV2 = (MoveSlow_Dest2 - Slot(SlotMotor2)) / MSV6
	MSVR1 = (MoveSlow_DestRot1 - Slot(SlotMotorRot1)) / MSV6
	MSVR2 = (MoveSlow_DestRot2 - Slot(SlotMotorRot2)) / MSV6
	' ----------------------------------- loop for MSV6 times
	For MSV5 = 1 To MSV6
		' ------------------------------- increment, move and wait
		Slot(SlotMotor1) = Limit(Slot(SlotMotor1) + MSV1, Mot1Min, Mot1Max)
		Slot(SlotMotor2) = Limit(Slot(SlotMotor2) + MSV2, Mot2Min, Mot2Max)
		Slot(SlotMotorRot1) = Limit(Slot(SlotMotorRot1) + MSVR1, Mot1RotMin, Mot1RotMax)
		Slot(SlotMotorRot2) = Limit(Slot(SlotMotorRot2) + MSVR2, Mot2RotMin, Mot2RotMax)
		Wait Seconds 0.01 '<<< loop time
		Gosub PrintValues
	Next
	'Gosub WaitAllMotorsAndRecovery
	Gosub WaitAllMotors
Return

' ====================================================================
'  HOME MOTORS
' ====================================================================
Label HOME MOTORS
	SlotMotor = SlotMotor1
	SlotSensor = SlotSwitchStart1
	SpeedSearch = -0.25     ' search direction and speed
	SpeedReturn = +0.05     ' return direction and speed
	DeclaredSwitchPos = 0
	InitialPosition = 5		' relative to DeclaredSwitchPos
	Gosub HomeMotor
	'
	SlotMotor = SlotMotor2
	SlotSensor = SlotSwitchStart2
	SpeedSearch = -0.25		' search direction and speed
	SpeedReturn = +0.05		' returndirection and speed
	DeclaredSwitchPos = 0
	InitialPosition = 5		' relative to DeclaredSwitchPos
	Gosub HomeMotor
	'
	Gosub PrintValues
Return

' ====================================================================
'  Home the Motor(SlotMotor) to the Sensor(SlotSensor)
' ====================================================================
Variable Numeric SlotMotor
Variable Numeric SlotSensor
Variable Numeric SpeedSearch
Variable Numeric SpeedReturn
Variable Numeric DeclaredSwitchPos
Variable Numeric InitialPosition

Label HomeMotor
	Gosub DisableAllButtons
	' ------------------------- detach if sensor is active
	If Slot(SlotSensor) > 500 Or Key("CTRL")
		Slot(SlotMotor) = Slot(SlotMotor) + SpeedReturn
	EndIf
    ' ------------------------- search the sensor
Label SearchHome
	Slot(SlotMotor) = Slot(SlotMotor) + SpeedSearch
	Wait Seconds 0.001
	If Slot(SlotSensor) > 500 Or Key("CTRL")
		Slot(SlotMotor) = Reset
		Wait Seconds 0.1
		Slot(SlotMotor) = 0
		Wait Seconds 0.1
		Goto DetachSensor
	EndIf
Goto SearchHome
    ' ------------------------- detach from the sensor
Label DetachSensor
	Slot(SlotMotor) = Slot(SlotMotor) + SpeedReturn
	Wait Seconds 0.001
	If Slot (SlotSensor) < 500
		Slot(SlotMotor) = Reset
		Wait Seconds 0.1
		Slot(SlotMotor) = DeclaredSwitchPos
		Wait Seconds 0.1
		Goto SetFinalValues
	EndIf
Goto DetachSensor
    ' ------------------------- reset and move to InitialPos
Label SetFinalValues
	Slot(SlotMotor) = InitialPosition
	Gosub WaitMotor
	Gosub EnableAllButtons
Return

' ====================================================================
'   Wait until the motor(SlotMotor) is stopped
' ====================================================================
Label WaitMotor
	Wait Seconds 0.1
	Variable Numeric WaitMot1
	For WaitMot1 = 1 To 9e99
		If Abs(Slot(SlotMotor + 1)) < 0.01
			Exit
		EndIf
		Wait Seconds 0.005
	Next
Return


' ====================================================================
'   Wait until All-Motors are stopped
' ====================================================================
Label WaitAllMotors
	Wait Seconds 0.1 ' <<< ensures that motors are really started
	Variable Numeric WaitMot2
	For WaitMot2 = 1 To 9e99
		If Abs(Slot(SlotMotor1 + 1)) < 0.01 And Abs(Slot(SlotMotor2 + 1)) < 0.01
			Exit
		EndIf
		Wait Seconds 0.005
	Next
Return


' ====================================================================
'  INIT KEYS
' ====================================================================
Label InitKeys
	Key Right	Gosub ExecKeys
	Key Left 	Gosub ExecKeys
	Key Up 		Gosub ExecKeys
	Key Down 	Gosub ExecKeys
	Key Num6	Gosub ExecKeys
	Key Num4 	Gosub ExecKeys
	Key Num8 	Gosub ExecKeys
	Key Num2 	Gosub ExecKeys
Return

' ====================================================================
'  JOG with keyboard
' ====================================================================
Label ExecKeys
	Variable Numeric KeyJog_Speed
	Variable Numeric JogSpeed
	JogSpeed = KeyJog_Speed / 100
	Select True
		Case Key("SHIFT")
			JogSpeed = JogSpeed * 100
		Case Key("CTRL")
			JogSpeed = JogSpeed * 10
		Case Key("ALT")
			JogSpeed = JogSpeed * 0.1
	EndSelect
	'
	If Key("Right")
		Slot SlotMotor1 = Limit(Slot(SlotMotor1) + JogSpeed, Mot1Min, Mot1Max)
	EndIf
	If Key("Left")
		Slot SlotMotor1 = Limit(Slot(SlotMotor1) - JogSpeed, Mot1Min, Mot1Max)
	EndIf
	'
	If Key("Up")
		Slot SlotMotor2 = Limit(Slot(SlotMotor2) + JogSpeed, Mot2Min, Mot2Max)
	EndIf
	If Key("Down")
		Slot SlotMotor2 = Limit(Slot(SlotMotor2) - JogSpeed, Mot2Min, Mot2Max)
	EndIf
	'
	If Key("Num6")
		Slot SlotMotorRot1 = Limit(Slot(SlotMotorRot1) + JogSpeed, Mot1RotMin, Mot1RotMax)
	EndIf
	If Key("Num4")
		Slot SlotMotorRot1 = Limit(Slot(SlotMotorRot1) - JogSpeed, Mot1RotMin, Mot1RotMax)
	EndIf
	'
	If Key("Num8")
		Slot SlotMotorRot2 = Limit(Slot(SlotMotorRot2) + JogSpeed, Mot2RotMin, Mot2RotMax)
	EndIf
	If Key("Num2")
		Slot SlotMotorRot2 = Limit(Slot(SlotMotorRot2) - JogSpeed, Mot2RotMin, Mot2RotMax)
	EndIf
	'
	Gosub PrintValues
Return

' ====================================================================
'  PRINT VALUES
' ====================================================================
Label PrintValues
	s1 =       "Motor1=" + Format(Slot(SlotMotor1), "0.000")
	s1 = s1 + "   Motor2=" + Format(Slot(SlotMotor2), "0.000")
	s1 = s1 + "   MotorRot1=" + Format(Slot(SlotMotorRot1), "0.000")
	s1 = s1 + "   MotorRot2=" + Format(Slot(SlotMotorRot2), "0.000")
	Print CRLF + s1
Return


'WATCH Speed
'WATCH Wait Seconds 2
'WATCH Wait Seconds 2
'WATCH Wait Seconds 2
'WATCH Wait Seconds 2
'WATCH Wait Seconds 2
'EXEC Slot(2) = 0
