'---------------------------------------------------------------------
' File name:  WXM_600E_Seq_4.txt                     Date: 13 May 2020
'---------------------------------------------------------------------
' MicroStepper stepper  step for run
'---------------------------------------------------------------------
' Encoder 200 ppr
' HAL recognize 800 ppr
' Stepper 1.8 degree
' Step per full turn = 800 (jumper on B)
'----------------------------------------------------------------------

'Load POC.jpg
Option Speed 9

Button  1 Text "HOME"
Button  1 Color SkyBlue
Button  2 Text "Homing_M0"
Button  2 Color SkyBlue
Button  3 Text "Homing_M1"
Button  3 Color SkyBlue
Button  4 Text "Homing_M2 Z"
Button  4 Color SkyBlue
Button  5 Text "SCIUSCIA M0"
Button  5 Color Aquamarine
Button  6 Text "SCIUSCIA M1"
Button  6 Color Aquamarine
Button  7 Text "SCIUSCIA M2 Z"
Button  7 Color Aquamarine

Button 12 Text "Sciuscia tune"
Button 12 Color Aquamarine

Button 13 Text "Enable Motors"
Button 13 Color Tan
Button 14 Text "Disable Motors"
Button 14 Color Tan

'Button 15 Text "UPDATE FILE"

Button 16 Text "CLOSE ALL"
Button 16 Color Red

Key Right		Gosub MoveRight
Key Left 		Gosub MoveLeft
Key Up 			Gosub MoveUp
Key Down 		Gosub MoveDown
Key Pageup		Gosub MoveUpper
Key Pagedown	Gosub MoveLower
Key M			Gosub "Enable Motors"

Variable Numeric NumMotors
Variable Numeric Slot_M0
Variable Numeric Slot_M1
Variable Numeric Slot_M2
Variable Numeric Slot_M3
Variable Numeric Slot_M4
Variable Numeric Slot_M5
Variable Numeric Slot_M6
Variable Numeric Slot_M7
Variable Numeric Slot_M8
Variable Numeric Slot_SensorM0
Variable Numeric Slot_SensorM1
Variable Numeric Slot_SensorM2
Variable Numeric Slot_CounterReaderOutM0
Variable Numeric Slot_CounterReaderOutM1
Variable Numeric Slot_CounterReaderOutM2
Variable Numeric Slot_CounterReaderOutM3
Variable Numeric Slot_CounterReaderOutM4
Variable Numeric Slot_CounterReaderOutM5
Variable Numeric Slot_CounterReaderOutM6
Variable Numeric Slot_CounterReaderOutM7
Variable Numeric Slot_CounterReaderOutM8
Variable Numeric Slot_CounterReaderResetM0
Variable Numeric Slot_CounterReaderResetM1
Variable Numeric Slot_CounterReaderResetM2
Variable Numeric Slot_CounterReaderResetM3
Variable Numeric Slot_CounterReaderResetM4
Variable Numeric Slot_CounterReaderResetM5
Variable Numeric Slot_CounterReaderResetM6
Variable Numeric Slot_CounterReaderResetM7
Variable Numeric Slot_CounterReaderResetM8
Variable Numeric Slot_EnableMotors
Variable Numeric Slot_LedR
Variable Numeric Slot_LedG
Variable Numeric Slot_LedB


NumMotors = 3
Slot_M0 = 1
Slot_M1 = 3
Slot_M2 = 5

Slot_SensorM0 = 19
Slot_SensorM1 = 20
Slot_SensorM2 = 21

Slot_CounterReaderOutM0 = 25
Slot_CounterReaderResetM0 = 26

Slot_CounterReaderOutM1 = 27
Slot_CounterReaderResetM1 = 28

Slot_CounterReaderOutM2 = 29
Slot_CounterReaderResetM2 = 30

Slot_EnableMotors = 22

Slot_LedR = 16
Slot_LedG = 17
Slot_LedB = 18

Variable Numeric NumTest
Variable Numeric NumCycles
Variable Numeric ErrorNumber

Variable String FileName
Variable String FileHeader

FileName = "Log_Encoder_0_200.txt"
FileHeader = "- DATE - " + "  - TIME - " + "  CYCLE " + " Dest.=0 " + " Dest.=50 " + + " Pulse"
Save StringToFile FileHeader FileName


' -------------------------------------- LOAD EXTERNAL PROGRAMS
Load "Theremino_HAL\Theremino_HAL.exe"
Load "Theremino_HAL\Theremino_SlotViewer.exe"
Load "Theremino_HAL\Theremino_CounterReader1.exe"
Load "Theremino_HAL\Theremino_CounterReader2.exe"
'Load "Theremino_HAL\Theremino_CounterReader3.exe"

' ---------------------------------------------- TEST MASTER PRESENT
For v1 = 1 To 9e99
	For v2 = 1 To 20 ' <<< testing for 2 seconds
		If (Slot(0) >= 1 And Slot(0) <= 2) Or Key("CTRL")
			Exit 2
		EndIf
		Wait Seconds 0.1
	Next
	Print " Invalid master number: " + Slot(0)
	Slot 0 = Recognize
	Beep
Next
Print ""


' -------------------------------------- COLOR

'Gosub "WHITE"
'Wait Seconds 0.1
'Gosub "RED"
'Wait Seconds 0.1
'Gosub "GREEN"
'Wait Seconds 0.1
'Gosub "BLU"
'Wait Seconds 0.1

Stop

' =====================================================================
'  FUNCTIONS
' =====================================================================
' --------------------------------------------------------
' We work with 800 steps per turn
' one step = 0.00125 of turn‬
' --------------------------------------------------------
Variable Numeric Tolerance

Variable Numeric SlotCounterReaderOut
Variable Numeric FirstPosition
Variable Numeric SecondPosition

'Label "SCIUSCIA Enc"
'	Button 2 Color Flashing8 Red Black
'	'NumTest = Input "Tell me how many cycle:"
'	NumTest = 300000
'	Gosub "Enable Motors"
'	Slot 1 = 0
'	Wait Abs(Slot(2)) < 0.01
'
'	For v1 = 1 To NumTest
'		NumCycles = NumCycles + 1
'		If NumCycles > NumTest
'			Gosub SaveVars
'			Return
'		EndIf
'
'		Slot Slot_M0 = 0
'		Wait Abs(Slot(2)) < 0.01
'		Wait Seconds 0.03
'		v1 = Slot(7)
'
'		Slot Slot_M0 = 5
'		Wait Abs(Slot(2)) < 0.01
'		Wait Seconds 0.03
'		v2 = Slot(7)
'
'		Gosub "UPDATE FILE"
'	Next
'
'	Button 4 Color Aquamarine
'Return

Label "SCIUSCIA M0"
	Button 5 Color Flashing8 Red Black
	Gosub "Homing_M0"
	'
	SlotMotor  = Slot_M0
	SlotSensor = Slot_SensorM0
	SlotCounterReaderOut = Slot_CounterReaderOutM0
	'
	FirstPosition = 270
	SecondPosition = 90
	Tolerance = 0.2
	Gosub ExecuteSciuscia
	Button 5 Color Aquamarine
Return

Label "SCIUSCIA M1"
	Button 6 Color Flashing8 Red Black
	Gosub "Homing_M1"
	'
	SlotMotor  = Slot_M1
	SlotSensor = Slot_SensorM1
	SlotCounterReaderOut = Slot_CounterReaderOutM1
	'
	FirstPosition = 270
	SecondPosition = 90
	Tolerance = 0.2
	Gosub ExecuteSciuscia
	Button 6 Color Aquamarine
Return

Label "SCIUSCIA M2 Z"
	Button 7 Color Flashing8 Red Black
	Gosub "Homing_M2 Z"
	'
	SlotMotor  = Slot_M2
	SlotSensor = Slot_SensorM2
	SlotCounterReaderOut = Slot_CounterReaderOutM2
	'
	FirstPosition = 170
	SecondPosition = 150
	Tolerance = 0.2
	Gosub ExecuteSciuscia
	Button 7 Color Aquamarine
Return

Variable Numeric ExecuteSciusca1
Label ExecuteSciuscia
	'NumTest = Input "Tell me how many cycle:"
	NumTest = 300000
	Gosub "Enable Motors"
	For ExecuteSciusca1 = 1 To NumTest
		'
		Slot(SlotMotor) = FirstPosition
		Gosub WaitMotorAndRecovery
		Wait Seconds 0.5
		'
		Slot(SlotMotor) = SecondPosition
		Gosub WaitMotorAndRecovery
		Wait Seconds 0.5
		'
		'If Key("CTRL")
		'	Exit
		'EndIf
	Next
Return

Label ErrorRecovery
	'
	's1 = Message "MISALIGNMENT: " + s2
	s1 = "YES"
	If S1 = "YES"
		'Gosub "UPDATE FILE"
		'
		v4 = Slot(SlotMotor)
		Gosub "Disable Motors"
		Slot(SlotMotor) = Slot(SlotCounterReaderOut)
		Gosub WaitMotor
		Gosub "Enable Motors"
		Slot(SlotMotor) = v4
		Gosub WaitMotor
	Else
		End
	EndIf
	
	s2 = Slot(SlotMotor) - Slot(SlotCounterReaderOut)
	Print "ERROR:" + s2
	
											 'Breakpoint
Return

Label "Sciuscia tune"
	SlotMotor  = Slot_M0
	Button 12 Color Flashing8 Red Black
	Gosub "Enable Motors"
	For v1 = 1 To 100
		Slot(SlotMotor) = 0
		Gosub WaitMotor
		Wait Seconds 0.3
		
		Slot(SlotMotor) = 45
		Gosub WaitMotor
		Wait Seconds 0.3
	Next
	Button 12 Color Aquamarine
Return

Label "UPDATE FILE"
	v1 = Slot(Slot_M0)
	v2 = Slot(Slot_CounterReaderOutM0)
	v3 = v1 - v2
	S3 = Now
	s3 = S3 + "  " + Format(NumCycles, "00000")
	s3 = s3 + "   " + Format(v1, "00000")
	s3 = s3 + "   " + Format(v2, "00000")
	s3 = S3 + "   " + Format(v3, "00000")
	Save StringToFile s3 FileName
	'Print s3
Return

Label SaveVars
	v1 = NumCycles
	v2 = ErrorNumber
	Save Vars
Return

Label LoadVars
	Load Vars
	NumCycles = v1
	ErrorNumber = v2
Return

Label "Enable Motors"
	Slot Slot_EnableMotors = 1000 		'Enable Motors
Return

Label "Disable Motors"
	Slot Slot_EnableMotors = 0 	        'Disable Motors
Return


' =============== HOME ==============================================
Label Home
	Gosub "Enable Motors"
	Gosub "Homing_M2 Z"
	Gosub "Homing_M0"
	Gosub "Homing_M1"
Return

' ===================================================================
'  HOMING M0 M1 M2
' ===================================================================
Label "Homing_M0"
	Gosub "Enable Motors"
	SlotMotor = Slot_M0
	SlotSensor = Slot_SensorM0
	SlotCounterReaderReset = Slot_CounterReaderResetM0
	SpeedSearch = -0.1  	' search direction and speed
	SpeedReturn = +0.01	    ' return direction and speed
	DeclaredZero = 90		' declared switch angle
	InitialPosition = 180	' move to this angle
	Gosub HomeMotor
	'Gosub PrintValues
	Button 2 Color SkyBlue
Return

Label "Homing_M1"
	Gosub "Enable Motors"
	SlotMotor = Slot_M1
	SlotSensor = Slot_SensorM1
	SlotCounterReaderReset = Slot_CounterReaderResetM1
	SpeedSearch = -0.1		' 0.06 search direction and speed
	SpeedReturn = +0.015	' return direction and speed
	DeclaredZero = 90		' 33.3 declared switch angle
	InitialPosition = 180	' move to this angle
	Gosub HomeMotor
	'Gosub PrintValues
	Button 3 Color SkyBlue
Return

Label "Homing_M2 Z"
	Gosub "Enable Motors"
	SlotMotor = Slot_M2
	SlotSensor = Slot_SensorM2
	SlotCounterReaderReset = Slot_CounterReaderResetM2
	SpeedSearch = +0.04		' search direction and speed
	SpeedReturn = -0.02		' return direction and speed
	DeclaredZero = 192		' declared switch angle
	InitialPosition = 190	' move to this angle
	Gosub HomeMotor
	'Gosub PrintValues
	Button 4 Color SkyBlue
Return


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

Label HomeMotor
	' ------------------------- 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")
		Gosub WaitMotor
		Goto DetachSensor
	EndIf
Goto SearchHome
    ' ------------------------- detach from the sensor
Label DetachSensor
	Slot(SlotMotor) = Slot(SlotMotor) + SpeedReturn
	Wait Seconds 0.001
	If Slot (SlotSensor) < 500
		Gosub WaitMotor
		Goto AssignZero
	EndIf
Goto DetachSensor
    ' ------------------------- reset and move to InitialPos
Label AssignZero
	Slot(SlotMotor) = Reset
	Slot(SlotCounterReaderReset) = Reset
	Wait Seconds 0.08
	Slot(SlotMotor) = DeclaredZero
	Slot(SlotCounterReaderReset) = DeclaredZero
	Gosub WaitMotor
	Slot(SlotMotor) = InitialPosition
	Slot(SlotCounterReaderReset) = InitialPosition
Return

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

Label WaitMotorAndRecovery
	Wait Seconds 0.1
	For WaitMot1 = 1 To 9e99
		If Abs(Slot(SlotMotor + 1)) < 0.01
			Exit
		EndIf
		Wait Seconds 0.005
	Next
	Wait Seconds 0.2
	If Abs(Slot(SlotMotor) - Slot(SlotCounterReaderOut)) >= Tolerance
		Gosub ErrorRecovery
	EndIf
Return

' ==========================================================
'   Wait until all the Motors are stopped
' ==========================================================
Label WaitAllMotors
	Wait Seconds 0.1 ' <<< ensures that motors are really started
	For MotorNumber = 0 To NumMotors - 1
		Gosub MotorNumberToSlot
		If Abs(Slot(MotorSlot + 1)) > 0.01
			MotorNumber = 0
		EndIf
		Wait Seconds 0.01
	Next
Return

Label WaitAllMotorsAndRecovery
	Wait Seconds 0.1 ' <<< ensures that motors are really started
	For MotorNumber = 0 To NumMotors - 1
		Gosub MotorNumberToSlot
		If Abs(Slot(MotorSlot + 1)) > 0.01
			MotorNumber = 0
		EndIf
		Wait Seconds 0.01
	Next
	
	For MotorNumber = 0 To NumMotors - 1
		Gosub MotorNumberToSlot
		Gosub MotorNumberToSlotCounterReaderOut
		If Abs(Slot(MotorSlot) - Slot(SlotCounterReaderOut)) >= Tolerance
			Gosub "ErrorRecovery"
		EndIf
	Next
Return



' ===================================================================
'  LED COLORS
' ===================================================================
Label "Red"
	Slot Slot_LedR = 1000
	Slot Slot_LedG = 0
	Slot Slot_LedB = 0
Return

Label "Green"
	Slot Slot_LedR = 0
	Slot Slot_LedG = 1000
	Slot Slot_LedB = 0
Return

Label "Blu"
	Slot Slot_LedR = 0
	Slot Slot_LedG = 0
	Slot Slot_LedB = 1000
Return

Label "White"
	Slot Slot_LedR = 1000
	Slot Slot_LedG = 1000
	Slot Slot_LedB = 1000
Return

Label "Yellow"
	Slot Slot_LedR = 1000
	Slot Slot_LedG = 900
	Slot Slot_LedB = 0
Return

Label "NOCOLOR"
	Slot Slot_LedR = 0
	Slot Slot_LedG = 0
	Slot Slot_LedB = 0
Return

' ===================================================================
'  SEQUENCE EXAMPLES
' ===================================================================
Label "DemoSeq"
	SequenceFileName = "SEQUENCES\Sequence_DemoSeq.seq"
	Gosub ExecSequence
Return

Label "Shoes Demo"
	Button 9 Color Flashing8 Black White
	Gosub "Enable Motors"
	Gosub HOME
	For v1 = 1 To 2    '9e99
		SequenceFileName = "SEQUENCES\Sequence_RoundDemo.seq"
		Gosub ExecSequence
	Next
	Button 9 Color CornflowerBlue
Return

Label "Move Ribbon"
	SequenceFileName = "SEQUENCES\Sequence_MoveRibbon.seq"
	Gosub ExecSequence
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
	' ------------------------------------------
	Variable Numeric ExecSequenceStartTime
	ExecSequenceStartTime = ElapsedTime
	' ------------------------------------------
	SequenceLength = GetSeparatedStringCount(Sequence, CRLF)
	For SequenceIndex = 0 To SequenceLength-1
		SequenceCmd = GetSeparatedString(Sequence, SequenceIndex, CRLF)
		SequenceCmd = Replace(SequenceCmd, Chr(9), " ") 'TABS to SPACES
		SequenceCmd = RemoveComments(SequenceCmd)
		SequenceCmd = Trim(SequenceCmd)
		'
		Print " Executing command: " + SequenceCmd
		'
		Select UCase(SequenceCmd)
            ' -------------------------------------------------- End sequence
			Case "ENDSEQ"
				SequenceIndex = SequenceLength-1
		   ' --------------------------------------------------- ACTIONS
			Case "ENABLEMOTORS"     'enable movement motors
				Gosub "Enable Motors"
			Case "DISABLEMOTORS"    'disable movement motors
				Gosub "Disable Motors"
			'Case "STARTVACUUM"       'Start Vacuum
			'	Gosub "Start Vacuum"
			'Case "STOPVACUUM"        'Stop Vacuum
			'	Gosub "Stop Vacuum"
			Case "HOME"
				Gosub "HOME"
			Case "HOMING_M0"
				Gosub "Homing_M0"
			Case "HOMING_M1"
				Gosub "Homing_M1"
			'Case "HOMING_M2"
			'	Gosub "Homing_M2(Z)"
			'Case "HOMING_M3(R)"
			'	Gosub "Homing_M3(R)"
			Case "CLOSEALL"
				Gosub "CLOSE ALL"
			' -------------------------------------------------- COLORS
			Case "RED"
				Gosub "RED"
			Case "GREEN"
				Gosub "GREEN"
			Case "BLU"
				Gosub "BLU"
			Case "WHITE"
				Gosub "WHITE"
			' -------------------------------------------------- WAIT MOTORS
			Case "WAITALLMOTORS"
				Gosub WaitAllMotors
            ' -------------------------------------------------- With parameters
			CaseElse
				s1 = GetSeparatedString(SequenceCmd, 0)
				Select UCase(s1)
					Case "MOVE"
						MotorNumber = GetSeparatedString(SequenceCmd, 1)
						Gosub MotorNumberToSlot
						Slot(MotorSlot) = GetSeparatedString(SequenceCmd, 2)
						'Gosub WaitAllMotors
					Case "MOVESLOW"
						MotorNumber = GetSeparatedString(SequenceCmd, 1)
						Gosub MotorNumberToSlot
						MoveSlow_Dest = GetSeparatedString(SequenceCmd, 2)
						MoveSlow_Speed = GetSeparatedString(SequenceCmd, 3)
						Gosub MoveSlow
					'Case "PAUSE"
					'	Slot_Pause = 1000
					'	Gosub "RED"
					'	Wait Seconds GetSeparatedString(SequenceCmd, 1)
					CaseElse
						If s1 <> ""
							Controls OpenTextBox
							Print CRLF + " Unrecognized command: " + SequenceCmd
							Beep
							Stop
						EndIf
				EndSelect
		EndSelect
	Next
	SequenceFileName = Replace(SequenceFileName,".seq","")
	Print SequenceFileName + " executed in " + Format(ElapsedTime - ExecSequenceStartTime, "0.00") + " seconds"
Return

' =====================================================================
'  MOTOR NUMBER TO SLOT CONVERSION
' =====================================================================
Variable Numeric MotorNumber
Variable Numeric MotorSlot

Label MotorNumberToSlot
	Select MotorNumber
		Case 0
			MotorSlot = Slot_M0
		Case 1
			MotorSlot = Slot_M1
		Case 2
			MotorSlot = Slot_M2
		Case 3
			MotorSlot = Slot_M3
		Case 4
			MotorSlot = Slot_M4
		Case 5
			MotorSlot = Slot_M5
		Case 6
			MotorSlot = Slot_M6
		Case 7
			MotorSlot = Slot_M7
		Case 8
			MotorSlot = Slot_M8
	EndSelect
Return

Label MotorNumberToSlotCounterReaderOut
	Select MotorNumber
		Case 0
			SlotCounterReaderOut = Slot_CounterReaderOutM0
		Case 1
			SlotCounterReaderOut = Slot_CounterReaderOutM1
		Case 2
			SlotCounterReaderOut = Slot_CounterReaderOutM2
		Case 3
			SlotCounterReaderOut = Slot_CounterReaderOutM3
		Case 4
			SlotCounterReaderOut = Slot_CounterReaderOutM4
		Case 5
			SlotCounterReaderOut = Slot_CounterReaderOutM5
		Case 6
			SlotCounterReaderOut = Slot_CounterReaderOutM6
		Case 7
			SlotCounterReaderOut = Slot_CounterReaderOutM7
		Case 8
			SlotCounterReaderOut = Slot_CounterReaderOutM8
	EndSelect
Return

Label MotorNumberToSlotCounterReaderReset
	Select MotorNumber
		Case 0
			SlotCounterReaderReset = Slot_CounterReaderResetM0
		Case 1
			SlotCounterReaderReset = Slot_CounterReaderResetM1
		Case 2
			SlotCounterReaderReset = Slot_CounterReaderResetM2
		Case 3
			SlotCounterReaderReset = Slot_CounterReaderResetM3
		Case 4
			SlotCounterReaderReset = Slot_CounterReaderResetM4
		Case 5
			SlotCounterReaderReset = Slot_CounterReaderResetM5
		Case 6
			SlotCounterReaderReset = Slot_CounterReaderResetM6
		Case 7
			SlotCounterReaderReset = Slot_CounterReaderResetM7
		Case 8
			SlotCounterReaderReset = Slot_CounterReaderResetM8
	EndSelect
Return

' =====================================================================
'  STEPPER-MOTORS MOVE SLOW
' =====================================================================
Variable Numeric MoveSlow_Slot 		' motor slot
Variable Numeric MoveSlow_Dest 		' destination in mm or degree
Variable Numeric MoveSlow_Speed 	' speed in mm/sec

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

Label MoveSlow
	' ----------------------------------- get the actual position
	MSV2 = Slot(MoveSlow_Slot)
	' ----------------------------------- test for very low speed
	If MoveSlow_Speed < 0.1
		MoveSlow_Speed = 1
	EndIf
	' ----------------------------------- calc the movement in mm
	MSV1 = MoveSlow_Dest - MSV2
	MSV3 = MSV1 * MSV1
	MSV3 = Sqrt(MSV3)
	' ----------------------------------- calc the loops count
	MSV4 = MSV3 / 0.01 '<<< loop time
	MSV4 = Int(MSV4 / MoveSlow_Speed)
	' ----------------------------------- too much speed ?
	If MSV4 = 0
		MSV2 = MoveSlow_Dest
		Slot(MoveSlow_Slot) = MSV2
		Return
	EndIf
	' ----------------------------------- prepare the increments
	MSV1 = (MoveSlow_Dest - MSV2) / MSV4
	' ----------------------------------- loop for v4 times
	For MSV3 = 1 To MSV4
		' ------------------------------- increment, move and wait
		MSV2 = MSV2 + MSV1
		Slot(MoveSlow_Slot) = MSV2
		Wait Seconds 0.01 '<<< loop time
	Next
	Gosub WaitAllMotors
Return


' ===================================================================
'  JOG
' ===================================================================
Label MoveRight
	Gosub "Enable Motors"
	Gosub SpeedModifiers
	Slot Slot_M0 = Slot(Slot_M0) + JogSpeed
	Gosub PrintValues
Return

Label MoveLeft
	Gosub "Enable Motors"
	Gosub SpeedModifiers
	Slot Slot_M0 = Slot(Slot_M0) - JogSpeed
	Gosub PrintValues
Return

Label MoveUp
	Gosub "Enable Motors"
	Gosub SpeedModifiers
	Slot Slot_M1 = Slot(Slot_M1) + JogSpeed
	Gosub PrintValues
Return

Label MoveDown
	Gosub "Enable Motors"
	Gosub SpeedModifiers
	Slot Slot_M1 = Slot(Slot_M1) - JogSpeed
	Gosub PrintValues
Return

Label MoveUpper
	Gosub "Enable Motors"
	Gosub SpeedModifiers
	Slot Slot_M2 = Slot(Slot_M2) + JogSpeed
	Gosub PrintValues
Return

Label MoveLower
	Gosub "Enable Motors"
	Gosub SpeedModifiers
	Slot Slot_M2 = Slot(Slot_M2) - JogSpeed
	Gosub PrintValues
Return

Variable Numeric JogSpeed
Label SpeedModifiers
	JogSpeed = 0.1
	Select True
		Case Key("SHIFT")
			JogSpeed = JogSpeed * 100
		Case Key("CTRL")
			JogSpeed = JogSpeed * 10
		Case Key("ALT")
			JogSpeed = JogSpeed * 0.1
	EndSelect
Return

' ===================================================================
'  PRINT MOTOR POSITIONS
' ===================================================================

Label PrintValues
	s1 =        "M0 = " + Format(Slot(Slot_M0), "0.00")
	s1 = s1 + "  M1 = " + Format(Slot(Slot_M1), "0.00")
	s1 = s1 + "  M2 = " + Format(Slot(Slot_M2), "0.00")
	s1 = s1 + "  M3 = " + Format(Slot(Slot_M3), "0.00")
	s1 = s1 + "  M4 = " + Format(Slot(Slot_M4), "0.00")
	s1 = s1 + "  M5 = " + Format(Slot(Slot_M5), "0.00")
	Print s1
Return


'---------------------------------------------------------------------------------
'----- CLOSING -------------------------------------------------------------------
'---------------------------------------------------------------------------------

Label "CLOSE ALL"
	Load CloseApps
Return

Label "EventStop"
	Save Vars
	Gosub "Disable Motors"
	Window Sizable
End


'====== DUMMY AREA ===============================================================

'====== TO DO AREA ===============================================================


'WATCH SequenceCmd
'WATCH s1
'WATCH UCase(SequenceCmd)
