' ------------------------------
'  TEST - ALREADY USED VARS
' ------------------------------

Slot_RunScope = 0
Slot_SoundLevel = 0
Slot_SoundLevel2ndHarm = 0
Slot_SoundLevel3rdHarm = 0
SoundLevelMin = 0
SoundNoiseMax = 0
SoundDist2ndHarmMax = 0
SoundDist3rdHarmMax = 0

Slot_S0 = 0
Slot_S1 = 0
Slot_S2 = 0
Slot_S3 = 0
Slot_Freq = 0

SequenceFileName = ""
Sequence = ""
SequenceCmd = ""
SequenceIndex = 0
SequenceLength = 0


'Stop

' ===========================================================
'   AUDIO TESTS
' ===========================================================
Variable Numeric Slot_RunScope
Variable Numeric Slot_SoundLevel
Variable Numeric Slot_SoundLevel2ndHarm
Variable Numeric Slot_SoundLevel3rdHarm
Variable Numeric SoundLevelMin
Variable Numeric SoundNoiseMax
Variable Numeric SoundDist2ndHarmMax ' Max level for the 2nd harmonics
Variable Numeric SoundDist3rdHarmMax ' Max level for the 3rd harmonics

' ' ----------------------------------------- COLOR RECOGNITION
Variable Numeric Slot_S0
Variable Numeric Slot_S1
Variable Numeric Slot_S2
Variable Numeric Slot_S3
Variable Numeric Slot_Freq

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

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


' =====================================================================
'  SERVO-MOTORS MOVEMENTS
' =====================================================================
Variable Numeric POWER_IN
Variable Numeric POWER_OUT
Variable Numeric USB3_IN
Variable Numeric USB3_OUT
Variable Numeric USBC_IN
Variable Numeric USBC_OUT
Variable Numeric HDMI_IN
Variable Numeric HDMI_OUT
Variable Numeric USBPWR_IN
Variable Numeric USBPWR_OUT
Variable Numeric ONOFF_IN
Variable Numeric ONOFF_OUT
Variable Numeric VOLUME_IN_UP
Variable Numeric VOLUME_OUT
Variable Numeric VOLUME_IN_DW
Variable Numeric MEMORY_IN
Variable Numeric MEMORY_OUT
Variable Numeric HEARSET_IN
Variable Numeric HEARSET_OUT
Variable Numeric PEN_DOWN
Variable Numeric PEN_UP
Variable Numeric PEN_TOUCH_TIME

Variable Numeric SLOT_POWER			' digital motor 1
Variable Numeric SLOT_USB3			' digital motor 2
Variable Numeric SLOT_USBC			' digital motor 3
Variable Numeric SLOT_HDMI			' digital motor 4
Variable Numeric SLOT_USBPWR		' digital motor 5
Variable Numeric SLOT_ONOFF			' digital motor 6
Variable Numeric SLOT_VOLUME		' digital motor 7
Variable Numeric SLOT_MEMORY		' digital motor 8
Variable Numeric SLOT_HEARSET		' digital motor 9
Variable Numeric SLOT_PEN			' digital motor 10

'Label "START LOOP"
'	v2 = Input "How many loops ?"
'	For v1 = 1 To v2
'		Gosub "ALL IN"
'		Wait Seconds 0.5
'		Gosub "ALL OUT"
'		Wait Seconds 0.5
'	Next
'Return

Label "POWER OUT"
	Move_Slot = SLOT_POWER
	Move_Dest = POWER_OUT
	Move_Speed = 60
	Gosub MoveSlow
	Button 1 Color Chocolate
	Button 2 Color Red
Return

Label "POWER IN"
	Move_Slot = SLOT_POWER
	Move_Dest = POWER_IN
	Move_Speed = 60
	Gosub MoveSlow
	Button 1 Color Lime
	Button 2 Color Chocolate
Return

Label "USB3 OUT"
	Move_Slot = SLOT_USB3
	Move_Dest = USB3_OUT
	Move_Speed = 60
	Gosub MoveSlow
Return

Label "USB3 IN"
	Move_Slot = SLOT_USB3
	Move_Dest = USB3_IN
	Move_Speed = 60
	Gosub MoveSlow
Return

Label "USBC IN"
	Move_Slot = SLOT_USBC
	Move_Dest = USBC_IN
	Move_Speed = 60
	Gosub MoveSlow
Return

Label "USBC OUT"
	Move_Slot = SLOT_USBC
	Move_Dest = USBC_OUT
	Move_Speed = 60
	Gosub MoveSlow
Return

Label "HDMI OUT"
	Move_Slot = SLOT_HDMI
	Move_Dest = HDMI_OUT
	Move_Speed = 60
	Gosub MoveSlow
Return

Label "HDMI IN"
	Move_Slot = SLOT_HDMI
	Move_Dest = HDMI_IN
	Move_Speed = 60
	Gosub MoveSlow
Return

Label "USBPWR OUT"
	Move_Slot = SLOT_USBPWR
	Move_Dest = USBPWR_OUT
	Move_Speed = 60
	Gosub MoveSlow
Return

Label "USBPWR IN"
	Move_Slot = SLOT_USBPWR
	Move_Dest = USBPWR_IN
	Move_Speed = 60
	Gosub MoveSlow
Return

Label "ONOFF OUT"
	Move_Slot = SLOT_ONOFF
	Move_Dest = ONOFF_OUT
	Move_Speed = 60
	Gosub MoveSlow
Return

Label "ONOFF IN"
	Move_Slot = SLOT_ONOFF
	Move_Dest = ONOFF_IN
	Move_Speed = 60
	Gosub MoveSlow
Return

Label "VOLUME OUT"
	Move_Slot = SLOT_VOLUME
	Move_Dest = VOLUME_OUT
	Move_Speed = 60
	Gosub MoveSlow
Return

Label "VOLUME UP"
	Move_Slot = SLOT_VOLUME
	Move_Dest = VOLUME_IN_UP
	Move_Speed = 60
	Gosub MoveSlow
Return

Label "VOLUME DW"
	Move_Slot = SLOT_VOLUME
	Move_Dest = VOLUME_IN_DW
	Move_Speed = 60
	Gosub MoveSlow
Return

Label "MEMORY OUT"
	Move_Slot = SLOT_MEMORY
	Move_Dest = MEMORY_OUT
	Move_Speed = 60
	Gosub MoveSlow
Return

Label "MEMORY IN"
	Move_Slot = SLOT_MEMORY
	Move_Dest = MEMORY_IN
	Move_Speed = 30
	Gosub MoveSlow
Return

Label "HEARSET OUT"
	Move_Slot = SLOT_HEARSET
	Move_Dest = HEARSET_OUT
	Move_Speed = 60
	Gosub MoveSlow
Return

Label "HEARSET IN"
	Move_Slot = SLOT_HEARSET
	Move_Dest = HEARSET_IN
	Move_Speed = 60
	Gosub MoveSlow
Return

Label "PEN UP"
	Move_Slot = SLOT_PEN
	Move_Dest = PEN_UP
	Slot(Move_Slot) = Move_Dest
	Wait Seconds 0.1
Return

Label "PEN DOWN"
	Move_Slot = SLOT_PEN
	Move_Dest = PEN_DOWN
	Slot(Move_Slot) = Move_Dest
	Wait Seconds 0.1
Return

Label "PEN SINGLE"
	Gosub "PEN DOWN"
	Wait Seconds 0.35
	Gosub "PEN UP"
Return

Label "PEN DOUBLE"
	Slot(SLOT_PEN) = PEN_DOWN
	Wait Seconds 0.2
	Slot(SLOT_PEN) = PEN_UP
	Wait Seconds 0.1
	Slot(SLOT_PEN) = PEN_DOWN
	Wait Seconds 0.2
	Slot(SLOT_PEN) = PEN_UP
Return

Label "PEN TYPING"
	Move_Slot = SLOT_PEN
	Move_Dest = PEN_DOWN
	Slot(Move_Slot) = Move_Dest
	Wait Seconds PEN_TOUCH_TIME
	Move_Slot = SLOT_PEN
	Move_Dest = PEN_UP
	Slot(Move_Slot) = Move_Dest
	
	'Button 10 Text "PEN UP"
Return

Label "POWER CYCLE"
	Button 4 Color Flashing8 Yellow5 Red5
	Gosub "ONOFF IN"
	Wait Seconds 0.6
	Gosub "ONOFF OUT"
	Wait Seconds 3
	Button 4 Color Red
Return


' =====================================================================
'  SERVO-MOTORS MOVE SLOW
' =====================================================================
Variable Numeric Move_Slot
Variable Numeric Move_Speed
Variable Numeric Move_Dest
Variable Numeric Move_Pos

Label MoveSlow
	Move_Pos = Slot(Move_Slot)
	If Move_Dest > Move_Pos
		Move_Pos = Move_Pos + Move_Speed
		If Move_Pos > Move_Dest
			Move_Pos = Move_Dest
		EndIf
	Else
		Move_Pos = Move_Pos - Move_Speed
		If Move_Pos < Move_Dest
			Move_Pos = Move_Dest
		EndIf
	EndIf
	Slot(Move_Slot) = Move_Pos
	Wait Seconds 0.01
	If Move_Pos = Move_Dest
		Return
	EndIf
Goto MoveSlow

' =====================================================================
'  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
		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
		Wait Seconds 0.01 '<<< loop time
	Next
Return

' =====================================================================
'  STEPPER-MOTORS DRAW CIRCLE
' =====================================================================
Variable Numeric DrawCircle_X 	    ' Center X in mm
Variable Numeric DrawCircle_Y 	    ' Center Y in mm
Variable Numeric DrawCircle_Radius  ' Radius in mm
Variable Numeric DrawCircle_Speed 	' Speed in mm/sec

Variable Numeric DCV1
Variable Numeric DCV2
Variable Numeric DCV3
Variable Numeric DCV4

Label DrawCircle
	' ----------------------------------- test for very low speed
	If DrawCircle_Speed < 0.1
		DrawCircle_Speed = 1
	EndIf
	' ----------------------------------- calc the movement in mm
	DCV3 = DrawCircle_Radius * Math_PI * 2
	' ----------------------------------- calc the loops count
	DCV4 = DCV3 / 0.01 '<<< loop time
	DCV4 = Int(DCV4 / DrawCircle_Speed)
	' ----------------------------------- too much speed ?
	If DCV4 < 16
		DCV4 = 16
	EndIf
	' ----------------------------------- prepare the increments
	DCV1 = 2 * Math_PI / DCV4
	' ----------------------------------- loop for v4 times
	For DCV3 = 0 To DCV4
		' ------------------------------- calc circle
		DCV2 = DCV3 * DCV1
		DestX = DrawCircle_X + DrawCircle_Radius * Sin(DCV2)
		DestY = DrawCircle_Y + DrawCircle_Radius * Cos(DCV2)
		' ------------------------------- move and wait
		Wait Seconds 0.01 '<<< loop time
	Next
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
	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
		Wait Seconds 0.01 '<<< loop time
		' ------------------------------- pen up
		If SLV3 = SLV5
			Slot(SLOT_PEN) = PEN_UP
		EndIf
	Next
Return

' =====================================================================
'  ROTATIONS VERTICAL/HORIZZONTAL
' =====================================================================
Variable Numeric Slot_RotHV_Drive
Variable Numeric Slot_RotHV_Current
Variable Numeric Slot_RotHV_MaxCurrent

Label "ROTATE VERT"
	Slot 998 = 600			'start Signal Scope
	Slot Slot_RotHV_Drive = 600
	Wait Seconds 0.5
	For v1 = 1 To 1300        ' <<< 1000 = 10 sec timeout (approx)
		Wait Seconds 0.009
		If Slot(Slot_RotHV_Current) > Slot_RotHV_MaxCurrent
			Slot Slot_RotHV_Drive = 600
			Slot 998 = 0	'stop Signal Scope
			Button 12 Text "ROTATE HORZ"
			Goto ExitRotateVert
		EndIf
	Next
	Slot Slot_RotHV_Drive = 600
	Slot 998 = 0			'stop Signal Scope
	Button 12 Text "ROTATE HORZ"
'
Label ExitRotateVert
Return

Label "ROTATE MIDL"
	Slot 998 = 600			'start Signal Scope
	Slot Slot_RotHV_Drive = 600
	Wait Seconds 0.5
	For v1 = 1 To 600        ' <<< 1000 = 10 sec timeout (approx)
		Wait Seconds 0.009
		If Slot(Slot_RotHV_Current) > Slot_RotHV_MaxCurrent
			Slot Slot_RotHV_Drive = 600
			Slot 998 = 0	'stop Signal Scope
			Goto ExitRotateMidl
		EndIf
	Next
	Slot Slot_RotHV_Drive = 600
	Slot 998 = 0			'stop Signal Scope
'
Label ExitRotateMidl
Return

Label "ROTATE HORZ"
	Slot 998 = 600			'start Signal Scope
	Slot Slot_RotHV_Drive = 0
	Wait Seconds 0.3
	For v1 = 1 To 1300        ' <<< 1000 = 10 sec timeout (approx)
		Wait Seconds 0.009
		If Slot(Slot_RotHV_Current) > Slot_RotHV_MaxCurrent
			Slot Slot_RotHV_Drive = 600
			Slot 998 = 0	'stop Signal Scope
			Button 12 Text "ROTATE VERT"
			Goto ExitRotateHorz
		EndIf
	Next
	Slot Slot_RotHV_Drive = 600
	Slot 998 = 0			'stop Signal Scope
	Button 12 Text "ROTATE VERT"
	'
Label ExitRotateHorz
Return


'WATCH s1
'WATCH len(s1)
'WATCH v1
'WATCH v2
'WATCH v3
'EXEC DestX = 12
