' ==========================================================
'   Program: Stepper_FindZero.txt
'     Usage: Homing for multiple stepper motors
' ==========================================================

Option Speed 9
Button 1 Text HomeM0
Button 2 Text HomeM1
Button 3 Text HomeM2
Button 4 Text HomeAllMotors
Stop


' ==========================================================
'  HOME MOTORS
' ==========================================================
Label HomeAllMotors
	Gosub HomeM0
	Gosub HomeM1
	Gosub HomeM2
Return

Label HomeM0
	SlotMotor = 1
	SlotSensor = 7
	SpeedSearch = -0.5   ' direction and speed
	SpeedReturn = +0.05  ' direction and speed
	DeclaredZero = 0
	InitialPosition = 1  ' relative to DeclaredZero
	Gosub Home
Return

Label HomeM1
	SlotMotor = 3
	SlotSensor = 8
	SpeedSearch = -0.5   ' direction and speed
	SpeedReturn = +0.05  ' direction and speed
	DeclaredZero = 0
	InitialPosition = 1  ' relative to DeclaredZero
	Gosub Home
Return

Label HomeM2
	SlotMotor = 5
	SlotSensor = 9
	SpeedSearch = +0.5   ' direction and speed
	SpeedReturn = -0.05  ' direction and speed
	DeclaredZero = 0
	InitialPosition = -1 ' relative to DeclaredZero
	Gosub Home
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 Home
	' ------------------------- detach if sensor is active
	If Slot(SlotSensor) > 500
		Slot(SlotMotor) = Slot(SlotMotor) + SpeedReturn
	EndIf
    ' ------------------------- search the sensor
Label SearchHome
	Slot(SlotMotor) = Slot(SlotMotor) + SpeedSearch
	Wait Seconds 0.001
	If Slot(SlotSensor) > 500
		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


'WATCH Abs(Slot(SlotMotor))
'WATCH s2
'WATCH SlotMotor
'WATCH Abs(Slot(SlotMotor))
'WATCH Abs(Slot(SlotMotor))
'EXEC Sin2 = Sin(33)
