REM test_prog.bas
REM 
REM Simple test program for the DAZLE camera rotator.  Moves over the full range of travel, in 
REM both directions, and spits out pairs of raw motor/encoder position measurements on the serial port.
REM
REM WARNING! DO NOT RUN THIS PROGRAM UNLESS THE LIMIT SWITCHES ARE WORKING!
REM This program relies on them to prevent it from breaking things at the end stops.  Use limittest.bas
REM to verify that the limit switches are working correctly.
REM
REM Anthony Horton
REM 20041123

WHILE TRUE
	PAUSE KEY

	REM 1 motor unit is now one motor rev
	UNITS = 51200
 
	REM Data sampling rate, readings per second.
	sampling = 5.0
	interval = 1000.0 / sampling

	REM Set up the motor currents
	CURRENTIDLE = 1000
	CURRENTRUN = 2000
	CURRENTBOOST = 2500
	BOOST = AUTO 

	REM Set up motion parameters
	SPEEDMAX = 0.8333333
	REM 0.5 degree per second at bearing, I think
	ACCEL = 500
	DECEL = 500
	DECELRAPID = 1000

	REM Initialise the limit switch warning lights
	WHENRATE = 40
	LED(1) = OFF
	LED(2) = OFF

	REM Limit switch setup
	INTMODE(1) = FALL
	INTMODE(2) = FALL
	INTEN(1) = ON
	INTEN(2) = ON

	REM Check the limit switches
	IF IN(1) = OFF THEN
		END
	ENDIF
	IF IN(2) = OFF THEN
		END
	ENDIF

	ENERGISE = ON

	REM The move should be stopped by either limit switch.
	SYNCSTOP = 3
	REM This is a move of 120 degrees, should be sufficent to hit limit switch	
	MOVE = 200
	PAUSE IDLE

	REM Store limit switch trip status in order to check all went as expected
	int_trip1 = FALSE
	int_trip2 = FALSE
	int_trip1 = INTRCVD(1)
	int_trip2 = INTRCVD(2)
	
	REM Check that move was terminated by the limit switch we expected it to be.
	IF int_trip2 AND NOT int_trip1 THEN
		REM Set the position at which limit switch tripped to be zero point
		ENCPOS = ENCPOS - INTENC(2)
		POSITION = POSITION - INTPOS(2)
		REM Now back off so that the limit switch is no longer being tripped.
		counter = 0 
		REPEAT
			counter = counter + 1
			IF counter > 10 THEN
				END
			ENDIF
			SYNCSTOP = 0
			MOVE = -0.83333333
			PAUSE IDLE
		UNTIL IN(2) = ON
	ELSE
		END
	ENDIF

	REM Check the limit switches
	IF IN(1) = OFF THEN
		END
	ENDIF
	IF IN(2) = OFF THEN
		END
	ENDIF

	REM Now ready to do a data taking run to one limit.
	PRINT POSITION, ENCPOS
	SYNCSTOP = 3
	MOVE = -200
	REPEAT
		PRINT POSITION, ENCPOS
		WAIT interval
	UNTIL IDLE

	REM Now go back again.

	REM Store limit switch trip status in order to check all went as expected
	int_trip1 = FALSE
	int_trip2 = FALSE
	int_trip1 = INTRCVD(1)
	int_trip2 = INTRCVD(2)

	REM Check that move was terminated by the limit switch we expected it to be.
	IF int_trip1 AND NOT int_trip2 THEN
		REM Now back off so that the limit switch is no longer being tripped.
		counter = 0 
		REPEAT
			counter = counter + 1
			IF counter > 10 THEN
				END
			ENDIF
			SYNCSTOP = 0
			MOVE = 0.83333333
			PAUSE IDLE
		UNTIL IN(1) = ON
	ELSE
		END
	ENDIF

	REM Check the limit switches
	IF IN(1) = OFF THEN
		END
	ENDIF
	IF IN(2) = OFF THEN
		END
	ENDIF

	REM Now do a data taking run to the other limit.
	PRINT POSITION, ENCPOS
	SYNCSTOP = 3
	MOVE = 200
	REPEAT
		PRINT POSITION, ENCPOS
		WAIT interval
	UNTIL IDLE

	STOP
	ENERGISE = OFF

WEND

REM Shutdown
DEFHALT
	STOP
	ENERGISE = OFF
ENDHALT

REM When commands to control the limit switch lights
WHEN IN(1) = ON
	LED(1) = OFF
ENDWHEN

WHEN IN(1) = OFF
	LED(1) = ON
ENDWHEN

WHEN IN(2) = ON
	LED(2) = OFF
ENDWHEN

WHEN IN(2) = OFF
	LED(2) = ON
ENDWHEN