REM DICS - TITS REM DAzLE Instrument Control System - Taranis Interface & Tracking Software REM REM Anthony Horton REM REM 20040814 - Version 0.1 REM Initialisation sequence GOSUB Initialise REM Main Loop REPEAT REM Signal that system is ready for next command PRINT "R" cmd = GET IF cmd = ASC("T") THEN REM Begin new tracking sequence GOSUB Tracking ELSE IF cmd = ASC("H") THEN REM Find the home position and zero the encoder GOSUB Home ELSE IF cmd = ASC("O") THEN REM Energise the motor ENERGISE = ON ELSE IF cmd = ASC("X") THEN REM De-energise the motor ENRGISE = OFF ENDIF ENDIF ENDIF ENDIF UNTIL cmd = ASC("S") REM Shutdown sequence END DEFSUB Initialise REM Initialisation sequence REM Setup some constants dt = 200 REM Tracking time interval, in milliseconds tolerance = 150 REM Tolerance for hitting the correct start position for tracking. About 0.1 degree. limit_lower = 14674 limit_upper = 150367 REM Initialise the limit switch warning lights WHENRATE = 40 LED(1) = OFF LED(2) = OFF REM Set up units so motor units sync with encoder units UNITS = 58.15100821 ENCUNITS = 1 REM One step of both motor and encoder now correspond to 6.814571275E-4 degree of bearing REM 1.467443745E3 steps per degree REM 1.189367058E-5 radians per step. REM Set up motor motion parameters, in new units slew_speed = 4400 SPEEDMAX = slew_speed REM Slewing speed of about 3 degrees per second ACCEL = 4000 REM 10x sufficent for tracking acceleration. DECEL = 4000 REM 10x sufficient for tracking acceleration DECELRAPID = 40000 REM Emergency stop. Note that this is ~400 times default value. REM Set up the motor currents CURRENTIDLE = 1000 CURRENTRUN = 2000 CURRENTBOOST = 2500 BOOST = AUTO REM Limit switch setup INTMODE(1) = FALL INTMODE(2) = FALL INTEN(1) = ON INTEN(2) = ON REM TODO: Restoration and saving of encoder position to NVARS. REM TODO: Option to execute homing routine. REM Ask whether to find the home position or restore saved encoder position WHILE TRUE PRINT "H?" cmd = GET IF cmd = ASC("H") THEN GOSUB Home EXIT ELSE IF cmd = ASC("L") THEN REM Load saved encoder position NVRLOAD ENCPOS = NVAR(1) PRINT ENCPOS EXIT ENDIF ENDIF WEND ENDSUB DEFSUB Home REM Go home.. REM Check the limit switches IF IN(1) = OFF THEN PRINT "E0001" RETURN ENDIF IF IN(2) = OFF THEN PRINT "E0002" RETURN ENDIF REM Make sure the motor is energised ENERGISE = ON REM Move into lower limit switch, slowly, stopping if we've gone 120 degrees without hitting it. SYNCSTOP = 3 SPEEDMAX = 500 MOVEABS = ENCPOS - 170000 PAUSE IDLE SPEEDMAX = slew_speed 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) PRINT INTRCVD(1) PRINT INTRCVD(1) REM Check that move was terminated by the limit switch we expected it to be. IF int_trip1 AND NOT int_trip2 THEN REM Set the position at which limit switch tripped to be zero point ENCPOS = ENCPOS - INTENC(1) REM Now back off so that the limit switch is no longer being tripped. counter = 0 REPEAT counter = counter + 1 IF counter > 10 THEN PRINT "E0007" RETURN ENDIF MOVE = 1500 PAUSE IDLE UNTIL IN(1) = ON ELSE IF NOT int_trip1 THEN PRINT "E0006" IF int_trip2 THEN PRINT "E0002" PRINT "E0000" RETURN ENDIF ENDSUB DEFSUB Tracking REM Get the starting position for the new track INPUT ; start_pos REM Check the limit switches IF IN(1) = OFF THEN PRINT "E0001" RETURN ENDIF IF IN(2) = OFF THEN PRINT "E0002" RETURN ENDIF REM Check requested position is within allowed range IF start_pos < limit_lower THEN PRINT "E0003" RETURN ENDIF IF start_pos > limit_upper THEN PRINT "E0004" RETURN ENDIF REM Make sure motor is energised ENERGISE = ON REM Seems OK, so make the move, subject to limit switches, REM while sending updates on position. SYNCSTOP = 3 MOVEABS = start_pos - ENCPOS REPEAT PRINT ENCPOS WAIT 100 UNTIL IDLE REM Check that move wasn't terminated by hitting limit switches IF INTRCVD(1) THEN PRINT "E0001" RETURN ENDIF IF INTRCVD(2) THEN PRINT "E0002" RETURN ENDIF REM Check that we're now roughly where we wanted to be IF ABS(start_pos - ENCPOS) > tolerance THEN PRINT "E0005" RETURN ENDIF REM If all is fine then send ready signal and enter tracking loop. PRINT "R" REM Tracking loop, repeated until halt ("H") received, or REM an error occurs (limts reached, etc.). REPEAT cmd = GET IF cmd = ASC("N") THEN REM Command is for next position, first grab current position. current_pos = ENCPOS REM Now get next desired position. INPUT ; next_pos REM Check requested position is within allowed range IF next_pos < limit_lower THEN PRINT "E0003" RETURN ENDIF IF next_pos > limit_upper THEN PRINT "E0004" RETURN ENDIF REM Check that the tracking hasn't been terminated by a limit switch IF INTRCVD(1) THEN PRINT "E0001" RETURN ENDIF IF INTRCVD(2) THEN PRINT "E0002" RETURN ENDIF REM Now adjust motor speed appropriately SYNCSTOP = 3 JOG = (next_pos - current_pos) / dt REM Report current position back PRINT current_pos REM If all is OK return to start of loop and await next command ENDIF REM TODO: Add unwind command. UNTIL cmd = ASC("H") REM Received command to halt, first check to see if limits were hit IF INTRCVD(1) THEN PRINT "E0001" RETURN ENDIF IF INTRCVD(2) THEN PRINT "E0002" RETURN ENDIF REM If the motor is moving then stop it STOP PAUSE IDLE REM Report final position PRINT ENCPOS REM Finished tracking now, return to main loop for next command. ENDSUB DEFHALT REM Shutdown sequence STOP ENERGISE = OFF NVAR(1) = ENCPOS NVRSAVE PRINT ENCPOS PRINT "S" ENDHALT 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