'>CyclerBody2 '{$Stamp BS2} ' 'todo '---- 'Bugs '---- '----------------------------------------------------------- 'Notes '----- 'in WMove - #### comment to set servo pot 'if servo CP bad then will never get beyond Gosub Servoff in Init as no timeouts '03Sep07 tried Shelley with keeping the 24v on as long as one wheel was going forward ' exciting but not safe. '----------------------------------------------------------- 'History '------- '05Sep07 uploaded to Byron '02Sep07 uploaded to Shelley, Shakespeare '02Sep07 WMOVE rewritten ' WstateNIB was just Lgo, Rgo; now includes all four relays ' f_Wmoving always ended up =1, WMOVE rewritten with f_WmovBk '>CyclerBody2 '----------------------------------------------------------- 'BS2 pins Leds VAR OUTA 'outL.lownib WheelsLed CON 0 ArmsLed CON 1 HeadLed CON 2 MouthLed CON 3 Aenable CON 4 'enable L6203 drivers for arms armSafety CON 5 WLBkpin CON 6 'Left Wheel Back WRBkpin CON 7 'Right Wheel Back listenpin CON 15 'listen_to_me from master controller listen_i VAR IN15 'listen_to_me from master controller cmndRx CON 14 'cmnd from Brain cmndTx CON 13 'ack to Brain enable24v CON 12 'enable 24v armsflow CON 11 armsdata CON 10 WRgopin CON 9 WLgopin CON 8 i96n CON 16468 '9600 BS2 i24n CON 16780 '2400 baud, BS2 i3n CON 19697 '300 baud, BS2 tout CON 100 'timeout cmnd in cmnd VAR Byte 'serin cmnd from Brain keys VAR cmnd.LOWNIB 'cmnd from BS2_Brain keyA VAR cmnd.BIT0 keyB VAR cmnd.BIT1 keyC VAR cmnd.BIT2 keyD VAR cmnd.BIT3 chan VAR cmnd.HIGHNIB 'base=0, arms=1, head=2, MP3=3, (Eyes=4) 'actual transmitted channel is chan +1 be_still CON 128 'cmnd from BS2_Brain oldchan VAR Nib it VAR Byte 'idletime itkeymove CON 50 'idletime after a key move ptcount VAR Byte '@nib 'loop counter - _pause pt VAR Byte 'frame delay pause time, no point in less ptA CON 20 ptWrev CON 200 delay24 CON 500 '500=~360deg,6ft 'delay24 CON 400 '400=~ 'delay24 CON 300 '300=~190deg,3ft 'delay24 CON 250 '250=~170deg,2ft 'delay24 CON 200 '200=~140deg,1ft6 count24 VAR Word movedflags VAR Nib 'moved flags for servos movedAflags VAR Nib 'moved flags for arm servos, D1,D0 only f_be_still VAR Bit i VAR ptcount '@counter randval VAR Word Wstate VAR Nib WRgo VAR Wstate.BIT0 WRBk VAR Wstate.BIT1 WLgo VAR Wstate.BIT2 WLBk VAR Wstate.BIT3 servset CON %00000000 'command to set servo position servread CON %00000100 'command to read servo position Qstatus CON %00001000 'command to get status of all servos srelax CON %00001100 'command to turn off a servo servo VAR Nib '0 - 3 'left arm=3, right arm=2, left wheel =1, right wheel =0 'arms coproc 1, wheels coproc 0 'arms down=0 up=255 smax CON 255 smin CON 0 tox VAR Byte 'new servo position ALspeed VAR Nib ARspeed VAR Nib AKspeedup CON 3 AKspeeddn CON 2 AKendspeed CON 3 ARndspeed CON 0 AR CON 2 ARto VAR Byte ARat VAR Byte ARmin CON 0 ARmax CON 255 AL CON 3 ALto VAR Byte ALat VAR Byte ALmin CON 0 ALmax CON 255 flags VAR Nib f_freemove VAR flags.BIT0 f_WmovBk VAR flags.BIT1 'at least one wheel is moving back or was f_ARup VAR flags.BIT2 f_ALup VAR flags.BIT3 basepulse CON 250 '1000ms in 4us units pscale CON 2 'BS2 4us to 2us Sspeed VAR Byte 'Servo speed cpSaddress VAR Nib 'Servo address for co-processor Sstatus VAR Byte 'servo status byte from co-processor flowx VAR Nib 'cp flow control line txx VAR Nib 'cp data line rxx VAR txx 'cp data line Wtxx CON 10 'see servmap Wflowx CON 11 'see servmap Atxx CON 10 'see servmap Arxx CON Atxx Aflowx CON 11 'see servmap command VAR Byte 'command to serial co-processor action VAR Byte 'desired action '------------------------------------------- 'Servo co-processor info: '9600 baud, 8 bits, no parity, inverted 'Send two bytes, 'Command byte format: XXXXYYZZ 'ZZ is servo number - 0,1,2 'XXXX is speed 0 to 15 - extra increments per frame (0=slow,15=fast) 'YY is command : 00 - set servo end position ' 01 - report servo position ' XXXX ignored, ZZ =servo number ' response =0 to 255 =current servo position. ' 10 - report all servos ' XXXX ignored, ZZ must be %00 ' response = %STUVWXYZ ' where S,T,U,V,W,X,Y and Z may be 0 or 1 ' STUV ' S - (servo 4) always 1 on 3 servo chip ' T - servo 2 ' U - servo 1 ' V - servo 0 ' 0 => relaxed ' 1 => being pulsed ' WXYZ ' W - (servo 4) always 1 on 3 servo chip ' X - servo 2 ' Y - servo 1 ' Z - servo 0 ' 0 => not reached end point ' 1 => has reached end point ' (ie a quick way to read all servos) ' 11 - relax servo, ie stop pulsing it until 00 sent 'Position byte is from 0 to 255 '-------------------------------------------------------------------- Init: DEBUG"I" DIRL =%11111111 'all pins output DIRH =%00001000 'all pins input except armsflow LOW Aenable 'p4 'disable L6203 arm drivers servo =2 'right arm GOSUB servoff servo =3 'left arm GOSUB servoff tox =0 Sspeed =3 servo =2 GOSUB seroutval servo =3 GOSUB seroutval HIGH armSafety 'switch on 24v motor supply PAUSE 100 'supply latches 'brain pauses for 500 so we are running first LOW armSafety 'p5 LOW enable24v 'p12 '12v to motors LOW WRgopin LOW WLgopin LOW WRBkpin LOW WLBkpin Wstate =0 chan =0 oldchan =255 'illegal value forces LEDs update pt =255 :GOSUB Lpause 'stop arms thrashing at power on 'stop 'goto Move '----------------------------- AMOVES: DEBUG "M" 'armSafety & Aenable here for after wake up, ' idle time is delay LOW enable24v count24 =0 LOW armSafety 'p5 'if it's low it won't matter RANDOM randval 'new arm position 'scrunch them down into bottom quarter randval.HIGHBYTE =randval.HIGHBYTE *randval.HIGHBYTE /256 randval.LOWBYTE =randval.LOWBYTE *randval.LOWBYTE /256 ARspeed =ARndspeed ALspeed =ARndspeed IF f_freemove>0 THEN incfree ARspeed =AKendspeed ALspeed =AKendspeed incfree: f_freemove =f_freemove |1 ALto =randval.HIGHBYTE /12 ARto =randval.LOWBYTE /12 RANDOM randval 'new arm position it =randval.LOWBYTE /2 MIN 1 MOVE: IF chan=0 THEN WMOVE 'goto Tmodule '@ AMOVE: 'debug"A" IF f_be_still=1 THEN Apt HIGH Aenable 'p4 'enable L6203 arm drivers Apt: pt =ptA Ato: tox =ARto servo =AR Sspeed =ARspeed GOSUB seroutval tox =ALto servo =AL Sspeed =ALspeed GOSUB seroutval Ppause: 'Pulse pause APaskR: f_ARup =0 servo =AR GOSUB getspos 'get atx into tox IF tox<120 THEN APaskL f_ARup =1 APaskL: f_ALup =0 servo =AL GOSUB getspos 'get atx into tox IF tox<120 THEN A24 f_ALup =1 A24: IF (f_ARup | f_ALup)=1 THEN A24on LOW enable24v GOTO ALP A24on: HIGH enable24v ALP: GOSUB Lpause LOW armSafety 'p5 'if it's low it won't matter 'here for after wakeup, needs to be after a pause 'to give relay time GOSUB checkAdone 'check if Arms done IF movedAflags & %11 <>%11 THEN Ppause ALat =ALto ARat =ARto IF f_be_still=0 THEN Idle 'debug " stillP",dec f_be_still LOW Aenable 'p4 'disable L6203 arm drivers if be_still Idle: 'debug "i" GOSUB Lpause it =it -1 IF it<>0 THEN Idle IF f_be_still=1 THEN Idle GOTO AMOVES WMOVE: pt =4 'just for arm safety and to loop round Lpause GOSUB Lpause LOW armSafety 'p5 'if it's low it won't matter 'here for after wakeup, needs to be after a pause 'to give relay time count24 =0 LOW enable24v IF Wstate=0 THEN WMOVE 'not moving so just loop IF WLBk=1 OR WRBk=1 OR f_WmovBk=1 THEN WLpStop _WM: count24 =count24 +1 MAX delay24 'debug dec count24 OUTPUT 6 'WLBkpin CON 6 OUTPUT 7 'WRBkpin CON 7 OUTPUT 8 'WLgopin CON 8 OUTPUT 9 'WRgopin CON 9 OUT9 =Wstate.BIT0 'WRgopin CON 9 OUT7 =Wstate.BIT1 'WRBkpin CON 7 OUT8 =Wstate.BIT2 'WLgopin CON 8 OUT6 =Wstate.BIT3 'WLBkpin CON 6 IF count24"] SERIN cmndRx,i96n,tout,Move,[cmnd] 'debug bin4 keys," " ' Debug "@",dec chan,"#",bin4 keys,cr 'Debug cr,bin4 keys 'Note - cmnd=0=wakeup or =128=be_still or chan=1,2,3,4 IF cmnd=be_still THEN _be_still '(be_still=127) IF cmnd =0 THEN _wakeup chan =chan -1 'convert to absolute format IF chan=oldchan THEN DoKeys oldchan =chan 'debug ?chan Leds =DCD chan 'update body Leds DoKeys: IF keys =0 THEN EndKeyMove BRANCH chan,[DoWheels,DoArms] GOTO Move DoWheels: 'keys CA LF RF ' E DB LB RB 'if A&B then A LOW WLgopin 'Wheels stop LOW WRgopin Wstate =0 W_kA: IF keyA=0 THEN W_kB WRgo =1 'Wstate.BIT0 WRBk =0 'Wstate.BIT1 GOTO W_kC W_kB: IF keyB=0 THEN W_kC WRgo =1 'Wstate.BIT0 WRBk =1 'Wstate.BIT1 'if C&D then C W_kC: IF keyC=0 THEN W_kD WLgo =1 'Wstate.BIT2 WLBk =0 'Wstate.BIT3 GOTO W_endk W_kD: IF keyD=0 THEN W_endk WLgo =1 'Wstate.BIT2 WLBk =1 'Wstate.BIT3 W_endk: 'debug dec WLto,",",dec WRto," " GOTO dokeymove DoArms: 'keys CA LU RU ' DB LD RD 'if A&B then B A_kB: IF keyB=0 THEN A_kA ARto =ARmin ARspeed =AKspeeddn GOTO A_kD A_kA: IF keyA=0 THEN A_kD ARto =ARmax ARspeed =AKspeedup 'if C&D then D A_kD: IF keyD=0 THEN A_kC ALto =ALmin ALspeed =AKspeeddn GOTO A_endk A_kC: IF keyC=0 THEN A_endk ALto =ALmax ALspeed =AKspeedup A_endk: 'debug ?ARspeed,?ALspeed GOTO dokeymove _wakeup: DEBUG " wake ",DEC keys chan =0 oldchan =chan 'initialise led vars after wakeup Leds =DCD chan 'turn on body Leds f_be_still =0 'incase Tx was turned off to rearmSafety LOW enable24v 'make sure only 12v to motors LOW WLgopin 'Wheels stop LOW WRgopin LOW WRBkpin 'turn off relays to save current LOW WLBkpin Wstate =0 HIGH armSafety 'p5 'switch on 24v motor supply 'low in Moves after idletime itkeymove HIGH Aenable 'enable L6202 arm drivers GOTO DoKeyMove _be_still: IF f_be_still=1 THEN DoKeyMove f_be_still =1 'debug " still ",dec f_be_still OUTL.LOWNIB =0 'turn off all body Leds LOW WLgopin 'Wheels stop LOW WRgopin LOW WRBkpin 'turn off relays to save current LOW WLBkpin Wstate =0 LOW WRBkpin LOW WLBkpin ALto =ALmin 'Arms down ARto =ARmin LOW enable24v 'p12 'only 12v to motors LOW armSafety 'p5 'in case it's high GOTO DoKeyMove EndKeyMove: 'debug " EndKey " 'Wheels stop LOW WLgopin 'Wheels stop LOW WRgopin LOW WRBkpin 'turn off relays to save current LOW WLBkpin Wstate =0 ' low enable24v 'only 12v to motors '(low enable24v not used - switches 24v off) '(at the end of a key cmnd, so if arms up and on, 24v relay) '(clicks off until switched back on in A24.) 'Arms stay where they are 'got to do it here otherwise ' they will have moved on and jerk back servo =AR GOSUB getspos 'get atx into tox ARat =tox ARto =tox GOSUB seroutval 'update tox servo =AL GOSUB getspos 'get atx into tox ALat =tox ALto =tox GOSUB seroutval 'update tox f_freemove =0 DoKeyMove: it =itkeymove GOTO Move '---------------------- sub routines ------------------------- 'Lpause: see before LISTEN: '--------------------------------- seroutval:tox =(tox MIN smin) MAX smax 'don't overtravel servos movedAflags.LOWBIT(servo) =0 GOSUB servmap 'debug ?Sspeed command =Sspeed<<4 +servset +cpSaddress SEROUT txx\flowx,i96n,[command,tox] RETURN '--------------------------------- checkdone: 'not used FOR servo =0 TO 3 STEP 2 GOSUB servmap askdone: 'DEBUG" checking",#servo SEROUT txx\flowx,i96n,[Qstatus,0] '?status SERIN rxx,i96n,50,askdone,[Sstatus] FOR cpSaddress =0 TO 1 movedflags.LOWBIT(servo+cpSaddress) =Sstatus.LOWBIT(cpSaddress) NEXT NEXT RETURN '--------------------------------- checkAdone: SEROUT Atxx\Aflowx,i96n,[Qstatus,0] '?status SERIN Arxx,i96n,50,checkAdone,[movedAflags] RETURN '--------------------------------- 'checkSdone: 'subroutine not used GOSUB servmap askSdone: 'DEBUG" checking",#servo SEROUT txx\flowx,i96n,[Qstatus,0] '?status SERIN rxx,i96n,50,askSdone,[Sstatus] movedAflags.LOWBIT(servo) =Sstatus.LOWBIT(cpSaddress) RETURN '--------------------------------- servmap: LOOKUP servo,[0,1,0,1],cpSaddress LOOKUP servo,[8,8,10,10],txx LOOKUP servo,[9,9,11,11],flowx RETURN '-------------------------- getspos: GOSUB servmap command =servread +cpSaddress ask1: 'DEBUG"willask ",#servo," " SEROUT txx\flowx,i96n,[command,tox] SERIN rxx,i96n,50,ask1,[tox] 'DEBUG "getspos=",#tox,cr RETURN '-------------------------- servoff: 'debug"so" GOSUB servmap command =srelax +cpSaddress SEROUT txx\flowx,i96n,[command,0] RETURN '--------------------------------------------------------------