REM Armdroid Control Program REM For BBC Micro model B REM OS 0.10 REM REM (C) M.D.Bates January 1983 REM Some (hopefully) useful comments added by Neil Fazakerley, Jan 2006 MODE 7 PROC_ASSEMBLE :REM Assembles general code routines called by PROCS PROC_READ :REM Reads various DATA values into arrays PROC_ZERO :REM Zeros start values for each motor PROC_MENU :REM Prints menu options to screen MDE=1:CTRL=0:@%=6 PROC_UPDATE :REM Prints red or white in Mode 7 (‘greys out’ some menu items) REPEAT PROC_SELECT :REM Reacts to menu selections and calls relevant PROCs. Probably the program hub UNTIL FALSE END DEF PROC_ZERO REM Puts zero low/high byte in each motor’s stored start position FOR I=1 TO 6 :REM motor numbers PSNL?I=0:PSNH?I=0 NEXT:ENDPROC DEF PROC_LEARN ?LEARN=1 :REM LEARN only set to 1 for ‘learn’ mode; so = ’learn’ mode flag OA(1,1)=1:OA(4,1)=1:OA(3,1)=1 OA(6,1)=1:OA(10,1)=1 PROC_CONTROL NOS=?SC :REM NOS=number of steps; SC set to zero when ‘learn’ mode is chosen REM SC = number of current step ENDPROC DEF PROC_MANUAL ?LEARN=0 :REM LEARN set to zero when not in ‘learn’ mode PROC_CONTROL ENDPROC DEF PROC_CONTROL CTRL=1 :REM CTRL initialised to zero at program start PROC_UPDATE CALL Manual ?LEARN=0 :REM set LEARN to zero on exit as any learning has now finished ENDPROC DEF PROC_REPEAT REM repeats learned sequence indefinitely PROC_TO_START FOR J=0 TO NOS-1 ?SC=J BIG=0 ER=FALSE FOR I=1 TO 6 :REM motor numbers IF INKEY$(0)="K"THEN ER=TRUE :REM =’Stop playback’ PSNW=PSNL?I+256*PSNH?I :REM PSNW = position wide (position low+high combined) IF PSNH?I>127 THEN PSNW=PSNW-65536 DEST=?((?SC)+SQALL?I+256*SQALH?I)+256*?((?SC)+(SQAHL?I+256*SQAHH?I)) IF DEST>32767 THEN DEST=DEST-65536 :REM Max step = +/- 32767 DIF=DEST-PSNW :REM DIF = difference; DEST = destination DIRS?I=0 :REM set direction flag neutral IF DIF>0 THEN DIRS?I=1 :REM set direction flag forwards if DIF positive IF DIF<0 THEN DIRS?I=255 :REM set direction flag negative(255) if DIF negative DIF=ABS(DIF) :REM lose negative sign if there NSTEPSL?I=DIF MOD 256 :REM convert number of steps into hex value... NSTEPSH?I=(DIF DIV 256)MOD 256 IF DIF>BIG THEN BIG=DIF:?BIGL=NSTEPSL?I:?BIGH=NSTEPSH?I NEXT IF BIG=0 THEN 630 :if BIG is zero then loop back to do next step CALL Stepm :REM send step to userport IF ER THEN J=NOS :REM if error then abandon step sequence NEXT :REM line number = 630 PROC_TO_START ENDPROC DEF PROC_TO_START BIG=0 FOR I=1 TO 6 PSNW=PSNL?I+256*PSNH?I IF PSNW>32767 THEN PSNW=PSNW-65536 :REM Max step = +/- 32767 DIRS?I=-SGN(PSNW):PSNW=ABS(PSNW) :REM set direction flag; cancel sign on PSNW NSTEPSL?I=PSNW MOD 256 NSTEPSH?I=PSNW DIV 256 IF PSNW>BIG THEN BIG=PSNW:?BIGL=NSTEPSL?I:?BIGH=NSTEPSH?I NEXT IF BIG=0 THEN ENDPROC CALL Stepm :REM execute the steps ENDPROC DEF PROC_TORQUE_OFF :REM only called direct from menu MDE=3:REM mode? PROC_UPDATE :REM Updates screen graphics FOR J=1 TO 6 :motor select X%=J Y%=8 CALL Output NEXT REPEAT UNTIL INKEY(-85) :REM waits for ‘H’ keypress OA(1,1)=0:OA(4,1)=0:OA(3,1)=0 OA(6,1)=0:OA(10,1)=0 REM OA(x,x) is used as a ref for red/white menu lines in PROC_UPDATE FOR J=1 TO 6 :REM motor select X%=J:Y%=0 CALL Output NEXT:ENDPROC DEF PROC_DISPLAY REM Displays recorded steps from learning session, in six columns, one column for each motor CLS:PRINT CHR$(131);"STEP GRAB BASE SHLDR ELBOW WRIST WRIST"; PRINT CHR$(131);"_______________________________________"; PRINT TAB(6,24);CHR$(130);"Press SHIFT to continue"; VDU 28,0,22,39,3,14 FOR J=0 TO NOS-1 PRINT;" ";J; FOR I=1 TO 6 DEST=?(J+SQALL?I+256*SQALH?I)+256*?(J+(SQAHL?I+256*SQAHH?I)) IF DEST>32767 THEN DEST=DEST-65536 PRINT DEST; NEXT PRINT NEXT VDU 26,15 PRINT TAB(2,24);CHR$(133);CHR$(136);"Press SPACE to return to MENU"; A=GET PROC_MENU ENDPROC DEF PROC_SAVE CLS INPUT "What name do you want ",NAME$ X=OPENOUT NAME$ PRINT#X,NOS FOR J=0 TO NOS-1 FOR I=1 TO 6 PRINT#X,?(J+SQALL?I+256*SQALH?I),?(J+(SQAHL?I+256*SQAHH?I)) NEXT I,J CLOSE#X PROC_MENU ENDPROC DEF PROC_LOAD CLS INPUT "What name is it ",NAME$ PRINT"Press PLAY then RETURN" A=GET X=OPENUP NAME$ INPUT#X,NOS FOR J=0 TO NOS-1 FOR I=1 TO 6 INPUT#X,SL,SH :REM step low, step high ?(J+SQALL?I+256*SQALH?I)=SL ?(J+(SQAHL?I+256*SQAHH?I))=SH NEXT I,J CLOSE#X OA(1,1)=1:OA(4,1)=1:OA(3,1)=1 OA(6,1)=1:OA(10,1)=1 PRINT"Note that the ORIGIN may not be in the correct place for this sequnce. If not set manually by turning off the torque." A=INKEY(1000) PROC_MENU ENDPROC END DEF PROC_MENU CLS:PRINT CHR$(134);"ARMDROID CONTROL PROGRAM"''CHR$(134);"Menu options are shown bright"'' RESTORE 1920 FOR I=1 TO 12 READ A$ REM Reads “Playback,Set zero,Continue learning,Display program,Manual,Repeat forever,Torque off,Torque on,Load,Save,Stop playback,Learn” PRINT TAB(0,I+5);CHR$(129);CHR$(I+64);" ";A$ NEXT FOR I=1 TO 14 READ A$ REM Reads “f0 Grab,f1 Release,f2 Base left,f3 base right,f4 Lower arm down,f5 Lower arm up,f6 Upper arm down,f7 Upper arm up,f8 Remember posn.,f9 Exit control,[ Grab left,] Grab right,^ Grab up,v Grab down”. (Last four include symbols that translate as cursor key arrow graphics in Mode 7.) PRINT TAB(20,I+5);CHR$(129);A$ NEXT PRINT'CHR$(134);"Press a key to make selection" ENDPROC DEF PROC_UPDATE REM decides which lines are greyed out in menu and redraws accordingly FOR I=1 TO 12 :REM 12 lines of menu IF OA(I,MDE)=1 THEN PRINT TAB(0,I+5);CHR$(135) ELSE PRINTTAB(0,I+5);CHR$(129):REM 135=white, 129=red NEXT FOR I=1 TO 14 PRINT TAB(20,I+5); IF CTRL=1 THEN PRINT CHR$(135) ELSE PRINT CHR$(129): REM 135=white, 129=red NEXT PRINT' ENDPROC DEF PROC_SELECT *FX 15,1 :REM flush input buffer REPEAT REPEAT SEL=GET-64 :REM makes key A=1... key L=12 UNTIL SEL>0 AND SEL <13 :REM until key A-L pressed VLID=FALSE IF OA(SEL,MDE)=1 THEN VLID=TRUE :REM OA() is 12x4 array of valid-option flags UNTIL VLID :REM until a non-greyed-out option is chosen MDE=0:PROC_UPDATE IF SEL=1 THEN MDE=2:PROC_UPDATE:PROC_REPEAT :REM A IF SEL=2 THEN PROC_ZERO :REM B IF SEL=3 THEN ?SC=NOS:PROC_LEARN :REM C IF SEL=4 THEN PROC_DISPLAY :REM D IF SEL=5 THEN PROC_MANUAL :REM E IF SEL=6 THEN MDE=2:PROC_UPDATE:REPEAT PROC_REPEAT:UNTIL ER=TRUE :REM F IF SEL=7 THEN PROC_TORQUE_OFF :REM G (H=Torque on) IF SEL=9 THEN PROC_LOAD :REM I IF SEL=10 THEN PROC_SAVE :REM J (K=Stop playback) IF SEL=12 THEN ?SC=0:PROC_LEARN :REM L CTRL=0 MDE=1 PROC_UPDATE ENDPROC DATA Playback,Set zero,Continue learning,Display program,Manual,Repeat forever,Torque off,Torque on,Load,Save,Stop playback,Learn DATA f0 Grab,f1 Release,f2 Base left,f3 base right,f4 Lower arm down,f5 Lower arm up,f6 Upper arm down,f7 Upper arm up,f8 Remember posn.,f9 Exit control,[ Grab left,] Grab right,^ Grab up,v Grab down DATA 0,0,0,0,0,0,0,0,0,0,0,0 DATA 0,1,0,0,1,0,1,0,1,0,0,1 DATA 0,0,0,0,0,0,0,0,0,0,1,0 DATA 0,0,0,0,0,0,0,1,0,0,0,0 DEF PROC_ASSEMBLE DIM WAVE 8,STATUS 6,TEMP 1,DIR 1,SPACE% 1000,PSNL 6,PSNH 6,KEYT 15,MOTORT 15,DIRT 15,KEYC 1,SEQL 1792,SEQH 1792,SC 1,SQALL 6,SQALH 6,SQAHL 6,SQAHH 6,LEARN 1 DIM DIRS 6,BIGL 6,BIGH 6,NSTEPSL 6,NSTEPSH 6 PORT=&FE60:OSBYTE=&FFF4 REM User port addresses SEQAL=&70:SEQAH=&72 FOR PASS=0 TO 1 P%=SPACE% :REM 1000 byte variable space where code will be stored [ OPT 0 .Set LDA STATUS,X:TAY .Output REM ‘Output’ is called twice by PROC_Torque_Off. Six times with X=1-6 and Y=8. Then waits for H key press (INKEY(-85)) (=Torque on). Then six times more with X=1-6 and Y=0. LDA WAVE,Y ;Load A with WAVE(Y) (=240 (torque off) or 192 (torque on)) STA TEMP ;Store 240 or 192 in TEMP TXA ;Load A with value in X (=1-6) (motor number) ASL A ;Multiply A by 2 (now =2-12) CLC ;Clear carry flag ready for ADD operation (housekeeping) INC TEMP ;Add 1 to TEMP (now =241 or 193) ADC TEMP ;Add TEMP to A (now = coil data(4 bits) + motor select(3 bits) + Strobe) STA PORT ;Store A in PORT (PORT=&FE60, = Userport). Sets Strobe line AND #&FE ;AND A with &FE (&FE=11111110), so clears the lowest bit STA PORT ;Store A in PORT, now with Strobe line clear RTS ;Return to calling routine .Stepmotor LDA STATUS,X \ STATUS is empty (0) when first called; X contains motor number CLC \ housekeeping ADC DIR \ Add 1 or -1 to A, based on current value from DIRT AND #7 \ 7 = first three bits (lines)? STA STATUS,X \ Save updated STATUS value LDA DIR \ To check whether step direction should be positive or negative BMI Backwards \ If DIR = -1 jump to Backwards to ‘reverse’ motor step LDA PSNL,X \ Otherwise, get current PSNL value (PSNL = position low byte?) CLC \ Clear carry ready for ADD op ADC #1 \ Increment PSNL value STA PSNL,X LDA PSNH,X \ PSNH = position high byte? ADC #0 STA PSNH,X JMP Send .Backwards LDA PSNL,X:SEC:SBC #1:STA PSNL,X LDA PSNH,X:SBC #0:STA PSNH,X .Send JSR Set RTS .Manual LDX#0 \ Initialise with zero for Keyscan loop counter .Keyscan TXA \ Load A with loop count from X PHA \ Push (save) current loop count onto stack LDA KEYT,X \ KEYT (=key table) holds 16 -INKEY numbers for F0-F7+cursor keys TAY:TAX \ Copy current –INKEY number into X and Y LDA #129:JSR OSBYTE \ Test for keypress against –INKEY value from key table TXA:BPL Nokey \ Branch to Nokey if key not pressed (loops again with next no.) PLA:PHA:TAX \ Copy loop count into A and then X LDA DIRT,X \ DIRT (=direction table?) holds either 1 or -1 in its 16 locations STA DIR \ Copy DIRT value into DIR (temporary scatchpad) LDA MOTORT,X \ MOTORT (=motor table) holds motor numbers (1-6) in 16 locations TAX:JSR Stepmotor \ Put loop count back in X and branch to Stepmotor CPX #2:BNE Nokey \ check if base motor; if not, then jump over delay JSR Delay:JSR Delay:JSR Delay \ remove this delay to speed up base motor .Nokey PLA:TAX:INX \ Get loop count off stack and increment it CPX #16:BNE Keyscan \ If <16, do Keyscan loop again with new key-table value LDA LEARN:BEQ Nostore \ If LEARN=zero then not ‘learn’ mode, so skip Storeseq LDX #137:LDY #137:LDA#129:JSR OSBYTE :REM test for F8 (‘Remember posn.’) TXA:BPL Nostore \ if F8 not pressed then check for F9 and repeat manual loop LDA #7:JSR&FFEE \ F8 pressed so call VDU7 (beep) to indicate storing op LDX #1 \ sets counter to 1 for step storage sequence (1-6) .Storeseq \ this is the sequence that stores steps during the learning process LDA SQALL,X:STA SEQAL:LDA SQALH,X:STA SEQAL+1 \ stores low/high address of 256 byte SEQL slots 1-6 in SEQAL and SEQAL+1 LDA SQAHL,X:STA SEQAH:LDA SQAHH,X:STA SEQAH+1 \ stores low/high address of 256 byte SEQH slots 1-6 in SEQAH and SEQAH+1 LDY SC \ put current step count in Y LDA PSNL,X:STA(SEQAL),Y \ puts each motor’s position-low byte into slot address held in SEQAL+SC LDA PSNH,X:STA(SEQAH),Y \ puts each motor’s position-high byte into slot address held in SEQAH+SC INX \ increment X to next motor number CPX#7:BNE Storeseq \ continue round loop until 6 motor positions done INC SC \ increment step count ready for next loop .wait \ checks for a key press and loops if no key LDX#137:LDY#137:LDA#129:JSR OSBYTE:TXA:BMI wait .Nostore JSR Delay LDX #136:LDY #136:LDA #129:JSR OSBYTE :REM check if F9 pressed? TXA:BMI Exit:JMP Manual \ Exit if F9, else repeat manual loop .Exit RTS \ return to Basic PROC .Delay LDX #3 .Loop1 LDY #255 .Loop2 DEY:BNE Loop2:DEX:BNE Loop1 RTS .Stepm JSR Delay:JSR Delay LDX #1 .loop3 LDA NSTEPSL,X:BNE Yesmove LDA NSTEPSH,X:BNE Yesmove JMP Nomove .Yesmove TXA:PHA:CPX #2:BNE Notbase \ if base motor (2) then insert extra step delays JSR Delay:JSR Delay:JSR Delay .Notbase \ base motor (2) stepped slowly, hence different motor noise PLA:TAX:LDA DIRS,X:STA DIR:BEQ Nomove:JSR Stepmotor LDA NSTEPSL,X:SEC:SBC #1 STA NSTEPSL,X:LDA NSTEPSH,X:SBC #0:STA NSTEPSH,X .Nomove INX:CPX#7:BNE loop3:LDA BIGL SEC:SBC#1:STA BIGL:LDA BIGH SBC#0:STA BIGH:LDA BIGL BNE Stepm:LDA BIGH:BNE Stepm RTS ] NEXT ENDPROC DEF PROC_READ DIM OA(16,4) RESTORE 3090 FOR I=0 TO 8 READ WAVE?I :REM Reads “192,128,144,16,48,32,96,64,240” NEXT ?&FE62=255 :REM defines all user port lines as outputs (0= all inputs) FOR I=0 TO 15 READ KEYT?I,MOTORT?I,DIRT?I REM Reads “-33,-114,-115,-116,-21,-117,-118,-23,-122,-26,-58,-42,-122,-26,-58,-42 into KEYT(I). REM Reads “1,1,2,2,3,3,4,4,5,5,5,5,6,6,6,6” into MOTORT(I) REM Reads “1,-1,1,-1,-1,1,1,-1,1,-1,-1,1,1,-1,1,-1” into DIRT(I) NEXT FOR I=1 TO 6 REM the following extracts the low/high bytes of the 6 slots in the two 1792-byte position stores (SEQL and SEQH) and ensures each slot is page aligned SQALL?I=(SEQL+256*I)MOD 256 :REM SQALL holds low byte of 6 SEQL slot address SQALH?I=(SEQL+256*I)DIV 256 :REM SQALL holds high byte of 6 SEQL slot address SQAHL?I=(SEQH+256*I)MOD 256 :REM SQAHL holds low byte of 6 SEQH slot address SQAHH?I=(SEQH+256*I)DIV 256 :REM SQAHL holds high byte of 6 SEQH slot address NEXT RESTORE 1940 FOR J=0 TO 3 FOR I=1 TO 12 READ OA(I,J) :REM Reads 48 zeros and ones into 12x4 array (OA = options array?) NEXT:NEXT ENDPROC DATA 192,128,144,16,48,32,96,64,240 DATA -33,1,1,-114,1,-1 DATA -115,2,1,-116,2,-1 DATA -21,3,-1,-117,3,1 DATA -118,4,1,-23,4,-1 DATA -122,5,1,-26,5,-1 DATA -58,5,-1,-42,5,1 DATA -122,6,1,-26,6,-1 DATA -58,6,1,-42,6,-1 Memory spaces (bytes) PSNL(6) – position low; motor stored start position, low byte PSNH(6) – position high; motor stored start position, high byte SEQL(1792) – sequence low; array to hold learned positions, low byte SEQH(1792) – sequence high; array to hold learned positions, high byte SQALL(6) – holds address low byte; all 6 locations initialised to &70 SQALH(6) – holds address high byte; locations initialised to 1,2,3,4,5,6 SQAHL(6) – holds address low byte; all 6 locations initialised to &72 SQAHH(6) – holds address high byte; locations initialised to 1,2,3,4,5,6 BIGL(6) – big low byte BIGH(6) – big high byte SC(1) – step count; incremented each time a learned step is stored WAVE(8) - STATUS(6) TEMP(1) 6502 code scratchpad DIR(1) – direction KEYT(15) – key table; –INKEY values for F0 to F7 plus the 4 cursor keys MOTORT(15) – motor number table DIRT(15) – direction flag table (1 or -1) KEYC(1) LEARN(1) – learn mode; 1=learn mode, 0=not learn mode DIRS(6) NSTEPSL(6) – number of steps, low NSTEPSH(6) – number of steps, high Global variables: SEQAL – initialised to &70 (112) SEQAH – initialised to &72 (114) CTRL – used to set menu lines to print red or white (CTRL=0 or CTRL=1) BIG OA(12,4) – options allowed for each menu line MDE – mode; indicates which option column should be used in OA(x,MDE) ER - error DIRT – direction table DEST - destination DIF – difference NOS – number of steps; used to count steps in learning mode and to replay them PSNW – position-wide; combines PSNL and PSNH PORT – address of userport output register (&FE60) OSBYTE – address of OS routine (&FFF4)