
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.002
	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.01
	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.01
	Gosub ExecuteSciuscia
	Button 7 Color Aquamarine
Return

Label ExecuteSciuscia
	'NumTest = Input "Tell me how many cycle:"
	NumTest = 300000
	Gosub "Enable Motors"
	For v1 = 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
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)
