'---------------------------------------------------------------------
' File name:  WXM_006_DEMO_10.txt                     Date: 09 APR 2020
'---------------------------------------------------------------------
' MicroStepper stepper  step for run
'---------------------------------------------------------------------
' Gear ratio:
'    Motor0 50:1     CB=1/8 step=1600
'    Motor1 30:1     CB=1/8 Step=1600
'    MotorZ  1: 4mm   C=1/8 Step=1600
'
'   M0 max speed 6000   acc 350  step x mm 228
'   M1 max speed 8000   acc 500  step x mm 133
'   M2 max speed 5000   acc 200  step x mm 400
'----------------------------------------------------------------------
' LIMITS
'  M0:  min:  68   max: 310
'  M1:  min:  32   max: 320
'  M2:  min: 130   max: 290
'----------------------------------------------------------------------

Load POC.jpg
Option Speed 9

Button  1 Text "Home"
Button  1 Color LightCyan
Button  2 Text "Homing_M0"
Button  2 Color LightCyan
Button  3 Text "Homing_M1"
Button  3 Color LightCyan
Button  4 Text "Homing_M2-(Z)"
Button  4 Color LightCyan
'Button  5 Text "Homing_M3-(R)"
'Button  5 Color LightCyan
'Button  6 Text "Homing_M4-(B)"
'Button  6 Color PaleGreen
'Button  7 Text "Homing_M5-(C)"
'Button  7 Color PaleGreen
Button  8 Text "All Sciuscia"
Button  8 Color CornflowerBlue

Button  9 Text "M0 90"
Button  9 Color PaleGreen
Button  10 Text "M0 270"
Button  10 Color PaleGreen
Button  11 Text "M1 90"
Button  11 Color PaleGreen
Button  12 Text "M1 270"
Button  12 Color PaleGreen
Button  13 Text "M2 190"
Button  13 Color PaleGreen
Button  14 Text "M2 60"
Button  14 Color PaleGreen

Button 16 Text "CLOSE ALL"
Button 16 Color Red

Button 17 Text "SCIUSCIA M0"
Button 17 Color Aquamarine
Button 18 Text "SCIUSCIA M1"
Button 18 Color Aquamarine
Button 19 Text "SCIUSC.M2(Z)"
Button 19 Color Aquamarine
'Button 20 Text "SCIUSC.M3(R)"
'Button 20 Color Aquamarine

Button 24 Text "Red"
Button 24 Color Red
Button 25 Text "Yellow"
Button 25 Color Yellow
Button 26 Text "Green"
Button 26 Color Green
Button 27 Text "Blu"
Button 27 Color Blue

Button 28 Text "White"
Button 28 Color White

Button 29 Text "Start Vacuum"
Button 29 Color Blue
Button 30 Text "Stop Vacuum"
Button 30 Color Blue
Button 31 Text "Enable Motors"
Button 31 Color Tan
Button 32 Text "Disable Motors"
Button 32 Color Tan

'Key 5 			Gosub FarFomZero
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 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_SensorM3
Variable Numeric Slot_SensorM4
Variable Numeric Slot_SensorM5
Variable Numeric Slot_SensorM6
Variable Numeric Slot_SensorM7
Variable Numeric Slot_SensorM8

Variable Numeric Slot_EnableMotors
Variable Numeric Slot_LedR
Variable Numeric Slot_LedG
Variable Numeric Slot_LedB
Variable Numeric Slot_Vacuum

Slot_M0 = 1
Slot_M1 = 3
Slot_M2 = 5
Slot_M3 = 7
Slot_M4 = -1
Slot_M5 = -1
Slot_M6 = 37
Slot_M7 = 39
Slot_M8 = 41

Slot_SensorM0 = 19
Slot_SensorM1 = 19
Slot_SensorM2 = 21
Slot_SensorM3 = 40
Slot_SensorM4 = -1
Slot_SensorM5 = -1
Slot_SensorM6 = -1
Slot_SensorM7 = -1
Slot_SensorM8 = -1

Slot_EnableMotors = 11
Slot_LedR = 16
Slot_LedG = 17
Slot_LedB = 18
Slot_Vacuum = 46

Load Vars
PosM0 = v1
PosM1 = v2
PosM2 = v3
PosM3 = v4
PosM4 = v5
PosM5 = v6
Gosub PrintValues

' -------------------------------------- LOAD EXTERNAL PROGRAMS
Load "Theremino_HAL\Theremino_HAL.exe"
Load "Theremino_HAL\Theremino_SlotViewer_1.exe"
Load "Theremino_HAL\Theremino_SlotViewer_2.exe"

' -------------------------------------- TEST MASTER PRESENT
'Wait Seconds 1
For v1 = 1 To 9999
	If Slot(0) >= 1 And Slot(0) <= 4
		Exit
	EndIf

	Print " Invalid master number: " + Slot(0)
	Slot 0 = Recognize
	Beep
	Wait Seconds 2
Next

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

Gosub White
Wait Seconds 0.2

Gosub Red
Wait Seconds 0.2

Gosub Green
Wait Seconds 0.2

Gosub Blu
Wait Seconds 0.2

Load Hide
Stop

' =====================================================================
'  SEQUENCES
' =====================================================================

Label All Sciuscia
	Button 8 Color Flashing8 Red Black
	Gosub Home
	For v8 = 1 To 10
		SequenceFileName = "SEQUENCES\Sequence_AllSciuscia.seq"
		Gosub ExecSequence
		Print "Cycle num.: " + V8
	Next
	Button 8 Color Aquamarine
Return


' =====================================================================
'  SIMPLE OPERATIONS
' =====================================================================

Label M0 90
	Gosub Enable Motors
	Slot Slot_M0 = 90
	Gosub WaitAllMotors
Return

Label M0 270
	Gosub Enable Motors
	Slot Slot_M0 = 270
	Gosub WaitAllMotors
Return

Label M1 90
	Gosub Enable Motors
	Slot Slot_M1 = 90
	Gosub WaitAllMotors
Return

Label M1 270
	Gosub Enable Motors
	Slot Slot_M1 = 270
	Gosub WaitAllMotors
Return

Label M2 190
	Gosub Enable Motors
	Slot Slot_M2 = 190
	Gosub WaitAllMotors
Return

Label M2 60
	Gosub Enable Motors
	Slot Slot_M2 = 60
	Gosub WaitAllMotors
Return

' =====================================================================
'  FUNCTIONS
' =====================================================================

Label All_Sciuscia
	Button 8 Color Flashing8 Red Black
	Gosub Home
	For v8 = 1 To 10
		Gosub SCIUSCIA M0
		Gosub SCIUSCIA M1
		Gosub SCIUSC.M2(Z)
		Print "Cycle num.: " + V8
	Next
	Button 8 Color Aquamarine
Return


Label SCIUSCIA M0
	Button 17 Color Flashing8 Red Black
	Gosub Home
	Gosub Enable Motors
	For v1 = 1 To 30
		Slot Slot_M0 = 70		' M0: min: 68
		Gosub WaitAllMotors
		Slot Slot_M0 = 310      ' M0: max: 310
		Gosub WaitAllMotors
		Print "Cycle num.: " + V9
	Next
	Slot Slot_M0 = 180
	Gosub WaitAllMotors
	Button 17 Color Aquamarine
Return

Label SCIUSCIA M1
	Button 18 Color Flashing8 Red Black
	Gosub Home
	Gosub Enable Motors
	For v9 = 1 To 30
		Slot Slot_M1 = 35		' M1: min: 32
		Gosub WaitAllMotors
		Slot Slot_M1 = 318      ' M1: max: 310
		Gosub WaitAllMotors
		Print "Cycle num.: " + V9
	Next
	Slot Slot_M1 = 180
	Gosub WaitAllMotors
	Button 18 Color Aquamarine
Return

Label SCIUSC.M2(Z)
	Button 19 Color Flashing8 Red Black
	Gosub Home
	Gosub Enable Motors
	For v9 = 1 To 3
		Slot Slot_M2 = 60		' M2:
		Gosub WaitAllMotors
		Slot Slot_M2 = 190      ' M2: max: 190
		Gosub WaitAllMotors
		Print "Cycle num.: " + V9
	Next
	Slot Slot_M2 = 140
	Gosub WaitAllMotors
	Button 19 Color Aquamarine
Return

'Label "SCIUSC.M3(R)"		' Vacuum pick rotation
'	Button 20 Color Flashing8 Red Black
'	Gosub ""Enable Motors""
'	Gosub "Homing_M3-(R)"
'	For v9 = 1 To 10
'		Slot Slot_M3 = 90		' M3: min:
'		Gosub WaitAllMotors
'		Slot Slot_M3 = 270      ' M3: max:
'		Gosub WaitAllMotors
'		Print "Cycle num.: " + V9
'	Next
'	Button 20 Color Aquamarine
'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
	Gosub Homing_M3-(R)
	'Slot Slot_M0 = 90
Return

' ===================================================================
'  HOMING M0 M1 M2 M3 (M4) (M5)
' ===================================================================
Label Homing_M0
	Gosub Enable Motors
	SlotMotor = Slot_M0
	SlotSensor = Slot_SensorM0
	SpeedSearch = -0.11  	' 0.04 search direction and speed
	SpeedReturn = +0.015	' 0.02 return direction and speed
	DeclaredZero = 70.69	' declared switch angle
	InitialPosition = 180	' move to this angle
	Gosub HomeMotor
	PosM0 = 180
	Gosub PrintValues
Return

Label Homing_M1
	Gosub Enable Motors
	SlotMotor = Slot_M1
	SlotSensor = Slot_SensorM1
	SpeedSearch = -0.08		' 0.06 search direction and speed
	SpeedReturn = +0.015		' return direction and speed
	DeclaredZero = 29		'33.3 declared switch angle
	InitialPosition = 180	' move to this angle
	Gosub HomeMotor
	PosM1 = 180
	Gosub PrintValues
Return

Label Homing_M2-(Z)
	Gosub Enable Motors
	SlotMotor = Slot_M2
	SlotSensor = Slot_SensorM2
	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
	PosM2 = 290
	Gosub PrintValues
Return

Label Homing_M3-(R)
	Gosub Enable Motors
	SlotMotor = Slot_M3
	Slot(SlotMotor) = Reset
	Wait Seconds 0.08
	Slot(SlotMotor) = 0
	Wait Seconds 0.3   		'MECHANICAL STABILIZATION
	Slot Slot_M3 = -360
	Gosub WaitAllMotors
	Slot(SlotMotor) = Reset
	Wait Seconds 0.3		'MECHANICAL STABILIZATION
	Slot(SlotMotor) = 0
	Wait Seconds 0.08
	Slot Slot_M3 = 159
	Gosub PrintValues
Return

'Label Homing_M4-(B)
'	Gosub Enable Motors
'	SlotMotor = Slot_M4
'	SlotSensor = Slot_SensorM4
'	SpeedSearch = -0.6		' search direction and speed
'	SpeedReturn = +0.4		' return direction and speed
'	DeclaredZero = 0		' declared switch angle
'	InitialPosition = 10	' move to this angle
'	Gosub HomeMotor
'	PosM4 = 10
'	Gosub PrintValues
'Return

'Label Homing_M5-(C)
'	Gosub Enable Motors
'	SlotMotor = Slot_M5
'	SlotSensor = Slot_SensorM5
'	SpeedSearch = -0.6		' search direction and speed
'	SpeedReturn = +0.4		' return direction and speed
'	DeclaredZero = 0		' declared switch angle
'	InitialPosition = 10	' move to this angle
'	Gosub HomeMotor
'	PosM4 = 10
'	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 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
	Wait Seconds 0.08
	Slot(SlotMotor) = DeclaredZero
	Gosub WaitMotor
	Slot(SlotMotor) = InitialPosition
Return

' ==========================================================
'   Wait until the motor(SlotMotor) is stopped
' ==========================================================
Label WaitMotor
	Wait Seconds 0.1
	Label Wait1
		If Abs(Slot(SlotMotor + 1)) < 0.01
			Return
		EndIf
		Wait Seconds 0.005
	Goto Wait1
Goto WaitMotor

' ==========================================================
'   Wait until all the Motors (0,1,2,3,4,5) are stopped
' ==========================================================
Label WaitAllMotors
	Wait Seconds 0.1          ' <<< ensures that motors are really started
	Label WaitAllMotors2
		If Abs(Slot(Slot_M0 + 1)) < 0.01 Or Slot_M0 < 0
			If Abs(Slot(Slot_M1 + 1)) < 0.01  Or Slot_M1 < 0
				If Abs(Slot(Slot_M2 + 1)) < 0.01  Or Slot_M2 < 0
					If Abs(Slot(Slot_M3 + 1)) < 0.01  Or Slot_M3 < 0
						If Abs(Slot(Slot_M4 + 1)) < 0.01  Or Slot_M4 < 0
							If  Abs(Slot(Slot_M5 + 1)) < 0.01 Or Slot_M5 < 0
								If  Abs(Slot(Slot_M6 + 1)) < 0.01 Or Slot_M6 < 0
									If  Abs(Slot(Slot_M7 + 1)) < 0.01 Or Slot_M7 < 0
										If  Abs(Slot(Slot_M8 + 1)) < 0.01 Or Slot_M8 < 0
											Return
										EndIf
									EndIf
								EndIf
							EndIf
						EndIf
					EndIf
				EndIf
			EndIf
		EndIf
		Wait Seconds 0.01
	Goto WaitAllMotors2
Goto WaitAllMotors

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

Label Start Vacuum
	Slot Slot_Vacuum = 1000
	Wait Seconds 0.5
Return

Label Stop Vacuum
	Slot Slot_Vacuum = 0
	Wait Seconds 0.5
Return

' ===================================================================
'  JOG
' ===================================================================
Label MoveRight
	Gosub Enable Motors
	Gosub SpeedModifiers
	PosM0 = PosM0 + JogSpeed
	Slot Slot_M0 = PosM0
	Gosub PrintValues
Return

Label MoveLeft
	Gosub Enable Motors
	Gosub SpeedModifiers
	PosM0 = PosM0 - JogSpeed
	Slot Slot_M0 = PosM0
	Gosub PrintValues
Return

Label MoveUp
	Gosub Enable Motors
	Gosub SpeedModifiers
	PosM1 = PosM1 + JogSpeed
	Slot Slot_M1 = PosM1
	Gosub PrintValues
Return

Label MoveDown
	Gosub Enable Motors
	Gosub SpeedModifiers
	PosM1 = PosM1 - JogSpeed
	Slot Slot_M1 = PosM1
	Gosub PrintValues
Return

Label MoveUpper
	Gosub Enable Motors
	Gosub SpeedModifiers
	PosM2 = PosM2 + JogSpeed
	Slot Slot_M2 = PosM2
	Gosub PrintValues
Return

Label MoveLower
	Gosub Enable Motors
	Gosub SpeedModifiers
	PosM2 = PosM2 - JogSpeed
	Slot Slot_M2 = PosM2
	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
' ===================================================================
Variable Numeric PosM0
Variable Numeric PosM1
Variable Numeric PosM2
Variable Numeric PosM3
Variable Numeric PosM4
Variable Numeric PosM5

Label PrintValues
	s1 =        "M0 = " + Format(PosM0, "0.00")
	s1 = s1 + "  M1 = " + Format(PosM1, "0.00")
	s1 = s1 + "  M2 = " + Format(PosM2, "0.00")
	s1 = s1 + "  M3 = " + Format(PosM3, "0.00")
	s1 = s1 + "  M4 = " + Format(PosM4, "0.00")
	s1 = s1 + "  M5 = " + Format(PosM5, "0.00")
	Print s1
Return

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

Label CLOSE ALL
	Load CloseApps
Return

Label EventStop
	v1 = PosM0
	v2 = PosM1
	v3 = PosM2
	v4 = PosM3
	v5 = PosM4
	v6 = PosM5
	Save Vars
	Gosub Disable Motors
	Window Sizable
End




' ===================================================================
'  SEQUENCE EXAMPLES
' ===================================================================
Label DemoSeq
	SequenceFileName = "SEQUENCES\Sequence_DemoSeq.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 "YELLOW"
				Gosub Yellow
			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"
						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

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


' =====================================================================
'  END PROGRAM IF MASTER ERROR
' =====================================================================
'Label EventTimer
'	If DecodeCommandSlot(0) = "Master disconnected ERROR"
'		End
'	EndIf
'Return


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

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


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