' Program name: MM3D_MB500_01.txt                               14 Mar 2021
'        Autor: leonardo DE PALO
'   Begin date: 27 Mar 2021

' ========== VARIABLE ASSIGNEMENT =====================================
' v1 to Joystick
' v2 to Joystick
' v3 to Joystick
' v4 to Working loop
' v5 to Working loop

' =========== HARDWARE SET UP AT THE END OF THE LISTING ===============



Load MB500_early.jpg

'Speed 9
Button 1 Text HomeAll
Button 2 Text Home_X
Button 3 Text Home_Y
Button 4 Text Home_Z
'Button 5 Text Joy
'Button 6 Text MoneyPlay
'Button 7 Text Pump
'Button 8 Text DisableMotor

Load Theremino_HAL\Theremino_HAL.exe
Load Theremino_HAL\theremino_SlotViewer.exe
"
'Load Theremino_HAL\theremino_Gamepad.exe


'Key 5 			Gosub FarFomZero
Key Right		Gosub MoveRightM0
Key Left 		Gosub MoveLeftM0
Key Up 			Gosub MoveRightM1
Key Down 		Gosub MoveLeftM1
Key Pageup		Gosub MoveUp
Key Pagedown	Gosub MoveDown

'Gosub DisableMotor
Stop



' =============== HOME ==============================================
Label HomeAll
	Gosub EnableMotor
	Slot(3) = Slot(3) + 30 		'disengage M1
	Slot(1) = Slot(1) + 30		'disengage M0
	Gosub Home_X
	Gosub Home_Y
	Gosub Home_Z
	Gosub WaitAllMotors
	'Gosub DisableMotor
Return
' ===================================================================


' =============== HOMING MOTOR X ====================================
Label Home_X
	Gosub EnableMotor
	Slot(3) = Slot(3) + 30 		'disengage M1
	Slot(1) = Slot(1) + 30		'disengage M0
	Label Home_X_1
		Slot(1) = Slot(1) - 0.4		' direction and speed goto switch
		Wait Seconds 0.01			' do not alter, loop time 0.02 Sec
		If Slot(7) > 500 			' limit switch
			Goto Home_X_2
		EndIf
		Goto Home_X_1
Label Home_X_2
	Slot(1) = Slot(1) + 0.8		' direction and speed return from switch
	Wait Seconds 0.02			' do not alter, loop time 0.02 Sec
	If Slot (7) < 500			' limit switch
		Goto Home_X_3
	EndIf
	Goto Home_X_2
Label Home_X_3
	Slot 1 = Reset				' Reset the slot value
	Wait Seconds 0.05
	Slot 1 = 75					' declared switch angle
	Wait Seconds 0.05
	Slot 1 = 180					' move to this angle
Return
' ===================================================================


' =============== HOMING MOTOR Y ====================================
Label Home_Y
	Gosub EnableMotor
	Slot(1) = Slot(1) + 30	'disengage M0
	Slot(3) = Slot(3) + 30	'disengage M1
	Label Home_Y_1
		Slot(3) = Slot(3) - 0.4		' direction and speed goto switch
		Wait Seconds 0.01			' do not alter, loop time 0.02 Sec
		If Slot(8) > 500 			' limit switch
			Goto Home_y_2
		EndIf
	Goto Home_Y_1
	
Label Home_y_2
	Slot(3) = Slot(3) + 0.8    ' direction and speed return from switch
	Wait Seconds 0.02			' do not alter, loop time 0.02 Sec
	If Slot (8) < 500			' limit switch
		Goto Home_y_3
	EndIf
Goto Home_y_2

Label Home_y_3
	Slot 3 = Reset				' Reset the slot value
	Wait Seconds 0.1
	Slot 3 = 30				    ' Declared switch angle
	Wait Seconds 0.1
	Slot 3 = 180				' move to this angle
Return
' ===================================================================


' =============== HOMING MOTOR Z ====================================
Label Home_Z
	Gosub EnableMotor
	Label Home_Z_1
		Slot(5) = Slot(5) + 1		' direction and speed goto switch
		Wait Seconds 0.06			' do not alter, loop time 0.02 Sec
		If Slot(9) > 500 			' limit switch
			Goto Home_Z_2
		EndIf
		Goto Home_Z_1
Label Home_Z_2
	Slot(5) = Slot(5) - 0.5	' direction and speed return from switch
	Wait Seconds 0.06			' do not alter, loop time 0.02 Sec
	If Slot (9) < 500			' limit switch
		Goto Home_Z_3
	EndIf
	Goto Home_Z_2
Label Home_Z_3
	Slot 5 = Reset				' Reset the slot value
	Wait Seconds 0.05
	Slot 5 = 149 				' declared initial position
	Wait Seconds 0.05
	Slot 5 = 135				' move to start position
Return
' ===================================================================

Label MoveRightM0
	Gosub EnableMotor
	Slot(1) = Slot(1) - 1
Return

Label MoveLeftM0
	Gosub EnableMotor
	Slot(1) = Slot(1) + 1
Return

Label MoveRightM1
	Gosub EnableMotor
	Slot(3) = Slot(3) + 1
Return

Label MoveLeftM1
	Gosub EnableMotor
	Slot(3) = Slot(3) - 1
Return

Label MoveUp
	Gosub EnableMotor
	Slot(5) = Slot(5) + 1
Return

Label MoveDown
	Gosub EnableMotor
	Slot(5) = Slot(5) - 1
Return

' =============== JOISTICK MANAGEMENT ==========================
Label Joy
	Gosub EnableMotor
	v1 = Slot(13)
	If v1 > 550 								'move to Right M0
		Slot(1) = Slot(1) - (v1 - 550) * 0.01 	'incremental move
	EndIf
	If v1 < 450									'move to Left M0
		Slot(1) = Slot(1) + (450 - v1) * 0.01	'incremental move
	EndIf
	v2 = Slot(14)
	If v2 > 550 							'move to left M1
		Slot(3) = Slot(3) - (v2 - 550) * 0.01 	'incremental move
	EndIf
	If v2 < 450 							'move to right M1
		Slot(3) = Slot(3) + (450 - v2) * 0.01	'incremental move
	EndIf
	v3 = Slot(15)
	If v3 > 550 		   					'move to up M2 (Z)
		Slot(5) = Slot(5) + (450 - v3) * 0.001	'incremental move
	EndIf	 					'nota: joy Z verticale difettoso
	If v3 < 450 							'move to down M2 (Z)
		Slot(5) = Slot(5) - (v3 - 550) * 0.001 	'incremental move
	EndIf
	If Slot(23) < 550
		Slot(12) = 0							'stop pump
	Else
		Slot(12) = 1000							'start pump
	EndIf
	If Slot(26) > 500 							'Button 4 to exit joy mode
		Return
	EndIf
Goto Joy
Return

'---------------------------------------------------------------------------------
'----- subroutine area -----------------------------------------------------------
'---------------------------------------------------------------------------------
' =============== ENABLE / DISABLE MOTORS ==========================
Label EnableMotor
	Slot 11 = 1000
	Wait Seconds 0.05
Return

Label DisableMotor
	Slot 11 = 0
	Wait Seconds 0.05
Return

' =============== ENABLE / DISABLE & TEST PUMP =====================
Label StartPump
	Slot 12 = 1000
	Wait Seconds 0.05
Return

Label StopPump
	Slot 12 = 0
	Wait Seconds 0.05
Return

Label Pump
	Gosub StartPump
	Wait Seconds 0.5
	Gosub StopPump
Return

' =============== W A I T   A L L   M O T O R S ====================
Label WaitAllMotors
' ------------------ this Wait ensures that the motors are really started
	Wait Seconds 0.1
	Label WaitAllMotors2
		If Abs(Slot(2)) < 0.01 And Abs(Slot(4)) < 0.01 And Abs(Slot(6)) < 0.01
			Return
		EndIf
		Wait Seconds 0.01
	Goto WaitAllMotors2
' ------------------ this line is required to indent correctly
Goto WaitAllMotors


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

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



'====== SET UP AREA ===============================================================

' =========== HARDWARE SET UP DVR8825 =============================================
' Motor M0 current 200 mA
' Microstep "ABC" 32 Speed 10000 Acc 300 Step/mm 74.2   reduction ratio   4:1
' Motor M1 current 200 mA
' Microstep "ABC" 32 Speed 10000 Acc 300 Step/mm 74.2   reduction ratio   4:1
' Motor Z current 200 mA
' Microstep " BC"  8 Speed  1000 Acc 100 Step 400       l	eadscrew: 4mm for one turn

' StepperDriver V2 board jumper set up with DRV8825:
' Motor M0: A=insert    B=remove   C=Insert   1/32 step 6400 step/run
' 1    0 1000 100 10000 300  52
' Motor M1: A=insert    B=remove   C=Insert   1/32 step 6400 step/run
' 3 1000    0 100 10000 300  52
' Motor MZ: A=insert    B=insert   C=remove   1/4  step  800 step/run current 200 mA
' 5 1000    0 100  1000 100 400

' ENABLE stepper motors C7:
' from center to ON = always on
' from center to Out2 = SW controlled
' Gear ratio for M0 and M1 1:4   Step x mm = 800 x 360 / 360 = 59.556 ???????????
' Leadscrew: 4mm for one turn
' Inductive sensor: Blue=GND  Brown=+5VDC  Black=output


'WATCH New watch
