'---------------------------------------------------------------------
' File name:  WXM_ENCODER_200_12_arm.txt             Date: 23 APR 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

Variable Numeric Slot_M0
Variable Numeric Slot_M1
Variable Numeric Slot_M2
Variable Numeric Slot_SensorM0
Variable Numeric Slot_SensorM1
Variable Numeric Slot_SensorM2
Variable Numeric Slot_CounterReaderOutM0
Variable Numeric Slot_CounterReaderResetM0
Variable Numeric Slot_CounterReaderOutM1
Variable Numeric Slot_CounterReaderResetM1
Variable Numeric Slot_CounterReaderOutM2
Variable Numeric Slot_CounterReaderResetM2
Variable Numeric Slot_EnableMotors
Variable Numeric Slot_LedR
Variable Numeric Slot_LedG
Variable Numeric Slot_LedB



Slot_M0 = 1
Slot_M1 = 3
Slot_M2 = 5

Slot_SensorM0 = 19
Slot_SensorM1 = 20
Slot_SensorM2 = 21

Slot_CounterReaderOutM0 = 26
Slot_CounterReaderResetM0 = 25

Slot_CounterReaderOutM1 = 28
Slot_CounterReaderResetM1 = 27

Slot_CounterReaderOutM2 = 30
Slot_CounterReaderResetM2 = 29

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 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 WaitMotor
		Wait Seconds 0.5
		'If Abs(Slot(SlotMotor) - Slot(SlotCounterReaderOut)) >= Tolerance
		'	Gosub "ErrorRecovery"
		'EndIf
		Wait Seconds 0.5
		'
		Slot(SlotMotor) = SecondPosition
		Gosub WaitMotor
		Wait Seconds 0.5
		'If Abs(Slot(SlotMotor) - Slot(SlotCounterReaderOut)) >= Tolerance
		'	Gosub "ErrorRecovery"
		'EndIf
		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
' ==========================================================
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



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


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