;###############################################################
;
; Step1CW
;
; Moves one step Clockwise
;
; Uses Finite state machine to determine which combination is next
;
;	State Number	Coil 2 Direction	Coil 1 Direction
;
;		3			F			F
;		1			B			F
;		0			B			B
;		2			F			B
;
; to turn clockwise, move down the table
; to turn counterclockwise, move up the table
;
;###############################################################

Declare Function step1cw()

Select Case step1state

	Case 0
		SetMotor	coil12,"F",100
		SetMotor	coil11,"B",100
		step1state = 2
;		Wait stepdelay

	Case 1
		SetMotor	coil12,"B",100
		SetMotor	coil11,"B",100
		step1state = 0
;		Wait stepdelay

	Case 2
		SetMotor	coil12,"F",100
		SetMotor	coil11,"F",100
		step1state = 3
;		Wait stepdelay


	Case 3
		SetMotor	coil12,"B",100
		SetMotor	coil11,"F",100
		step1state = 1
;		Wait stepdelay
	
	
End Select


End Function

;###############################################################
;
; Step1CCW
;
; Moves one step CounterClockwise;
; Uses Finite state machine to determine which combination is next
; see StepCW for State table
;###############################################################

Declare Function step1ccw()
Select Case step1state

	Case 0
		SetMotor	coil12,"B",100
		SetMotor	coil11,"F",100
		step1state = 1
;		Wait stepdelay

	Case 1
		SetMotor	coil12,"F",100
		SetMotor	coil11,"F",100
		step1state = 3
;		Wait stepdelay

	Case 2
		SetMotor	coil11,"B",100
		SetMotor	coil12,"B",100
		step1state = 0
;		Wait stepdelay

	Case 3
		SetMotor	coil11,"B",100
		SetMotor	coil12,"F",100
		step1state = 2
;		Wait stepdelay
	
End Select


End Function

;###############################################################
;
; StepMotor1
;
; Moves a number of steps, in one direction.
; Accelerates and decelerates symetrically.
;
;###############################################################

Declare Function stepmotor1(direction As Byte, steps As Word)

Declare Word	step1count
Declare Word	decelzone

decelzone = steps - decelband

step1count = 0
Do 
	If direction = "F" Then
		Call step1cw()
	Else
		Call step1ccw()
	End If

	If step1count < decelband Then
		timetowait = decelband - step1count
	Else
		
	If step1count > decelzone Then
		timetowait = step1count - decelzone
	Else
		timetowait = 0
	End If
	End If

	If timetowait <> 0 Then
		Wait timetowait
	End If

	Increment step1count 
	stepdisplay = LowByte(step1count)
	DisplayNumber stepdisplay

	If stepdisplay = 0 Then
		step1count = step1count - 256
	End If

Until step1count = steps

; now put on correct holding current
Select Case step1state

	Case 0
		SetMotor	coil12,"B",holdpower
		SetMotor	coil11,"B",holdpower

	Case 1
		SetMotor	coil12,"B",holdpower
		SetMotor	coil11,"F",holdpower


	Case 2
		SetMotor	coil12,"B",holdpower
		SetMotor	coil11,"F",holdpower

	Case 3
		SetMotor	coil11,"F",holdpower
		SetMotor	coil12,"F",holdpower

End Select

End Function



;###############################################################
;
; Step2CW
;
; Moves one step Clockwise, then reapplies hold power
;
; Uses Finite state machine to determine which combination is next
;
;	State Number	Coil 2 Direction	Coil 1 Direction
;
;		3			F			F
;		1			B			F
;		0			B			B
;		2			F			B
;
; to turn clockwise, move down the table
; to turn counterclockwise, move up the table
;
;###############################################################

Declare Function step2cw()

Select Case step2state

	Case 0
		SetMotor	coil22,"F",100
		SetMotor	coil21,"B",100
		step2state = 2
;		Wait stepdelay

	Case 1
		SetMotor	coil22,"B",100
		SetMotor	coil21,"B",100
		step2state = 0
;		Wait stepdelay

	Case 2
		SetMotor	coil22,"F",100
		SetMotor	coil21,"F",100
		step2state = 3
;		Wait stepdelay


	Case 3
		SetMotor	coil22,"B",100
		SetMotor	coil21,"F",100
		step2state = 1
;		Wait stepdelay
	
	
End Select


End Function

;###############################################################
;
; Step2CCW
;
; Moves one step CounterClockwise, then reapplies hold power
;
; Uses Finite state machine to determine which combination is next
; see StepCW for State table
;###############################################################

Declare Function step2ccw()
Select Case step2state

	Case 0
		SetMotor	coil22,"B",100
		SetMotor	coil21,"F",100
		step2state = 1
;		Wait stepdelay

	Case 1
		SetMotor	coil22,"F",100
		SetMotor	coil21,"F",100
		step2state = 3
;		Wait stepdelay

	Case 2
		SetMotor	coil21,"B",100
		SetMotor	coil22,"B",100
		step2state = 0
;		Wait stepdelay

	Case 3
		SetMotor	coil21,"B",100
		SetMotor	coil22,"F",100
		step2state = 2
;		Wait stepdelay
	
End Select


End Function

;###############################################################
;
; StepMotor2
;
; Moves a number of steps, in one direction.
; Accelerates and decelerates symetrically.
;
;###############################################################

Declare Function stepmotor2(direction As Byte, steps As Word)


decelzone = steps - decelband

step2count = 0
Do 
	If direction = "F" Then
		Call step2cw()
	Else
		Call step2ccw()
	End If

	If step2count < decelband Then
		timetowait = decelband - step2count
	Else
		
	If step2count > decelzone Then
		timetowait = step2count - decelzone
	Else
		timetowait = 0
	End If
	End If

	If timetowait <> 0 Then
		Wait timetowait
	End If

	Increment step2count 
	stepdisplay = LowByte(step2count)
	DisplayNumber stepdisplay

	If stepdisplay = 0 Then
		step2count = step2count - 256
	End If

Until step2count = steps

; now put on correct holding current
Select Case step2state

	Case 0
		SetMotor	coil22,"B",holdpower
		SetMotor	coil21,"B",holdpower

	Case 1
		SetMotor	coil22,"B",holdpower
		SetMotor	coil21,"F",holdpower


	Case 2
		SetMotor	coil22,"B",holdpower
		SetMotor	coil21,"F",holdpower

	Case 3
		SetMotor	coil21,"F",holdpower
		SetMotor	coil22,"F",holdpower

End Select

End Function


;###############################################################
;
; Bothstep
;
;###############################################################
Declare Function bothstep(dir1 As Byte,step1 As Byte, dir2 As Byte,step2 As Byte)

steploop = step1 * step2
trigger1 = step2
trigger2 = step1
decelzone1 = MakeWord(step1)
decelzone1 = decelzone1 - decelband
decelzone2 = MakeWord(step2)
decelzone2 = decelzone2 - decelband
step1count = 0
step2count = 0

If step1 = 0 Then
	step2count = MakeWord(step2)
	Call stepmotor2(dir2,step2count)
	Return
End If
If step2 = 0 Then
	step1count = MakeWord(step1)
	Call	stepmotor1(dir1,step1count)
	Return
End If

Do

	stepped = False

	Decrement trigger1
	If trigger1 = 0 Then
		trigger1 = step2	; set up for next trigger
		If dir1 = "F" Then
			Call step1cw()
		Else
			Call step1ccw()
		End If
		stepped = True
		Increment step1count
	End If

	Decrement trigger2
	If trigger2 = 0 Then
		trigger2 = step1	; set up for next trigger
		If dir2 = "F" Then
			Call step2cw()
		Else
			Call step2ccw()
		End If
		stepped = True
		Increment step2count
	End If


	If stepped = True Then
		Wait stepdelay


		If step1count < decelband Then
			timetowait1 = decelband - step1count
		Else
		If step1count > decelzone1 Then
				timetowait1 = step1count - decelzone1
			Else
				timetowait1 = 0
			End If
		End If

		If step2count < decelband Then
			timetowait2 = decelband - step2count
		Else
			If step2count > decelzone2 Then
				timetowait2 = step2count - decelzone2
			Else
				timetowait2 = 0
			End If
		End If

		If timetowait1 > timetowait2 Then
			timetowait = timetowait1
		Else
			timetowait = timetowait2
		End If

		If timetowait <> 0 Then
			Wait timetowait
		End If

	End If 	; stepped

	Decrement steploop

Until steploop = 0

; now put on correct holding current
Select Case step1state

	Case 0
		SetMotor	coil12,"B",holdpower
		SetMotor	coil11,"B",holdpower

	Case 1
		SetMotor	coil12,"B",holdpower
		SetMotor	coil11,"F",holdpower


	Case 2
		SetMotor	coil12,"B",holdpower
		SetMotor	coil11,"F",holdpower

	Case 3
		SetMotor	coil11,"F",holdpower
		SetMotor	coil12,"F",holdpower

End Select


; now put on correct holding current
Select Case step2state

	Case 0
		SetMotor	coil22,"B",holdpower
		SetMotor	coil21,"B",holdpower

	Case 1
		SetMotor	coil22,"B",holdpower
		SetMotor	coil21,"F",holdpower


	Case 2
		SetMotor	coil22,"B",holdpower
		SetMotor	coil21,"F",holdpower

	Case 3
		SetMotor	coil21,"F",holdpower
		SetMotor	coil22,"F",holdpower

End Select



End Function; bothstep


;###############################################################
;
; Main
;
;###############################################################

Declare Boolean	stepped
Declare Word	steploop
Declare Word	step1count
Declare Word	step2count
Declare Word	decelzone
Declare Word	decelzone1
Declare Word	decelzone2

Declare Byte	trigger1 
Declare Byte	trigger2 

Declare Word 	timetowait
Declare Word 	timetowait1
Declare Word 	timetowait2


Declare Byte	step1state 
Declare Byte	Step2state
Declare Byte	stepdisplay
Declare Byte	holdpower = 10	; hold stepper in position, with low power
Declare Constant	decelband = 4	; how many steps to ramp up speed and to slow down at the end
Declare Constant	stepdelay = 1	; wait to allow magnetic field to build up
Declare Constant	coil11 = 1
Declare Constant	coil12 = 2
Declare Constant	coil21 = 3
Declare Constant	coil22 = 4



Begin Program

; put stepper motor into known state
step1state = 0
SetMotor	coil11,"B",100
SetMotor	coil12,"B",100
Wait stepdelay
SetMotor	coil11,"B",holdpower
SetMotor	coil12,"B",holdpower

; put stepper motor into known state
step2state = 0
SetMotor	coil21,"B",100
SetMotor	coil22,"B",100
Wait stepdelay
SetMotor	coil21,"B",holdpower
SetMotor	coil22,"B",holdpower



Loop
Call bothstep("F",48,"F",24)
Wait 200
Call bothstep("F",24,"F",48)
Wait 200
Call bothstep("F",24,"F",24)
Wait 200
Call bothstep("B",192,"B",48)
Wait 200
Call bothstep("B",48,"F",96)
Wait 200
Call bothstep("F",48,"F",96)
Wait 200




End Loop


Call stepmotor1("F",2)
Call stepmotor2("F",2)
Call stepmotor1("F",4)
Call stepmotor2("F",4)
Call stepmotor1("F",10)
Call stepmotor2("F",10)
Call stepmotor1("F",16)
Call stepmotor2("F",16)
Call stepmotor1("F",10)
Call stepmotor2("F",10)
Call stepmotor1("F",4)
Call stepmotor2("F",4)
Call stepmotor1("F",2)
Call stepmotor2("F",2)



Call stepmotor1("F",48)
Call stepmotor2("F",48)

Call stepmotor1("B",96)
Call stepmotor2("B",96)
Call stepmotor1("F",192)
Call stepmotor2("F",192)

Call stepmotor1("B",96)
Call stepmotor2("B",96)
Call stepmotor1("F",48)
Call stepmotor2("F",48)

Wait 200


End Program