
Variable Numeric Slot_M0
Variable Numeric Slot_SensorM0
Variable Numeric Slot_CounterReaderOutM0
Variable Numeric Slot_CounterReaderResetM0

Slot_M0 = 1

Slot_SensorM0 = 19

Slot_CounterReaderOutM0 = 25
Slot_CounterReaderResetM0 = 26

Variable Numeric NumTest

Gosub SCIUSCIA M0

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 M0"
	Button 5 Color Flashing8 Red Black
	'
	SlotMotor  = Slot_M0
	SlotSensor = Slot_SensorM0
	SlotCounterReaderOut = Slot_CounterReaderOutM0
	'
	FirstPosition = 270
	SecondPosition = 90
	Tolerance = 0.2
	Gosub ExecuteSciuscia
	Button 5 Color Aquamarine
Return


Variable Numeric ExecuteSciusca1
Label ExecuteSciuscia
	
	NumTest = 300000
	
	For ExecuteSciusca1 = 1 To NumTest
		'
		Slot(SlotMotor) = FirstPosition
		Gosub WaitMotorAndRecovery
		Wait Seconds 0.5
		'
		Slot(SlotMotor) = SecondPosition
		Gosub WaitMotorAndRecovery
		Wait Seconds 0.5
		
	Next
Return

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



' ==========================================================
'  Home the Motor(SlotMotor) to the Sensor(SlotSensor)
' ==========================================================
Variable Numeric SlotMotor
Variable Numeric SlotSensor


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


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