//  Program for Azimuth axis

//	User variable 0   = position setpoint in 0,01 degree
//	User variable 1   = actual position in 0,01 degree
//	User variable 2   = position setpoint Change of state register in 0,01 degree
//	User variable 3   = position setpoint in µsteps
//	User variable 5   = operation mode select 0 1 2 3 4 5
//	User variable 6   = buffered mode select Change of state register 0 1 2 3 4 5

//	User variable 11  = Reference position Known flag
//	User variable 12  = homing sequence register

SIO 0, 0, 0    			// disable pull-up on all inputs
SIO 255, 2, 0    		// Set all outputs to ZERO

SAP 140, 0, 5     		// set Microstep Resolution to 32 µsteps
SAP 210, 0, 12801  		// set Encoder prescaler to match 
					// Encoder direction reversed because motorwiring has been reversed

SAP 12, 0, 1    		// set Right limit switch disable to 1 ( disable )
SAP 13, 0, 1           	// set Left limit switch disable to 1 ( disable )
SAP 149, 0, 1    		// set Soft stop flag to 1 ( no hard stop )

VECT 27, Capture		// Define Capture position routine

EI 255				// Enable global interrupts
SGP 5, 2, 0    			// set User variable 5  Start up in mode 0
SGP 11, 2, 0    		// set User variable 11 to Zero ( Ref. Known flag )
SGP 12, 2, 0    		// set User variable 12 to Zero ( homing sequence )

CSUB Convert_uSteps_to_Degree
AGP 0, 2    			// accu to User variable 0 position setpoint in 0,01 degree
					// make sure position setpoint is equal to actual position

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

MainLoop:

	CSUB Convert_uSteps_to_Degree

	GGP 5, 2    			// get User variable 5 ( Mode selection )
	CALCX LOAD				// Store user variable 5 in register X
	GGP 6, 2    			// get User variable 6 ( Mode select change of state )
	CALCX SUB				// Subtract variable 5 from variable 6
	COMP 0				// Check if result = zero
	JC ZE , No_Change		// when zero end routine
	MST 0					// Stop motor when mode selection changes
No_Change:
	CALCX SWAP				// Copy X register into Accu ( user variable 5 )
	AGP 6, 2    			// Copy User Var 5 to User variable 6
	COMP 2				// Check if we are busy finding the reference position
	JC EQ, CONTINUE
	SGP 12, 2, 0    		// set User variable 12 to Zero homing sequence
CONTINUE:
	GGP 6, 2    			// get User variable 6 ( buffered Mode selection )
	COMP 0
	JC NE, Check_1
	CSUB MODE_0			// Stop motor and set the brake

Check_1:
	GGP 6, 2    			// get User variable 6 ( buffered Mode selection )
	COMP 1
	JC NE, Check_2
	CSUB MODE_1			// Release the brake

	GIO 1, 2    			// READ Status Output 1 (Brake )
	COMP 1
	JC NE , Complete	    	// If brake not free then preform no further action

//  Modes 2 , 3 and 4 are not allowed when the brake is not released

Check_2:
	GGP 6, 2    			// get User variable 6 ( buffered Mode selection )
	COMP 2
	JC NE, Check_3
	SAP 154, 0, 2     		// set Pulse divisor to 2
	SAP 4, 0, 1573     		// set Maximum speed [int] to 900 RPM = 10 degree/sec
	SAP 153, 0, 11   		// set Ramp divisor to 11
	SAP 5, 0, 1648 			// set Maximum acceleration [int] to 1.0 sec 
	CSUB MODE_2			// Find reference position

Check_3:
	GGP 6, 2    			// get User variable 6 ( buffered Mode selection )
	COMP 3
	JC NE, Check_4
	SAP 154, 0, 3     		// set Pulse divisor to 3
	SAP 4, 0, 1573 			// set Maximum speed [int] to 450 RPM = 5 degree/sec
	SAP 153, 0, 12    		// set Ramp divisor to 12
	SAP 5, 0, 1648			// set Maximum acceleration [int] to 2.0 sec 
	CSUB MODE_3			// Manual control ( Jog forward and Jog reverse )

Check_4:
	GGP 11, 2 				// get User variable 11 Reference position Known flag
	COMP 1
JC NE , Complete			// mode 4 operation is not allowed when reference is unknown

	GGP 6, 2    			// get User variable 6 ( buffered Mode selection )
	COMP 4
	JC NE, NEQ_4
	SAP 154, 0, 2     		// set Pulse divisor to 2
	SAP 4, 0, 1573      		// set Maximum speed [int] to 900 RPM = 10 degree/sec
	SAP 153, 0, 11   		// set Ramp divisor to 11
	SAP 5, 0, 1648 			// set Maximum acceleration [int] to 1.0 sec 
	CSUB MODE_4			// Move to position as specified in 0,01 degres in user variable 0
JA Check_5
NEQ_4:
	GGP 0, 2    			// get User variable 0 ( Setpoint in 0,01 degree )
	AGP 2, 2				// Setpoint change of state = Setpoint

Check_5:
	GGP 6, 2    			// get User variable 6 ( buffered Mode selection )
	COMP 5
	JC NE, Complete
	
	CSUB MODE_5			// ?????????????????????????

Complete:
JA MainLoop

STOP
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

//  00000000000000000000000000000000000000000000000000000000000000000000
MODE_0:					// Stops Motor and sets the brake
	GIO 1, 2    			// READ Status output 1 (Brake )
	COMP 1
	JC NE , End_0
	MST 0
	GAP 3, 0    			// get Actual speed [int]
	COMP 0
	JC NE , END_0
	SIO 1, 2, 0    			// change Output 1 to zero ( Brake )
END_0:
RSUB

//  111111111111111111111111111111111111111111111111111111111111111111111
MODE_1:					// Releases the brake
	GIO 1, 2    			// READ Status output 1 (Brake )
	COMP 1
	JC EQ , Brake_Free
	SIO 1, 2, 1    			// Set Output 1 to 1 ( Release Brake )
	WAIT TICKS, 0, 30		// wait 300 msec
Brake_Free:
RSUB

// 2222222222222222222222222222222222222222222222222222222222222222222222
MODE_2:					// Find the reference position
	GGP 12, 2    			// get User variable 12 homing sequence
	COMP 20
	JC GE ,  No_Action_2
	COMP 15
	JC EQ , Step_15
	COMP 10
	JC EQ , Step_10
	COMP 9
	JC EQ , Step_9
	COMP 8
	JC EQ , Step_8
	COMP 7
	JC EQ , Step_7
	COMP 6
	JC EQ , Step_6
	COMP 5
	JC EQ , Step_5
	COMP 4
	JC EQ , Step_4
	COMP 3
	JC EQ , Step_3
	COMP 2
	JC EQ , Step_2
	COMP 1
	JC Eq , Step_1
 
		//   Initialising reference search parameters
	SCO 0, 0, 0            	// set Coordinate 0 to 0    stores high-low transition
	SCO 1, 0, 0            	// set Coordinate 1 to 0    stores low-high transition
	SGP 11, 2, 0    		// set User variable 11 to Zero ( Ref. Known flag )
	SGP 12, 2, 1    		// set User variable 12 homing sequence to 1

	CSUB Add_Switch_Status	// Result in variable 12 will be 1 or 2
RSUB
Step_1:	// Limit switch is Off. So find it within 360 degrees
	MVP REL, 0, -3456000	// incremental move - 360 degrees
	SGP 12, 2, 3    		// set User variable 12 homing sequence to 3 
RSUB
Step_2:	// Limit switch is On. Move away 15 degrees
	MVP REL, 0, 144000		// incremental move 15 degrees
	SGP 12, 2, 15    		// set User variable 12 homing sequence to 15 
RSUB
Step_3:	// Check if we are at speed ( Limit switch must always be activated at same speed )
	GAP 4, 0    			// get Maximum speed [int]
	CALCX LOAD				// Store Maximum speed in X register
	GAP 3, 0    			// get Actual speed [int]
	COMP 0
	JC GE , Positive_Speed
	CALC MUL , -1			// Reverse sign of actual speed
Positive_Speed:
	CALCX SUB				// Subtract maximum speed from actual speed
	COMP 0				// zero difference means motor is at requested speed
	JC NE, No_Action_2		// when not at speed continue checking
	SGP 12, 2, 4    		// set User variable 12 homing sequence to 4
	CSUB Add_Switch_Status 	// Result in variable 12 will be 4 or 5
RSUB
Step_4:	// Enable the switch interrupt
	SGP 27, 3, 3    		// set Left limitswitch trigger transition to low-high high-low
	EI 27					// Capture the transitions of the Left limit switch
	SGP 12, 2, 6    		// set User variable 12 homing sequence to 6
RSUB
Step_5:	// While at speed the Limit switch is On. So stop and continue in step 2
	MST 0
	GAP 3, 0    			// get Actual speed [int]
	COMP 0				// motor stopped
	JC NE, No_Action_2
	SGP 12, 2, 1    		// set User variable 12 homing sequence to 1
    CSUB Add_Switch_Status 	// Result in variable 12 will be 1 or 2
RSUB
		// The capture interupt routine will increment the homing sequence register
Step_6:					// Wait for transition Low to High
Step_7:					// Wait for transition High to Low
	GAP 8, 0    			// get Position reached flag
	COMP 1
	JC EQ , Error			// when at end of move goto error
RSUB
Step_8:	// Calculate midpoint between transitions and move to that position
	DI 27					// Disable interupt 27 for left limit switch
	GCO 0, 0				// Get coordinate 0 High - Low transition\
	CALCX SWAP				// Store High - Low transition in X register
	GCO 1, 0				// Get coordinate 1 Low - High transition
	CALCX ADD				// Calculate sum of transitions
	CALC DIV , 2			// Calculate MIDpoint between transitions

	Calc ADD , 12345		// Hardcoded offset to referenceposition

	ACO 2, 0				// Store result in Coordinate 2
	MVP COORD, 0, 2			// Move to Coordinate 2
	SGP 12, 2, 9    		// set User variable 12 homing sequence to 9
RSUB
Step_9:	// wait for the position reached flag
	GAP 8, 0    			// get Position reached flag
	COMP 1
	JC NE , No_Action_2		// wait for end of motion
	SGP 12, 2, 10   		// set User variable 12 homing sequence to 10
RSUB
Step_10:	//	Update position registers with the reference position
	SAP 138, 0, 2   		// set Ramp mode to 2 ( velocity mode ) to prevent any
						// motion occurring when modifying the position registers
	SAP 1, 0, 0            	// set Actual position [µsteps]
	SAP 0, 0, 0            	// set Target position [µsteps]
	SAP 209, 0, 0          	// set Encoder position [µsteps]
	SGP 11, 2, 1    		// set User variable 11 to 1 ( Ref. Known flag )
	SGP 12, 2, 20    		// set User variable 12 to 20 homing sequence
RSUB
Step_15:	// While moving off the limitswitch wait for position reached flag
	GAP 8, 0    			// get Position reached flag
	COMP 1
	JC NE , No_Action_2
	GAP 11, 0    			// get Left limit switch status
	COMP 1
	JC EQ , Error
	SGP 12, 2, 1    		// set User variable 12 homing sequence to 1
RSUB
ERROR:
	DI 27					// Disable interupt 27 for left limit switch
	SGP 12, 2, 50    		// set User variable 12 to 50 ERROR homing sequence
	CSUB Add_Switch_Status
			// 50 = Left switch always off   51 = Left switch always on
RSUB
No_Action_2:
RSUB

// 3333333333333333333333333333333333333333333333333333333333333333333333
MODE_3:					// Manual operation
	// Input 0 and Input 1 are both two digital aswell as two analog inputs
	// they are interconnected on the board.
	// When using 24 V digital signals the analog values will simply saturate at 1023

	GIO 0, 1    			// read Analog Input 0
	AGP 0, 2    			// accu to User variable 0
	COMP 20				// No input active ( use minimum level of 20 to filter out noise )
	JC GT, Jog_Reverse
	GIO 1, 1    			// read Analog Input 1
	AGP 0, 2    			// accu to User variable 0
	COMP 20				// No input active ( use minimum level of 20 to filter out noise )
	JC GT, Jog_Forward
	GAP 3, 0    			// get Actual speed [int]
	COMP 0
	JC EQ , No_Action_3
	MST 0					// Stop the motor
JA No_Action_3
Jog_Forward:
	CSUB Calc_Speed
	MVP ABS, 0, 2592000		// move to max. position of 270,00 degree
JA No_Action_3
Jog_Reverse:
	CSUB Calc_Speed
	MVP ABS, 0, -2592000		// move to min. position of -270,00 degree
No_Action_3:
RSUB

//  4444444444444444444444444444444444444444444444444444444444444444444444
MODE_4:					// Move to Position as specified in user variable 0
	GGP 0, 2    			// get User variable 0 ( Setpoint in 0,01 degree )
	CALCX LOAD				// Store user variable 0 in register X
	GGP 2, 2    			// get User variable 2 ( Setpoint change of state )
	CALCX SUB				// Subtract variable 0 from variable 2
	COMP 0				// Check if result = zero
	JC ZE , Not_Changed		// when zero end routine
	CSUB Convert_Degree_to_uSteps
Not_Changed:
RSUB

//  55555555555555555555555555555555555555555555555555555555555555555555555
MODE_5:	//				??????????????????????????????????

RSUB
//  =========================================================================

Convert_uSteps_to_Degree:
	GAP 209, 0    			// get Encoder position [µsteps]
	CALC DIV, 96			// 9600 µsteps per degree
	AGP 1, 2    			// accu to User variable 1 position in 0,01 degree
RSUB

Convert_Degree_to_uSteps:
	GGP 0, 2    			// get User variable 0 ( Setpoint in 0,01 degree )
	CALC MOD , 36000		// Limit the input to + - 360 degrees
	COMP 27000				// upper limit of 270,00 degree
	JC LE, Check_Low_Limit
	CALC SUB, 36000			// subtract 360,00 degree
	AGP 0, 2			    	// set User variable 0 to desired setpoint - 360,00 degree
JA CONVERT
Check_Low_Limit:
	COMP -27000			// lower limit of -270,00 degree
	JC GE, CONVERT			// If value >=  
	CALC ADD, 36000			// add 360,00 degree
	AGP 0, 2    			// set User variable 0 to desired setpoint + 360,00 degree
CONVERT:
	GGP 0, 2    			// get User variable 0 ( Setpoint in 0,01 degree )
	AGP 2, 2				// Setpoint change of state = Setpoint
	CALC MUL, 96			// 9600 µsteps per degree
	ACO 10, 0   			// accu to User Coordinate 10 position in µsteps
	AGP 3, 2    			// accu to User variable 3 position in µsteps
	MVP COORD, 0, 10		// move to position in coordinate 10
RSUB

Add_Switch_Status:
	GGP 12,2				// Load user variable 12 into accumulator
	CALCX LOAD				// Load user variable 12 into X register
	GAP 11, 0    			// get Left limit switch status
	CALCX ADD				// Add status from left limitswitch
	AGP 12, 2    			// accu to User variable 12 
RSUB

Calc_Speed:
	GAP 4, 0    			// get Maximum speed [int]
	CALCX LOAD				// Load maximum speed into X register
	GGP 0, 2    			// get User variable 0 ( stored analog value of input 0 or 1 )
	CALCX MUL				// Multiply max speed with stored analog value
	CALC div , 1023			// Analog value ranges from 0 to 1023 ( 0 to 10.7 Volt )
	AAP 4, 0    			// accu to Maximum speed [int]
RSUB

Calc_Power_2:
	SGP 132, 2, 0    		// set User variable 132 to 0 Ticktimer
	SGP 20, 2, 1    		// set User variable 20 to 1 ( 2 to power of 0 )
Loop:
	GGP 21, 2    			// get User variable 21 ( power )
	COMP 0
	JC EQ , Ready
	CALC SUB , 1
	GGP 20, 2    			// get User variable 20
	CALC MUL , 2
JA Loop
Ready:
	GGP 132, 2    			// get User variable 132 to 0 Ticktimer
	AGP 22,2				//
RSUB

Capture:
	GGP 12, 2    			// get User variable 12 homing sequence
	CALC ADD ,1			// Add 1 to User variable 12
	AGP 12, 2              	// accu to User variable 12
	GAP 11, 0    			// get Left limit switch status
	COMP 0
	JC NE , High
	CCO 0, 0				// Store position from high to low transition in Coordinate 0
RETI
High:
	CCO 1, 0				// Store position from low to high transition in Coordinate 1
RETI