' ====================================================================
'     Name: CoreXY_V-slot_2.txt                      Date: 10 May 2021
'    Autor: Leonardo DE PALO
'    Scope: Test COREXY with V-slot
' Hardware: Standard Master.
'     Note: Proto version have the following rule:
'			MA= Blk Gre Red Blu   ????
'			MB= Blu Red Gre Blk   ????
'           MA=1000/0  MB=1000/0  ????
'           * MB have the stepper connector rotated
' ====================================================================

Option Speed 9
'Load Images\TabletTesterLeoProto.jpg
'Load Sound\gong.wav
'COM Open 1 115200

' -------------------------------------- Options
KeyJog_Speed = 800

' -------------------------------------- Software limits
DestxMax =   310
DestxMin =     1
DestyMax =   340
DestyMin =     1

' -------------------------------------- MOTORS ASSIGNEMENTS
' Digital motors fine tuning set up
PEN_DOWN       = 913 ' 900 Motor  1
PEN_UP         = 860


' ----------------------------------------- STEPPER SLOTS
Slot_MotorA        =  1			'1_TabTesterXY pin  1 STEPPER
Slot_MotorB        =  3			'1_TabTesterXY pin  3 STEPPER
Slot_SensorX       =  7			'1_TabTesterXY pin  7 DIG INPU
Slot_SensorY       =  8			'1_TabTesterXY pin  8 DIG INPU
Slot_EnableMotors  = 10			'1_TabTesterXY pin 10 DIG OUT

' ----------------------------------------- DIGITAL MOTOR SLOTS
SLOT_PEN           = 29			'3_TabTesterMot pin 4 PWMF

' ----------------------------------------- EMERGENCY and SPEED
Slot_Emergency     = 11
Slot_Speed         =  6

' ----------------------------------------- JOG WITH MOUSE
Jog_RangeX        = 256        '288
Jog_RangeY        = 144
Jog_Enable_Slot   = 34

' ----------------------------------------- FILE SET UP
' Variable String FileName
' FileName = "Test_Results.txt"

' ----------------------------------------- LOAD APPS
Load Theremino_HAL\Theremino_HAL.exe
Load Theremino_HAL\Theremino_SlotViewer.exe

'Load Theremino_MicroVideo\Theremino_MicroVideo.exe
'Load Theremino_AudioGenerator\Theremino_AudioGenerator.exe
'Load Theremino_AudioInput\Theremino_AudioInput.exe
'Load Theremino_HAL\Theremino_SignalScope.exe

'Load OpenApps

' ----------------------------------------- KEYS
Key Right		Gosub ExecKeys
Key Left 		Gosub ExecKeys
Key Up 			Gosub ExecKeys
Key Down 		Gosub ExecKeys
Key Pageup		Gosub PenUp
Key Pagedown	Gosub PenDown
Key End     	Gosub PenSingle

' ----------------------------------------- START
'Window MinSize

DestX = v1
DestY = v2

Gosub PenUp

Load Hide

' -----------------------------------------------------------------
'  BUTTONS
' -----------------------------------------------------------------

Button 1 Text Zero
Button Zero Text "ZERO"
Button Zero Color Yellow

Button 2 Text EnableMotors
Button EnableMotors Text "ENAB. MOTORS"
Button EnableMotors Color PaleGreen

Button 3 Text DisableMotors
Button DisableMotors Text "DISAB. MOTORS"
Button DisableMotors Color PaleGreen

Button 6 Text RunSequence1
Button RunSequence1 Text "Run sequence 1"
Button RunSequence1 Color Yellow8

Button 7 Text RunSequence2
Button RunSequence2 Text "Run sequence 2"
Button RunSequence2 Color Yellow8

Button 9 Text EditSequence1
Button EditSequence1 Text "Edit sequence 1"
Button EditSequence1 Color Yellow8

Button 10 Text EditSequence2
Button EditSequence2 Text "Edit sequence 2"
Button EditSequence2 Color Yellow8

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

Button 16 Text Emergency
Button Emergency Text "EMERGENCY"
Button Emergency Color Red
Button Emergency Slot Slot_Emergency

Stop												'

' -----------------------------------------------------------------
Variable Numeric DestxMax
Variable Numeric DestxMin
Variable Numeric DestyMax
Variable Numeric DestyMin

' -----------------------------------------------------------------
Label  ClearButtons
	For v1 = 1 To 32
		Button v1 Text ""
		Button v1 Color Moccasin
	Next
Return

Label  CloseAll
	Load CloseApps
Return

Label EventStop
	Window Sizable
End


' =====================================================================
'  SEQUENCES
' =====================================================================
Label InitSequencePaths
	Variable String EditorFolder
	Variable String EditorApp
	Variable String SQ1
	Variable String SQ2
	Variable String EditSQ1
	Variable String EditSQ2
	'
	EditorFolder = "Theremino_HAL\Theremino_Editor\"
	EditorApp = EditorFolder + "Theremino_Editor.exe"
	'
	SQ1 = EditorFolder + "Sequences\SquareTest.seq"
	SQ2 = EditorFolder + "Sequences\TestSeq.seq"
	'
	EditSQ1 = EditorApp + " " + SQ1
	EditSQ2 = EditorApp + " " + SQ2
Return

Label RunSequence1
	Gosub InitSequencePaths
	SequenceFileName = SQ1
	Gosub ExecSequence
	Gosub RunSequence1
Return

Label RunSequence2
	Gosub InitSequencePaths
	SequenceFileName = SQ2
	Gosub ExecSequence
Return

Label EditSequence1
	Gosub InitSequencePaths
	Load EditSQ1
Return

Label EditSequence2
	Gosub InitSequencePaths
	Load EditSQ2
Return


' =====================================================================
'  SEQUENCE LOADING AND EXECUTION
' =====================================================================
Variable String SequenceFileName
Variable String Sequence
Variable String SequenceCmd
Variable Numeric SequenceIndex
Variable Numeric SequenceLength

Label ExecSequence
	Load Sequence SequenceFileName
	If Len(Sequence) = 0
		Controls OpenTextBox
		Print CRLF + " Sequence file not found: " + SequenceFileName
		Beep
		Stop                                                         '
	EndIf
		' ------------------------------------------
	Controls OpenTextBox
	Controls ClearTextBox
	' ------------------------------------------
	Variable Numeric ExecSequenceStartTime
	ExecSequenceStartTime = ElapsedTime
	' ------------------------------------------
	SequenceLength = GetSeparatedStringCount(Sequence, CRLF)
	For SequenceIndex = 0 To SequenceLength-1
		Gosub ExecSpeed
		SequenceCmd = GetSeparatedString(Sequence, SequenceIndex, CRLF)
		SequenceCmd = Replace(SequenceCmd, Chr(9), " ") 'TABS to SPACES
		Print " Sequence: " + SequenceCmd + CRLF
		SequenceCmd = RemoveComments(SequenceCmd)
		SequenceCmd = Trim(SequenceCmd)
		'
		Select UCase(SequenceCmd)
            ' ------------------- exit from the sequence -------------
			Case "ENDSEQ"
				SequenceIndex = SequenceLength-1
			' ------------------- special multiple actions -----------
			Case "SLIDE"		     ' slide before insert PSW
				Gosub Slide
			Case "ZERO"              ' move pen to X=0 and Y=0 position
				Gosub Zero
		   ' --------------------- single action ----------------------
			Case "ENABLEMOTORS"
				Gosub EnableMotors
			Case "DISABLEMOTORS"
				Gosub DisableMotors
			Case "CLOSE ALL"
				Gosub CloseAll
			' --------------------- WAIT MOTORS -----------------------
			Case "WAIT MOTORS"
				Gosub WaitMotors
			' --------------------- Instruction with parameters -------
			CaseElse
				s1 = GetSeparatedString(SequenceCmd, 0)
				Select UCase(s1)
					Case "MOVE"
						DestX = GetSeparatedString(SequenceCmd, 1)
						DestY = GetSeparatedString(SequenceCmd, 2)
						Gosub CoreXY_MoveToDestXY_AndWait
					Case "MOVESLOW"
						MoveSlowXY_DestX = GetSeparatedString(SequenceCmd, 1)
						MoveSlowXY_DestY = GetSeparatedString(SequenceCmd, 2)
						MoveSlowXY_Speed = GetSeparatedString(SequenceCmd, 3)
						Gosub MoveSlowXY
					Case "PAUSE"
						Wait Seconds GetSeparatedString(SequenceCmd, 1)
					Case "PRESSKEYS"
						SequenceCmd = Mid(SequenceCmd, 11)
						PressKeys SequenceCmd
					Case "SENDKEYS"
						SequenceCmd = Mid(SequenceCmd, 10)
						SendKeys SequenceCmd
					Case "PENTYPING"
						PEN_TOUCH_TIME = GetSeparatedString(SequenceCmd, 1)
						Gosub PenTyping
					CaseElse
						If s1 <> ""
							Controls OpenTextBox
							Print CRLF + " Unrecognized command: " + SequenceCmd + CRLF
							Beep
							Stop
						EndIf
				EndSelect
		EndSelect
	Next
	SequenceFileName = Replace(SequenceFileName,".seq","")
	Print SequenceFileName + " executed in " + Format(ElapsedTime - ExecSequenceStartTime, "0.00") + " seconds" + CRLF
Return

' =====================================================================
'  CORE XY
' =====================================================================
Variable Numeric DestX				' destination in mm coords
Variable Numeric DestY				' destination in mm coords

Variable Numeric Slot_MotorA
Variable Numeric Slot_MotorB
Variable Numeric Slot_EnableMotors
Variable Numeric Slot_SensorX
Variable Numeric Slot_SensorY

Label CoreXY_MoveToDestXY_AndWait   ' move to destX & destY and wait stop motors
	Gosub CoreXY_MoveToDestXY
	Gosub WaitMotors
Return

Label CoreXY_MoveToDestXY			' move to destX & destY (with limits)
	DestX = Limit(DestX, DestxMin, DestxMax)
	DestY = Limit(DestY, DestyMin, DestyMax)
	If LimitReached
		Load sound\GeigerClick_16.wav
	EndIf
	Gosub CoreXY_PrepareCoreAB
	Slot(Slot_MotorA) = CoreA
	Slot(Slot_MotorB) = CoreB
Return

Label CoreXY_MoveToDestXY_NoLimits	' move to destX & destY (without using limits)
	Gosub CoreXY_PrepareCoreAB
	Slot(Slot_MotorA) = CoreA
	Slot(Slot_MotorB) = CoreB
Return

Label CoreXY_SetNewZero
    ' ------------------------- Reset axis X and Y
	Slot(Slot_MotorA) = Reset
	Slot(Slot_MotorB) = Reset
	Wait Seconds 0.15
	' ------------------------- Set reset values with CoreXY correction
	DestX = 0
	DestY = 0
	Gosub CoreXY_MoveToDestXY_NoLimits
	Gosub WaitMotors
Return

Variable Numeric CoreA
Variable Numeric CoreB

Label CoreXY_PrepareCoreAB
	CoreA = (DestX - DestY) / 2
	CoreB = (DestX + DestY) / 2
Return

' =====================================================================
'  HOME
' =====================================================================
Variable Numeric HomeAxis
Variable Numeric SlotSensor
Variable Numeric DetachMM
Variable Numeric SpeedSearch
Variable Numeric SpeedReturn
Variable Numeric InitialPosition

Label Zero
	Gosub CoreXY_SetNewZero
	Gosub EnableMotors
	Gosub PenUp
	Gosub HomeX
	Gosub HomeY
	Gosub CoreXY_SetNewZero
	Gosub PrintValues
	'Gosub "DISAB.MOTORS"
Return

Label HomeX
	HomeAxis = 0        	  ' 0 = Axis X
	SlotSensor = Slot_SensorX ' home sensor slot
	DetachMM = 1			  ' initial detach mm (neg or pos)
	SpeedSearch = -0.6		  ' direction and speed
	SpeedReturn = +0.05		  ' detach direction and speed
	InitialPosition = 1       ' 8 move to MM
	Gosub  Home
Return


Label HomeY
	HomeAxis = 1        	  ' 1 = Axis Y
	SlotSensor = Slot_SensorY ' home sensor slot
	DetachMM = 1			  ' initial detach mm (neg or pos)
	SpeedSearch = -0.6		  ' direction and speed
	SpeedReturn = +0.05		  ' detach direction and speed
	InitialPosition = 1       ' move to MM
	Gosub Home
Return

Label Home
	' -------------------------- detach if sensor is active
	If Slot(SlotSensor) > 500 Or Key("CTRL")
		If HomeAxis = 0
			DestX = DestX + DetachMM
		Else
			DestY = DestY + DetachMM
		EndIf
		Gosub CoreXY_MoveToDestXY_NoLimits
		Gosub WaitMotors
	EndIf
    ' -------------------------- search the sensor
Label SearchHome
	If HomeAxis = 0
		DestX = DestX + SpeedSearch
	Else
		DestY = DestY + SpeedSearch
	EndIf
	Gosub CoreXY_MoveToDestXY_NoLimits
	'
	Wait Seconds 0.005
	If Slot(SlotSensor) > 500 Or Key("CTRL")
		Gosub CoreXY_SetNewZero
		Goto DetachSensor
	EndIf
Goto SearchHome
    ' -------------------------- detach from the sensor
Label DetachSensor
	If HomeAxis = 0
		DestX = DestX + SpeedReturn
	Else
		DestY = DestY + SpeedReturn
	EndIf
	Gosub CoreXY_MoveToDestXY_NoLimits
	Wait Seconds 0.005
	If Slot (SlotSensor) < 500 Or Key("CTRL")
		Gosub WaitMotors
		Gosub CoreXY_SetNewZero
		Goto MoveToInitialPosition
	EndIf
Goto DetachSensor
    ' -------------------------- move to InitialPosition
Label MoveToInitialPosition
	If HomeAxis = 0
		DestX = InitialPosition
	Else
		DestY = InitialPosition
	EndIf
	Gosub CoreXY_MoveToDestXY_NoLimits
	Gosub WaitMotors
Return

' =====================================================================
'  WAIT until the motors A and B are stopped
' =====================================================================
Label WaitMotors
	Wait Seconds 0.1         ' this ensures that the motors are running
	Label WaitMotors1
		If Abs(Slot(Slot_MotorA + 1)) < 0.05
			If Abs(Slot(Slot_MotorB + 1)) < 0.05
				Return
			EndIf
		EndIf
		Wait Seconds 0.005
	Goto WaitMotors1
Goto WaitMotors

' =====================================================================
'  DIGITAL MOTORS MOVEMENTS
' =====================================================================
Variable Numeric PEN_DOWN
Variable Numeric PEN_UP
Variable Numeric PEN_TOUCH_TIME

Variable Numeric SLOT_PEN			' digital motor 1

Label PenUp
	Slot(SLOT_PEN) = PEN_UP
	Wait Seconds 0.1
	'Gosub SleepServo
Return

Label PenDown
	Slot(SLOT_PEN) = PEN_DOWN
	Wait Seconds 0.5
	'Gosub SleepServo
Return

Label PenSingle
	Slot(SLOT_PEN) = PEN_DOWN
	Wait Seconds 0.35
	Slot(SLOT_PEN) = PEN_UP
	Wait Seconds 0.1
	'Gosub SleepServo
Return

Label PenDouble
	Slot(SLOT_PEN) = PEN_DOWN
	Wait Seconds 0.09
	Slot(SLOT_PEN) = PEN_UP
	Wait Seconds 0.1
	Slot(SLOT_PEN) = PEN_DOWN
	Wait Seconds 0.09
	Slot(SLOT_PEN) = PEN_UP
	Wait Seconds 0.5
	'Gosub SleepServo
Return

Label PenTyping
	Slot(SLOT_PEN) = PEN_DOWN
	Wait Seconds PEN_TOUCH_TIME
	Slot(SLOT_PEN) = PEN_UP
Return

Label EnableMotors
	Gosub WaitMotors
	Slot(Slot_EnableMotors) = 1000
Return

Label DisableMotors
	Gosub WaitMotors
	Slot(Slot_EnableMotors) = 0
Return

' ====================================================================
'  EMERGENCY & SPEED
' ====================================================================
Variable Numeric Slot_Emergency
Variable Numeric Slot_Speed

Label Emergency
	Gosub DisableAllButtons
	Button 16 Enabled
	Button 16 Color Flashing7 Yellow Red
	KeyJog_Speed = 0
	For v1 = 1 To 9e99
		Wait Seconds 0.2	 '1.1
		'Load sound\Alarm10b.wav
		Load sound\Click1.wav
	Next
Stop

Label ExecSpeed
	'v9 = Slot(Slot_Speed)
	'v9 = 3 + 6 * v9 / 1000
	'Option Speed V9
Return


' ====================================================================
' 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

' =====================================================================
'  STEPPER-MOTORS MOVE SLOW
' =====================================================================
Variable Numeric MoveSlowXY_DestX 	' destination in mm
Variable Numeric MoveSlowXY_DestY 	' destination in mm
Variable Numeric MoveSlowXY_Speed 	' speed in mm/sec

Variable Numeric MSV1
Variable Numeric MSV2
Variable Numeric MSV3
Variable Numeric MSV4

Label MoveSlowXY
	' ----------------------------------- test for very low speed
	If MoveSlowXY_Speed < 0.1
		MoveSlowXY_Speed = 1
	EndIf
	' ----------------------------------- calc the movement in mm
	MSV1 = MoveSlowXY_DestX - DestX
	MSV2 = MoveSlowXY_DestY - DestY
	MSV3 = MSV1 * MSV1
	MSV3 = MSV3 + MSV2 * MSV2
	MSV3 = Sqrt(MSV3)
	' ----------------------------------- calc the loops count
	MSV4 = MSV3 / 0.01 '<<< loop time
	MSV4 = Int(MSV4 / MoveSlowXY_Speed)
	' ----------------------------------- too much speed ?
	If MSV4 = 0
		DestX = MoveSlowXY_DestX
		DestY = MoveSlowXY_DestY
		Gosub CoreXY_MoveToDestXY_AndWait
		Return
	EndIf
	' ----------------------------------- prepare the increments
	MSV1 = (MoveSlowXY_DestX - DestX) / MSV4
	MSV2 = (MoveSlowXY_DestY - DestY) / MSV4
	' ----------------------------------- loop for v4 times
	For MSV3 = 1 To MSV4
		' ------------------------------- increment, move and wait
		DestX = DestX + MSV1
		DestY = DestY + MSV2
		Gosub CoreXY_MoveToDestXY
		Wait Seconds 0.01 '<<< loop time
	Next
	Gosub WaitMotors
Return


' =====================================================================
'  STEPPER-MOTORS and PEN action SLIDE
' =====================================================================
Variable Numeric Slide_DestX 	 ' destination in mm
Variable Numeric Slide_DestY 	 ' destination in mm
Variable Numeric Slide_Speed 	 ' speed in mm/sec
Variable Numeric Slide_PenUpTime ' pen-up time in mS from start

Variable Numeric SLV1
Variable Numeric SLV2
Variable Numeric SLV3
Variable Numeric SLV4
Variable Numeric SLV5

Label Slide
	Gosub CoreXY_MoveToDestXY_AndWait
	Slide_DestX = 50                  '50
	Slide_DestY = 130                 '130
	Slide_Speed = 200                 '200
	Slide_PenUpTime = 500             '500
	' ----------------------------------- calc the movement in mm
	SLV1 = Slide_DestX - DestX
	SLV2 = Slide_DestY - DestY
	SLV3 = SLV1 * SLV1
	SLV3 = SLV3 + SLV2 * SLV2
	SLV3 = Sqrt(SLV3)
	' ----------------------------------- calc the loops count
	SLV4 = SLV3 / 0.01 '<<< loop time
	SLV4 = Int(SLV4 / Slide_Speed)
	' ----------------------------------- prepare the increments
	SLV1 = (Slide_DestX - DestX) / SLV4
	SLV2 = (Slide_DestY - DestY) / SLV4
	' ----------------------------------- prepare the pen-up time
	SLV5 = Slide_PenUpTime / 1000
	SLV5 = Int(SLV5 / 0.01) '<<< loop time
	' ----------------------------------- pen down
	Slot(SLOT_PEN) = PEN_DOWN
	' ----------------------------------- loop for v4 times
	For SLV3 = 1 To SLV4
		' ------------------------------- increment, move and wait
		DestX = DestX + SLV1
		DestY = DestY + SLV2
		Gosub CoreXY_MoveToDestXY
		Wait Seconds 0.01 '<<< loop time
		' ------------------------------- pen up
		If SLV3 = SLV5
			Slot(SLOT_PEN) = PEN_UP
		EndIf
	Next
	Gosub WaitMotors
Return

' ================================================
'  JOG WITH MOUSE
' ================================================
Variable Numeric Jog_Enable_Slot
Variable Numeric Jog_RangeX
Variable Numeric Jog_RangeY
Variable Numeric Jog_Active

Label JogWithMouse
	Gosub EnableMotors
	Button 8 Text "JOG STOP"
	Jog_Active = 1
	Label Jog2
		If Slot(Jog_Enable_Slot) > 500
			DestX = MouseX * Jog_RangeX
			DestY = MouseY * Jog_RangeY
			Gosub CoreXY_MoveToDestXY
			Gosub PrintValues
		EndIf
		If Jog_Active = 0
			Return
		EndIf
	Goto Jog2
Return

Label JogStop
	Button 8 Text "JOG with Mouse"
	Jog_Active = 0
Return

' ====================================================================
'  JOG with keyboard
' ====================================================================
Label ExecKeys
	Variable Numeric KeyJog_Speed
	Variable Numeric JogSpeed
	JogSpeed = KeyJog_Speed / 100
	Select True
		Case Key("SHIFT")
			JogSpeed = JogSpeed * 0.1
		Case Key("CTRL")
			JogSpeed = JogSpeed * 0.01
		Case Key("ALT")
			JogSpeed = JogSpeed * 0.001
	EndSelect
	'
	If Key("Right")
		DestX = DestX + JogSpeed
	EndIf
	If Key("Left")
		DestX = DestX - JogSpeed
	EndIf
	'
	If Key("Up")
		DestY = DestY + JogSpeed		'proto
		'DestY = DestY - JogSpeed		'Leo
	EndIf
	If Key("Down")
		DestY = DestY - JogSpeed		'proto
'		DestY = DestY - JogSpeed		'Leo
	EndIf
	'
	'If Key("PageUp")
	'	Slot Slot_Z = Limit(Slot(Slot_Z) + JogSpeed, Zmin, Zmax)
	'EndIf
	'If Key("PageDown")
	'	Slot Slot_Z = Limit(Slot(Slot_Z) - JogSpeed, Zmin, Zmax)
	'EndIf
	'
	Gosub CoreXY_MoveToDestXY
	Gosub PrintValues
Return

' ====================================================================
'  PRINT COORDINATES
' ====================================================================
Label PrintValues
	Controls OpenTextBox
	s1 =       "DestX=" + Format(DestX, "0.000")
	s1 = s1 + " DestY=" + Format(DestY, "0.000")
	Print CRLF + s1
Return




'WATCH v1
'EXEC DestX = 12
