;**************************************************************** ;* ;* PDI/2 -- Peanut Defence Initiative, the sequel. ;* ;* Peanut Throwing Program. ;* ;* Motor/Drive: Max RPM: 2500 ;* Pulse transducer: 1250 pulses/turn * 4 ;* ==> 5000 pulses/turn. ;* ;**************************************************************** .define status = r20 ; Ok for PC to restart prog if status=0 .define command = r21 ; Set by PC to execute a command. .define peanuts = r22 ; Set by PC to indicate # of peanuts in revolver. .define stsidle = 0 ; Program not running, PC must send ^C RUN .define okstart = 1 ; Ok for PC to send command. .define running = 2 ; Command in progress. .org 1 ; Start of program. restart: serial mode 16 ; Enable XON/XOFF. let status, running Vector 10,ctrlc ; Control-C trap.... Gosub initpdi ; Initialize. let status, okstart goto wcmd ; Then ready for command. .org 25 ; .linestep 2 .linestep 1 ; ; This is the main lupe. -- Top level -- Waiting for command. ; wcmd: let status, okstart gosub rblink ; Twinkle twinkle little led's. gosub swdec ; Decode switches. if 0 = command, wcmd ; Continue waiting if no command. gosub cdec ; go and decode command. goto wcmd ;**************************************************************** ;* ;* Decode input switches. ;* ;**************************************************************** swdec: if lo in and 1, wave ; wave spoon. if lo in and 2, throw1 ; swing, mode 1. if lo in and 4, throw2 ; swing, mode 2. if lo in and 8, throw3 if lo in and 16, throw4 return ;**************************************************************** ;* ;* Routines to take care of input switches. ;* ;**************************************************************** wave: goto wavef throw1: gosub gohome ; Start at home pos. throw1a: gosub swing1 if lo in and 2, throw1a ; Do again if button pressed. return throw2: gosub swing2 return throw3: gosub swing3 return throw4: gosub swing4 return ;**************************************************************** ;* ;* Decode input command. ;* ;**************************************************************** cdec: sub command, 1 if 0 = command, cmd1 ; load point to high sub command, 1 if 0 = command, cmd2 ; load point to low sub command, 1 if 0 = command, cmd3 ; right but to slow. sub command, 1 if 0 = command, cmd4 sub command, 1 if 0 = command, cmd5 sub command, 1 if 0 = command, cmd6 sub command, 1 if 0 = command, cmd7 sub command, 1 if 0 = command, cmd8 sub command, 1 if 0 = command, cmd9 sub command, 1 if 0 = command, cmd10 sub command, 1 if 0 = command, cmd11 sub command, 1 if 0 = command, cmd12 sub command, 1 if 0 = command, cmd13 ; This is the destructor command. return ; Noo good, wait for new command. .org 100 ;**************************************************************** ;* ;* Routines to take care of the individual commands. ;* ;**************************************************************** cmd1: let r2,150 ; offset position -- load to hi. goto faicmd cmd2: let r2,-200 ; load to low. goto faicmd cmd3: let r2,0 ; load ok. goto faicmd faicmd: acc 50 ret 50 pos speed 30 000 pos abs r2 wait rdy gosub loadnut clr tmr wait tmr > 1000 gosub xthrow return ; ; CATCH. ; cmd4: gosub gohome ; Goto home position. gosub loadnut ; Load a nut. gosub jiggle ; Jiggle it in position. pos speed 40 000 acc 50 ret 50 pos rel -145 ; Backup to launch postion. wait rdy acc 200 ret 300 pos rel 500 ; Throw nut into the air. wait rdy pos speed 60 000 acc 600 ret 400 pos abs -5000 ; Back spoon one turn when nut wait rdy ; is in the air. clr tmr wait tmr > 1500 ; Wait for nut to land on spoon gosub xthrow ; then lazy throw away. return ; ; Load And Drop. ; cmd5: gosub gohome ; Load nut and then drop it. gosub loadnut acc 1 ; slow backwards until drop. ret 1 pos rel -600 wait rdy return cmd6: goto wave cmd7: goto swing1 cmd8: goto swing2 cmd9: goto swing3 cmd10: goto swing4 cmd11: gosub gohome cmd11n: if 0 = r23, cmd11e if 0 > r23, cmd11e gosub swing1 ; Eject it... sub r23,1 goto cmd11N cmd11e: return ; ; CMD12 -- Second hand of watch, eject peanut every minute, ; until out of peanuts or other command. ; cmd12: gosub gohome clr tmr cmd12l: let r0,20/2 ; One minute.... in 30 seconds. cmd12m: pos rel - 83*2 wait tmr > 1000 pos rel -83*2 wait tmr > 2000 pos rel -84*2 wait tmr > 3000 dec tmr 3000 if 0 < command, cmd12e ; Exit if new command. loop r0, cmd12m gosub swing1 ; Throw a nut. if 0 = command, cmd12 ; Start all over again if no new cmd12e: return ; command. ; ; CMD13 - Automateld med krossfunktion. ; cmd13: if 0 = r23, cmd13e if 0 > r23, cmd13e gosub swing3 ; Eject it... sub r23,1 goto cmd13 cmd13e: return ;**************************************************************** ; ; Perform complete swing, mode 1. ; swing1: let status, running gosub loadnut gosub fthrow return ; ; Perform complete swing, mode 2. ; swing2: let status, running ; Indicate status is running. Gosub gohome ; Goto home position. Gosub loadnut ; Load peanut. Gosub jiggle ; Jiggle peanut in position. Gosub ffire ; Fire! return ; Wait for next command. ; ; Perform complete swing, mode 3. ; swing3: let status, running gosub gohome gosub loadnut gosub jiggle gosub bfire ; Backwards fire. return ; ; Perform complete swing, mode 4 -- This empties revolver. ; swing4: let status, running acc 50 speed 50 000 wait spd = 50 000 swi4l: if 0 = peanuts, swi4e ; Exit when no more peanuts. if 0 > peanuts, swi4e gosub loadnut goto swi4l swi4e: speed 0 pos speed 50 000 pos abs 0 wait rdy return ;**************************************************************** ;* ;* Wave the spoon and look silly... ;* ;**************************************************************** wavef: Acc 50 ; Just 80 turns forward. Ret 10 Pos Speed 50000 ; This is 600 RPM. POS ABS 400000 Wait RDY clr tmr wait tmr > 1500 ; Pause... Acc 100 ; Then 80 turns back again. Ret 500 POS ABS 0 Wait RDY CLR TMR Wait TMR > 1000 ; Pause again. Acc 1500 ; 50 fast movements to impress audience. Ret 1500 POS ABS 6250 ; To start postion... Wait RDY CLR TMR Wait TMR > 1000 ; 1 sec wait. LET R1, 350 ; Inital time between movements. LET R0, 50 ; Number of movements. wavef1: POS REL 2500 ; 1/2 turn forward. CLR TMR wait rdy ; (to make sure it is not too fast) Wait TMR > R1 ; Wait SUB R1, 5 ; Decrement wait time. Loop R0,wavef1 ; Do it again.... 50 times..... wait tmr > 1000 ; Puh!!! Acc 50 ; New parameters, for silly wave.... Ret 50 wavef2: Push POS ; Get current position. Pop R0 DIV2 R0 ; Div by 8. DIV2 R0 DIV2 R0 CLR TMR ; 100 ms wait... Wait TMR > 100 ABS R0,-1 ; Make r0 negative. POS REL R0 ; Go 1/8 of distance from current pos ; towards 0. Wait RDY If 0 > R0,wavef2 ; If more to go.. POS ABS 0 ; Then the last movement.... wait rdy return ;**************************************************************** ;* ;* Forward FIRE with SERVE. ;* ;**************************************************************** ffire: acc 20 ; Backup to serve position. ret 20 pos rel -130 wait rdy Acc 200 ; Setup... Ret 250 Pos Speed 30000 CLR TMR ; Reference time. POS REL 500 ; Throw peanut into air. Wait RDY Acc 1000 ; Setup for SMASH! Ret 1500 Pos Speed 35000 ; This is the soft smash. POS ABS -1000 ; Backup.... Wait RDY Wait TMR > 380 ; Wait until time to SMASH! POS ABS 4000 ; SMASH!!!!! Wait RDY Return ; This is it! ;**************************************************************** ;* ;* Backward FIRE with ZAPPING SMASH! ;* ;**************************************************************** bfire: acc 20 ; Goto serve position. ret 20 pos rel 75 wait rdy acc 200 ; Parameters for "throw into the air" ret 250 pos speed 30 000 clr tmr ; Ref-Time pos rel 500 ; Serve wait rdy acc 1000 ; Parameters for SUPER SMASH! ret 1500 pos speed 100 000 ; This is 1200 RPM. wait tmr > 350 ; Ready... Steady... GO!!!! pos abs -12000 ; SMAAAASHHHHH! wait rdy return ; So, the world has one broken peanut more. ;**************************************************************** ;* ;* Lazy throw away.... (forward) ;* ;**************************************************************** fthrow: Acc 500 ; Good parameter for simple throw away. Ret 250 Pos Speed 30000 POS REL 5000 ; Go exactly one turn. Wait RDY Return ; Then ready. ;**************************************************************** ;* ;* eXtra Lazy throw away.... (forward) ;* ;**************************************************************** xthrow: Acc 50 ; Good parameter for simple throw away. Ret 50 Pos Speed 30000 POS REL 2500 ; Go exactly 1/2 turn Wait RDY pos rel -2500 ; and back again. wait rdy Return ; Then ready. ;**************************************************************** ;* ;* Load peanut. ;* ;**************************************************************** loadnut: ; ; First we must check if there is any penuts in the revolver. ; if 0 = peanuts, nonuts ; Error if nope. if 0 > peanuts, nonuts ; Just to make sure.... sub peanuts, 1 ; Take a nut. ; SET Out 64 ; Start peanut revolvermotor. ldn1: If lo IN and 64,ldn1 ; Wait while sensor active ldn2: If hi IN and 64,ldn2 ; Wait while sensor inactive. CLR Out 64 ; Got active, stop motor. CLR TMR ; Timer = 0 Wait TMR > 150 ; Wait 150 ms for peanut to begin to fall. SET Out 64 ; Start motor again Wait TMR > 350 ; Wait 200 ms more. CLR Out 64 ; Stop motor. Wait TMR > 500 ; Wait 150 ms more for peanut to stabilize. Return ; ; No Nuts, blink with the lamps.... ; nonuts: let r0,63 nonu1: clr out 63 set out r0 clr tmr wait tmr > 15 loop r0, nonu1 clr out 63 goto wcmd ; Wait for new command. ;**************************************************************** ;* ;* INIT PDI -- Find home position. ;* ;**************************************************************** initpdi: Ref POS 0 ; Setup system parameters etc... Scale 145 Zero 192 Pole 0 P Gain 13 vector 8, fposerr ; Setup vector for position error. pos err 1250 ; 1/4 turn is fatal..... Enable REF ; Want to look for ref-pulse. Acc 50 Ret 50 Speed 1000 ; Slow forward. initp1: If lo IN and 128,initp1 ; Wait util i find it. Ref POS 700 ; Yep, this is pos 700 rel to launch pos. Pos Speed 10000 ; Goto launch psition. POS ABS 0 Wait RDY Return ;**************************************************************** ;* ;* GOHOME -- Goto launch position and setup standard ACC & RET. ;* ;**************************************************************** gohome: Acc 100 Ret 100 Pos Speed 10000 POS ABS 0 ; go away... Wait RDY Return ;**************************************************************** ;* ;* JIGGLE -- Jiggle peanut in position. ;* ;**************************************************************** jiggle: LET R0, 40 ; Number of times to jiggle. LET R1, 15000 ; Jiggle force. LET R2,-15000 jloop: Profile ACC 440 ; Jiggle forwards Wait RDY Profile ACC -440 ; and then back again. Wait RDY CLR TMR SUB R1, 375 ; Decremnt jiggle force. ADD R2, 375 Loop R0,jloop ; Then ready for next jiggle. clr tmr ; Wait 0.5 seconds more for wait tmr > 500 ; panut to stablize. Return 440 P Data R1, 5 ; Data for jiggle. 441 P Data R2, 10 442 P Data R1, 5 443 P Data 0, 0 ;**************************************************************** ;* ;* FPOSERR -- Fatal positioning error, close down shop. ;* ;**************************************************************** fposerr: scale 0 ; Emergeny stop! pos abort ; Stop internal ramp gen etc... acc 1000 speed 0 fposer1: let status, stsidle ; Flag that we are offline. clr out 127 clr tmr wait tmr > 200 set out 63 wait tmr > 400 goto fposer1 ; Loop here until user aborts... ;**************************************************************** ;* ;* Control-C trap, go here when user types Control-C. ;* ;**************************************************************** ctrlc: vector 10,ctrlc ; Setup Control-C trap again. speed 0 acc 500 pos abort ; This stops main motor gracefully Out 0 ; This makes sure revolver motor is off. let status, stsidle STOP ; Then we can stop program. Goto restart ;**************************************************************** ;* ;* RANDOM BLINK ROUTINE. ;* ;**************************************************************** rblink: push tmr pop r0 sub r0,200 if 0 > r0, rblret add r10, 13611 ; This is a 16 bit random generator. mul r10, 271 and r10, 65535 ; Do only keep the 16 lsb's let r11, r10 div2 r11 div2 r11 div2 r11 div2 r11 and r11, 63 ; Want to random blink with 6 LED's clr out 63 set out r11 let r12,r10 and r12,127 add r12,50 dec tmr r12 ; Decrment the timer. rblret: return ; No change, return. ; ; SLUT!!!! ;