HXBOS42.pbp.txt '>HxtrBOS42 D Buckley 9-2-01 'Oh what a tangled web we weave when we set out to achieve ' - (with appologies) BOSver CON 42 'Compiled with Micro Engineering Labs - PIC Basic Pro (PBP) 'out of memory with 2.42 - use 2.20 Include "bs2defs.bas" ' Set DEBUG pin port DEFINE DEBUG_REG PORTC ' Set DEBUG pin bit DEFINE DEBUG_BIT 7 ' Set DEBUG baud rate DEFINE DEBUG_BAUD 9600 ' Set DEBUG mode: 0 = true, 1 = inverted DEFINE DEBUG_MODE 1 ' Set DEBUG pacing ms DEFINE DEBUG_PACING 10 '{$STAMP BS2} 'BS2sx and servo cps = 60ma ' ditto + servos (not pulsed) = 75ma '------------------------------------- 'Kludges @K '------- '------------------------------------- 'zChanges '-------- 'baud =i96n or i24n in ser2 for commands 'i96n used in ser2 for servos 'Sdelta var lupx now var byte '@ qflags () put in test '@init baudsel=1 was low on BS2 board ADCON1 =6 'set portA to digital io ' trisA.1 =0 'output, servo power control ' trisA.2 =0 'output, busy 'branch replaced by branchL 'BS2 pin numbering retained for servmap, ' 0-7 map to portB, 8-15 map to portC ' can't think of how to do it otherwise '------------------------------------- 'To do '----- '------------------------------------- 'Notes '----- 'Halt doesn't stop gripper movements ' halt is not freeze so it shouldn't, Halt does Stand 'movedflags=$FFFF in init ' checkdone sets bit15 for Wait ' nothing else alters bit15 ' askallpulsing doesn't set bit15 '------------------------------------- 'Bugs '---- '------------------------------------- 'Queries '------- 'when Relax cmnd is sent is 2nd byte (ie tox) ignored? ' must be as tox=0 is ok '------------------------------------- 'History '-------- '30Mar09 added note to Servo co-processor section, ' - pulse parameter to us conversion values. '26Feb08 42 ' in dosequence relay often glitches if off and not new charged battery ' added 100mS delay; ' changed sign-on msg to use BOSver. '21Nov06 4.1 ' BOSver CON added, and ?v for BOSver. ' CR added after DECx values returned for ? to delimit them. ' power was high and glitched servos while power on message was sent ' now set low before set to output. ' cmndbusy set high=busy before set to output. '12-11-01 HxtrBOS-4 ' qdonsequ: if P_ptr<>0 then dosequence, P_ptr never=0 here!! ' so test was always true. Now removed and loops back to dosequence. ' gotos now back to dosequence instead of qdonesequ '31-10-01 HxtrBOS-3 ' bug fix, new neutral values weren't written to eeprom, ' ? written and "gsrlmn" made non interactive, ' couldn't get n to work from BS2, hopefully all still work the same! '22-7-01 for production boards - voicepin portB.3, was portB.2 'beeper '17-5-01 serout echo added, renamed from zHxtrBOS.bse '16-5-01 tidied, debugs changed to upper case, time and frequ ' changed round in voice, renamed from zW_BOS.bse '15-5-01 Wavegait coded, finished ' cmnds now single byte, jump to CONTROL now on <6, no "-" prompt '4-4-01 Major changes to comms structure, eliminated "G" for gripper etc ' donsequ changed to qdonsequ ' qhalt now put before qdone ready for Quick gait. ' changed Sdelta to var lupx, was var action. ' combined - midswingx var lupx. '31-3-01 deleted var cp - not used, var leg - not used. ' added midstep to FD,BK after turn. ' added mask of flags top bit, lost 5 bytes. ' sayflags added, saved 6 bytes. '22-3-01 in direct control cmndget servo max increased to 14 from 13. '22-3-01 History section added. '------------------------------------- 'Overview:- 'On reset the pgm prints a msg:- ' "Bug Commander BC x.x - Milford Instruments",cr ' and waits for a serial ascii command, ready prompt is '>'. 'Control of the Behavioural Operating System BOS is by serial ' commands from another processor or a terminal. 'A command may be halted by:- ' raising the 'cmndhalt' pin until the 'busypin' goes low or ' by sending "H" continuously until ">" is received, ' It is not possible to interupt "h","V","v". 'dec arguments must be ended by CR or other non-numeric code ' 'Main Commands:- ' Can be used manually from a terminal. ' Commands ignored if cmndhalt pin is high or if no prompt. ' Cmndbusy goes high as soon as the first byte of a command ' is received and should be ignored for further bytes of ' multibyte commands. 'single character:- ' F - Forward one step ' B - Backward one step ' R - Rightturn one step ' L - Leftturn one step ' H - Halt ' S - Sit ' U - Up ' M - step so feet are in Midposition ' N - goto Neutral position, current values held in EEPROM ' K - Kill servo power, ' jumper selectable to exclude gripper, ' power turned on by any movement command ' or by jumping to direct Control. ' Z - Zzzz all servos to sleep ' Q - Quick, legs move in Trapezoid path, ' smooth but can stub toes ' T - Tall, legs move in Rectangular path, ' leg up/down is vertical ' p - pickup Gripper by 1/16 range ' P - Pickup Gripper by 1/4 range ' d - down Gripper by 1/16 range ' D - Down Gripper by 1/4 range ' o - open Gripper by 1/16 range ' O - Open Gripper by 1/4 range ' c - close Gripper by 1/16 range ' C - Close Gripper by 1/4 range ' W - Wait until all servos finish moving, ' only useful after gripper commands ' and direct control ' V - Voice, beep 1KHz for 0.1sec 'one argument:- ' s - speed, update variable Sspeed ' r - rideheight update variable legdown ' m - move update variable swing ' l - lift update variable lift ' h - halt for time, 1=0.1sec ' BOS deaf to HALT during this time, ' useful for direct control and voice etc ' w - wait until servo is finished moving ' only useful after gripper commands ' and direct control ' individual control of Legs or Body, used for Wave-Gait:- ' <0,1,2,3,4,5,6>, ' <0=body, 1-6=legs>, ' 1f => Leg 1 steps forwards ' 1m => Leg 1 steps to mid position ' 1b => Leg 1 steps backwards ' similarly for Legs 2,3,4,5,6 ' 0f => Body moves forwards, ie all Legs move backwards without steping ' 0m => Body moves to mid position, ie all Legs move to mid position without steping ' 0b => Body moves backwards, ie all Legs move forwards without steping 'two argument:- ' n - new neutral position ' servos 0 - 11, new values stored in BOS eeprom ' if servo >11 then return to main command prompt ' g - gripper lift and grip ' 0 => no change, 255 - relax servo ' (easy to type a 0 for no change ' or wait for 5sec timeout) ' v - voice, beep,Hz/10,ms/10 'Requests - one argument:- ' ?g - arm and grip positions -> ' ?s - Sspeed -> ' ?r - rideheight -> ' ?m - move -> ' ?l - lift -> ' ?n - neutral positions, ' get EEPROM data for neutral position ' for servos 0 - 11 -> ' ?v - BOS version -> <"ver",dec BOSver,cr> '3 byte:- ' - direct control of servos ' not suitable for human use from terminal, ' byte values and 100ms timeout ' turns on servo power and exits with servo power on ' first byte - control byte (non printing) character (<6) ' $speed;$servo - high nibble and low nibble respectively ' 0,$speed;$servo,position ' 1,$XX,$servo,X ->P actual servo position ' 2,$XX,$servo,X ->S servo status, bit1-on bit0-done ' 3,X,X ->hiF,then loF two bytes all servo moved status ' bitX=servoX, 1=>finished ' bit15 is set to 1 ' so all finished=$FFFF ' 4,X,X ->hiR,then loR two bytes all servo on/off status ' bitX=servoX, 1=>being pulsed, ie on ' bit15 always 1 ' 5,$XX;$servo,X stop pulsing servo 'The servos are refered to by their logical positions, ' see below and map. 'The Data area contains defaults: servo speed, stride, ride height, leg lift, ' these are mirrored in variables for dynamic control; ' and home position of servos - D_sneutral which are not mirrored. 'Following are the Pose data; servo,position,,,,, ending with 15. 'Following are the Sequence data: pose1,pose2,... ending with 0 'The pgm loops round 'dosequence' until all is done then waits for a command. 'If the command is ascii<6 then the pgm jumps to direct binary control of the ' servo co-processors,see above, then jumps back to normal control. 'The location of the logical servos on each servo cp and the location of the ' tx/rx and flow pins of each servo cp is mapped at 'servmap'. '-------- Legs & Logical-Servos -------- ' ' Plan view of Hextor S - Servo(0 - 13) L - Legs(1 - 6) ' S L L S ' ( ) gripper 13 ' | | lift 12 ' L---F---R LEG NUMBERS ' 1,0 | 1 2 | 6,7 1=LF 2=RF ,L - left, R - Right ' 3,2 | 3 4 | 8,9 3=LM 4=RM ,M - mid, F - Front ' 5,4 | 5 6 | 10,11 5=LB 6=RB ,B - Back ' \-------/ ' '-------- Servo values and leg positions ------- ' f - forward m - middle b - backward 'Left Lf=255 Lm=127 Lb=0 'Right Rf=0 Rm=127 Rb=255 ' ' d - Down u - Up 'Left Ld=0 Lu=255 'Right Rd=255 Ru=0 ' '-------- Arm and Gripper positions ------- 'Arm up 255 'Arm down 0 'Gripper open 255 'Gripper closed 0 '------------------------------------- 'Notes '1. In data Pose area servo=15 is used a terminator value used in 'getpose' ' servos values 12-15 are not supported, after the FOR/NEXTs servo=12 '2. initial leg swing forward/backward controlled by DATA value at D_swing ' during runtime value held in swing '3. initial leg down from neutal position controlled by DATA at D_legdown ' during runtime value held in legdown '4. initial leg move speed controlled by DATA at D_Sspeed ' during runtime value held in Sspeed '5. leg midswing forward/backward positions controlled by DATA D_sneutral '6. WHAT DID I MEAN BY THIS, I DO NOT UNDERSTAND IT!!!!! 12-11-01 ' This code will result in P_stand being done and ' then the test of D_ptr=0 at the end of domove will result in ' the sequence S_FD being done ' S_ptr =S_FD ' P_ptr =P_stand :gosub getmove '7. In the data Sequence area, 0 is used as a terminator for the sequence '8. Gripper/Lift is not under Pose control ' When using IR control user can see when gripper has stopped moving ' When under BS2sx control either use a pause or test with direct control '9. The physical location of the EEPROM data space on the PIC starts at $2100. ' Given a line "S_null DATA 0" where S_null points to the first DATA item in the program. ' Examine the value of S_null:- (debug S_null=",hex4 S_null,13) -> $2100. ' Internally, PBP leaves the $21 part off and just uses the lower byte. [Jeff Schmoyer 31-5-01] '10.The '876 has 256 bytes of Data memory. So I used indexes of 256 and up for Wave Gaits. ' Initially I tested if pointer was >255 but this didn't work as base of Memory is at ' $7000. So for Wave gaites indexes are $7000 + 256+i '------------------------------------------- '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 '[pulse as measured by BS2, 0=>1018us, 255=>2042us ie 4us resolution] '-------- Logical posture constants = servo positions ------- smin CON 2 'minimum servo position, 0,1 metacmnds smid CON 127 'midd servo position smax CON 255 'maximum servo position 'values used in posture control, - s(ervo),L(eft)/R(ight),f(orward)/m(idd)/b(ack) 'modified by physical values in D_sneutral, D_swing 'f - forward m - middle b - backward sLf CON smax :sLm CON smid :sLb CON smin 'Left sRf CON smin :sRm CON smid :sRb CON smax 'Right 'd - Down u - Up sLd CON smin :sLu CON smax 'Left sRd CON smax :sRu CON smin 'Right '------- Posture Control ---------- S_null DATA 0 'data ptr=0 =>non valid location 'even servos mid swing, odd servos up 'use NEUTRAL to calibrate these logical servo values D_sneutral DATA 127,255,127,255,127,255,127,0,127,0,127,0 D_swing DATA 40 '(0-127)40 default leg swing +-about midswing D_legdown DATA 170 '(0-255) 170, down from neutral position D_lift DATA 90 '(0-255) 90, up from down position D_Sspeed DATA 4 '(0-15) 4 is recommended start value '+++++++ Pose data, uses logical servo values ++++++++++++++ 'data is (servo 0-11),(position),...,(servo=15 is terminator) P_stand DATA 1,sLd,3,sLd,5,sLd,7,sRd,9,sRd,11,sRd,15 '=========== Tripod Wallking ====== 'lead legs are 1,2 so standliftL=>stand on legs 2,3,6 and lift 1,4,5 P_standliftL DATA 1,sLu,5,sLu,9,sRu,15 'standwalkL=>stand on legs 1,4,5 and lift 2,3,6 P_standliftR DATA 3,sLu,7,sRu,11,sRu,15 P_standMidup DATA 3,sLu,9,sRu,15 '------------ pace ------------ 'swing right foot(2) forwards and left front(1) backwards P_paceBL DATA 'same address as FR P_paceFR DATA 0,sLb,2,sLf,4,sLb,6,sRf,8,sRb,10,sRf,15 'swing left foot(1) forwards and right front(2) backwards P_paceBR DATA 'same address as FL P_paceFL DATA 0,sLf,2,sLb,4,sLf,6,sRb,8,sRf,10,sRb,15 'swing legs to mid position P_paceMIDL DATA 0,sLm,8,sRm,4,sLm,15 P_paceMIDR DATA 6,sRm,2,sLm,10,sRm,15 ' ----------- turn ----------- 'turn Anticlockwise Left foot Down 'or turn Clockwise Right foot Down P_turnALD DATA 'same address as CRD P_turnCRD DATA 0,sLf,2,sLb,4,sLf,6,sRf,8,sRb,10,sRf,15 'turn Anticlockwise Right foot Down 'or turn Clockwise Left foot Down P_turnARD DATA 'same address as CLD P_turnCLD DATA 0,sLb,2,sLf,4,sLb,6,sRb,8,sRf,10,sRb,15 ' ----------- pose-cmnds -- (final 15 ommited as is ignored) ---- P_Qnowait DATA 0,0 'if Gaittype="Q" then 'don't wait for a Pose to finish, load next Pose immediately, 'ie combine Pose values. P_Qfbwait DATA 0,1 'if Gaittype="Q" then load next Pose on flyby '++++++++++++ Sequence DATA +++end with P_ptr=0 ++++++++++++++ S_STAND DATA P_stand,0 S_FDrm DATA P_stand,P_Qnowait,P_standliftR,P_paceMIDR,P_Qfbwait,P_stand 'and drop through to FDl S_FDl DATA P_stand,P_Qnowait,P_standliftL,P_Qfbwait,P_paceFL,P_stand,0 S_FDlm DATA P_stand,P_Qnowait,P_standliftL,P_paceMIDL,P_Qfbwait,P_stand 'and drop through to FDr S_FDr DATA P_stand,P_Qnowait,P_standliftR,P_Qfbwait,P_paceFR,P_stand,0 S_BKrm DATA P_stand,P_Qnowait,P_standliftR,P_paceMIDR,P_Qfbwait,P_stand 'and drop through to BKl S_BKl DATA P_stand,P_Qnowait,P_standliftL,P_Qfbwait,P_paceBL,P_stand,0 S_BKlm DATA P_stand,P_Qnowait,P_standliftL,P_paceMIDL,P_Qfbwait,P_stand 'and drop through to BKr S_BKr DATA P_stand,P_Qnowait,P_standliftR,P_Qfbwait,P_paceBR,P_stand,0 S_RTr DATA P_stand,P_Qnowait,P_standliftR,P_Qfbwait,P_turnCLD,P_stand,0 S_RTl DATA P_stand,P_Qnowait,P_standliftL,P_Qfbwait,P_turnCRD,P_stand,0 S_LTl DATA P_stand,P_Qnowait,P_standliftL,P_Qfbwait,P_turnARD,P_stand,0 S_LTr DATA P_stand,P_Qnowait,P_standliftR,P_Qfbwait,P_turnALD,P_stand,0 S_MID DATA P_standliftL,P_paceMIDL,P_stand DATA P_standliftR,P_paceMIDR,P_stand,0 '-------- constants and pins -------- 'see above for posture constants 'i96n con 16624 'BS2sx 9600 baud, 8 bit, no parity, inverted i96n CON 16468 'BS2 9600 baud, 8 bit, no parity, inverted '$4000+84=16384+84=16468 i24n CON 16780 'BS2 2400 baud, 8 bit, no parity, inverted baudsel VAR portA.0 'baud select default high 9600, jumper low 2400 power VAR portA.1 'power relay cmndbusy VAR portA.2 'busy pin - high=busy cmndhalt VAR portA.3 'halt pin - HALT if high voicepin VAR portB.3 'beeper 'pins C0,1,2,3,4,5, B0,1,4,5 used for servo-coprocs 'pins B2,6,7 spare 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 cmndrx VAR portC.6 cmndtx VAR portC.7 CR CON 13 '-------- Sequence and Pose values for lookdown Data -------- 'The physical location of the EEPROM data space on the PIC is $2100 '$F000 is an arbitrary value greater than $2100+256 'originally P_ptr was going to be 0 to 255 for DATA and >255 for lookup ' but the EEPROM is at $2100 so lookup had to be >$2100+256, hence $F000 S_0F CON 256 +$F000 S_0M CON 257 +$F000 S_0B CON 258 +$F000 S_1F CON 259 +$F000 S_1M CON 260 +$F000 S_1B CON 261 +$F000 S_2F CON 262 +$F000 S_2M CON 263 +$F000 S_2B CON 264 +$F000 S_3F CON 265 +$F000 S_3M CON 266 +$F000 S_3B CON 267 +$F000 S_4F CON 268 +$F000 S_4M CON 269 +$F000 S_4B CON 270 +$F000 S_5F CON 271 +$F000 S_5M CON 272 +$F000 S_5B CON 273 +$F000 S_6F CON 274 +$F000 S_6M CON 275 +$F000 S_6B CON 276 +$F000 P_0F CON 256 +$F000 P_0M CON 257 +$F000 P_0B CON 258 +$F000 P_1uw CON 259 +$F000 P_1f CON 260 +$F000 P_1m CON 261 +$F000 P_1b CON 262 +$F000 P_2uw CON 263 +$F000 P_2f CON 264 +$F000 P_2m CON 265 +$F000 P_2b CON 266 +$F000 P_3uw CON 267 +$F000 P_3f CON 268 +$F000 P_3m CON 269 +$F000 P_3b CON 270 +$F000 P_4uw CON 271 +$F000 P_4f CON 272 +$F000 P_4m CON 273 +$F000 P_4b CON 274 +$F000 P_5uw CON 275 +$F000 P_5f CON 276 +$F000 P_5m CON 277 +$F000 P_5b CON 278 +$F000 P_6uw CON 279 +$F000 P_6f CON 280 +$F000 P_6m CON 281 +$F000 P_6b CON 282 +$F000 '-------- variables -------- baud VAR Word 'baudcode D_ptr VAR Word 'data ptr P_ptr VAR D_ptr 'pose ptr S_ptr VAR Word 'pointer to sequence flagbyte VAR Byte lfootpos VAR flagbyte.BIT0 'tox position of left front foot, servo0 'set in getpose. sLf(=smax) =>0 askew VAR flagbyte.BIT1 '1=> legs set askew by turn haltingf VAR flagbyte.BIT2 '1=> halting because haltpin is high or "H" received servo VAR Byte 'leg servos 0-11, lift 12, gripper 13 tox VAR Byte 'general to position toxl VAR Byte 'used in grippos toxg VAR Byte 'used in grippos movedflags VAR Word 'servos 0 -14, bit15 =1 m15f VAR movedflags.BIT15 'checkdone sets bit15=1 so all done=$FFFF lupx VAR Byte 'up position at D_sneutral midswingx VAR lupx Sdelta VAR Byte 'gripper movement 'lift legdown Sspeed initialised from DATA 'dynamically alter swing, lift, legdown, Sspeed, swing VAR Byte 'leg swing about mid position lift VAR Byte 'leg lift legdown VAR Byte 'leg down pos Sspeed VAR Byte 'Servo speed cpSaddress VAR Byte 'Servo address for co-processor Sstatus VAR Byte 'servo status byte from co-processor flowx VAR Byte 'cp flow control line txx VAR Byte 'cp data line rxx VAR txx 'cp data line command VAR Byte 'command to serial co-processor cmnd VAR command 'serin cmnd action VAR Byte 'desired action Gaittype VAR Byte 'T -Tall, Q -Quick, q -without wait, f -flyby wait flyby VAR Byte 'servo0 flyby value triggering load next pose vfreq VAR Word 'voice frequency vtime VAR Word 'note length htime VAR vtime 'halttime waveseq VAR Byte wavepose VAR Byte wavSindx VAR Byte wavPindx VAR Byte '-------- initialise variables -------- ADCON1 =6 'set portA to digital io baud =i96n IF baudsel=1 THEN i1 baud =i24n i1: Gaittype ="Q" 'default to Quick READ D_swing,swing READ D_legdown,legdown READ D_lift,lift READ D_Sspeed,Sspeed HIGH cmndbusy 'not ready yet LOW power 'servo power relay off trisA.1 =0 'output, servo power control trisA.2 =0 'output, busy movedflags =$FFFF 'movedflags.lowbit(servo)=<0 moving>,<1 moved> servo =15 'illegal value =>finished pose '------ uncomment for test routines ------------- 'goto tservo 'test each servo 'goto tsit 'test sitting position '------------------- Program start ------------------- serout2 cmndtx,baud,[0,"Bug Commander BOS ",DEC BOSver,", Milford Instruments",CR] '---------- initialise servo cps to relax ------------ Zzzz: FOR servo =0 TO 14 'include 15th servo so its pulse flag is zero GOSUB servoff NEXT 'DEBUG"relaxed",cr 'and drop through to Kill and getcmnd '---------------------------------- Kill: LOW power 'and drop through to getcmnd '----------- MAIN ROUTINE re-entry point ---------- getcmnd: haltingf =0 serout2 cmndtx,baud,[">"] LOW cmndbusy rxget: IF cmndhalt=1 THEN rxget 'wait until HALT removed serin2 cmndrx,baud,5000,rxget,[cmnd] serout2 cmndtx,baud,[cmnd] HIGH cmndbusy IF cmnd<6 THEN CONTROL '0 - 5 CONTROL cmnds action =255 LOOKDOWN cmnd,["FBRLHSUMNKZQTWVpPdDoOcCgsrmlnvwh0123456?"],action branchL action,[Fd,Bk,Rt,Lt,Halt,Sit,standUp,Mid,Neutral,_ Kill,Zzzz,GaitQ,GaitT,Waitf,Beeper,_ pick16,Pick4,down16,Down4,_ open16,Open4,close16,Close4,grippos,_ speedval,rideval,moveval,liftval,neutval,_ voice,waitservo,waittime,_ rxWget,rxWget,rxWget,rxWget,rxWget,rxWget,rxWget,_ Query] what: serout2 cmndtx,baud,[CR,"?"] GOTO getcmnd rxWget: serin2 cmndrx,baud,5000,getcmnd,[cmnd] serout2 cmndtx,baud,[cmnd] tox =255 LOOKDOWN cmnd,["fmb"],tox IF tox=255 THEN what 'invalid cmnd wavSindx =0 action =action -32 branchL action,[body,leg1,leg2,leg3,leg4,leg5,leg6] GOTO getcmnd '------------------------------------ Query: serin2 cmndrx,baud,5000,getcmnd,[cmnd] serout2 cmndtx,baud,[cmnd] action =255 LOOKDOWN cmnd,["gsrmlnv"],action branchL action,[Qg,Qs,Qr,Qm,Ql,Qn,Qv] GOTO getcmnd Qg: servo =12 GOSUB getspos serout2 cmndtx,baud,["l=",DEC tox,CR] servo =13 GOSUB getspos serout2 cmndtx,baud,["g=",DEC tox,CR] GOTO getcmnd Qs: serout2 cmndtx,baud,[DEC Sspeed,CR] GOTO getcmnd Qr: serout2 cmndtx,baud,[DEC legdown,CR] GOTO getcmnd Qm: serout2 cmndtx,baud,[DEC swing,CR] GOTO getcmnd Ql: serout2 cmndtx,baud,[DEC lift,CR] GOTO getcmnd Qn: serout2 cmndtx,baud,[CR,"neutral",CR] FOR servo =0 TO 11 READ D_sneutral+servo,tox serout2 cmndtx,baud,[DEC servo," ",DEC tox,CR] NEXT GOTO getcmnd Qv: PAUSE 20 'give BS2sx time to turn round serout2 cmndtx,baud,2,[DEC BOSver,CR] 'every 2ms GOTO getcmnd '------------------------------------ waittime: serin2 cmndrx,baud,5000,getcmnd,[DEC htime] serout2 cmndtx,baud,[DEC htime] PAUSE (htime *100) GOTO getcmnd '------------------------------------ voice: serin2 cmndrx,baud,5000,getcmnd,[DEC vfreq] serout2 cmndtx,baud,[DEC vfreq] serin2 cmndrx,baud,5000,getcmnd,[DEC vtime] serout2 cmndtx,baud,[DEC vtime] vfreq =vfreq *10 vtime =vtime *10 FREQOUT voicepin,vtime,vfreq GOTO getcmnd '------------------------------------ Beeper: FREQOUT voicepin,100,4000 GOTO getcmnd '------------------------------------ Waitf: GOSUB checkdone IF movedflags<>$FFFF THEN waitf GOTO getcmnd '------------------------------------ waitservo: serin2 cmndrx,baud,5000,getcmnd,[DEC servo] serout2 cmndtx,baud,[DEC servo] wservo: GOSUB checkSdone IF movedflags.LOWBIT(servo)=0 THEN wservo GOTO getcmnd '------------------------------------ GaitQ: Gaittype ="Q" GOTO getcmnd '------------------------------------ GaitT: Gaittype ="T" GOTO getcmnd '------------------------------------ speedval: serin2 cmndrx,baud,5000,getcmnd,[DEC Sspeed] serout2 cmndtx,baud,[DEC Sspeed] GOTO getcmnd '------------------------------------ rideval: serin2 cmndrx,baud,5000,getcmnd,[DEC legdown] serout2 cmndtx,baud,[DEC legdown] GOTO getcmnd '------------------------------------ moveval: serin2 cmndrx,baud,5000,getcmnd,[DEC swing] serout2 cmndtx,baud,[DEC swing] GOTO getcmnd '------------------------------------ liftval: serin2 cmndrx,baud,5000,getcmnd,[DEC lift] serout2 cmndtx,baud,[DEC lift] GOTO getcmnd '------------------------------------ neutval: servo =15 serin2 cmndrx,baud,5000,getcmnd,[DEC servo] serout2 cmndtx,baud,[DEC servo] serin2 cmndrx,baud,5000,getcmnd,[DEC tox] serout2 cmndtx,baud,[DEC tox] IF servo>11 THEN getcmnd WRITE D_sneutral+servo,tox GOTO getcmnd '----- Gripper ------------------------------- pick16: servo =12 Sdelta =16 GOTO ingrip Pick4: servo =12 Sdelta =64 GOTO ingrip down16: servo =12 Sdelta =16 GOTO outgrip Down4: servo =12 Sdelta =64 GOTO outgrip open16: servo =13 Sdelta =16 GOTO outgrip Open4: servo =13 Sdelta =64 GOTO outgrip close16: servo =13 Sdelta =16 GOTO ingrip Close4: servo =13 Sdelta =64 GOTO ingrip outgrip: GOSUB getspos tox =(tox +Sdelta) MIN 255 HIGH POWER GOSUB seroutval GOTO getcmnd ingrip: GOSUB getspos 'DEBUG "GRIP ",#TOX," ",#SDELTA tox =(tox MAX Sdelta) -Sdelta 'so we don't go below 0 'DEBUG "=",#TOX,CR HIGH POWER GOSUB seroutval GOTO getcmnd '------------------------------------ grippos: LOW cmndbusy serin2 cmndrx,baud,5000,getcmnd,[DEC toxl]'abort GRIPPER if no reply serout2 cmndtx,baud,[DEC toxl] serin2 cmndrx,baud,5000,getcmnd,[DEC toxg]'abort GRIPPER if no reply serout2 cmndtx,baud,[DEC toxg] HIGH cmndbusy IF toxl=0 THEN gpos 'don't change if 0 servo =12 IF toxl<>255 THEN serl GOSUB servoff 'servo sleep if 255 GOTO gpos serl: GOSUB seroutval gpos: IF toxg=0 THEN getcmnd 'don't change if 0 servo =13 IF toxg<>255 THEN serg GOSUB servoff 'sleep if 255 GOTO getcmnd serg: GOSUB seroutval GOTO getcmnd '------------------------------------ Sit: FOR servo =1 TO 11 STEP 2 'do all lift servos READ D_sneutral+servo,tox 'read neutral position GOSUB seroutval 'tell it to go there NEXT S_ptr =S_null GOTO qdone 'now wait for it to be done '------------------------------------ Neutral: FOR servo =0 TO 11 'do all servos READ D_sneutral+servo,tox 'read neutral position GOSUB seroutval 'tell it to go there NEXT S_ptr =S_null GOTO qdone 'now wait for it to be done '------------------------------------ Halt: 'stop where we are and Stand on 6 legs 'DEBUG"HALT",CR haltingf =1 FOR servo =0 TO 11 GOSUB servmap command =servread +cpSaddress _hq:serout2 txx\flowx,i96n,[command,0] 'ask where servo is serin2 rxx,i96n,500,_hq,[tox] 'get reply command =Sspeed<<4 +servset +cpSaddress serout2 txx\flowx,i96n,[command,tox] 'tell it to stay there NEXT 'don't need to wait for servos to get to tox as they are at tox movedflags =movedflags | $FFF 'movedflags.lowbit(servo)=<0 moving>,<1 moved> 'drop through to stand '------------------------------------ standUp: S_ptr =S_STAND GOTO dosequence '------------------------------------ Fd: tox =lfootpos IF askew=0 THEN Fdu tox =tox +2 Fdu: lookup2 tox,[S_FDr,S_FDl,S_FDlm,S_FDrm],S_ptr askew =0 GOTO dosequence '------------------------------------ Bk: tox =lfootpos IF askew=0 THEN Bku tox =tox +2 Bku: lookup2 tox,[S_BKl,S_BKr,S_BKrm,S_BKlm],S_ptr askew =0 GOTO dosequence '------------------------------------ Rt: lookup2 lfootpos,[S_RTr,S_RTl],S_ptr askew =1 GOTO dosequence '------------------------------------ Lt: lookup2 lfootpos,[S_LTl,S_LTr],S_ptr askew =1 GOTO dosequence '------------------------------------ Mid: S_ptr =S_MID askew =0 GOTO dosequence '-------------- Wave Gait ---------------------- body: branchL tox,[bodyF,bodyM,bodyB] bodyF: S_ptr =S_0F :GOTO dosequence bodyM: S_ptr =S_0M :GOTO dosequence bodyB: S_ptr =S_0B :GOTO dosequence '------------------------------------ leg1: branchL tox,[leg1F,leg1M,leg1B] leg1F: S_ptr =S_1F :GOTO dosequence leg1M: S_ptr =S_1M :GOTO dosequence leg1B: S_ptr =S_1B :GOTO dosequence '------------------------------------ leg2: branchL tox,[leg2F,leg2M,leg2B] leg2F: S_ptr =S_2F :GOTO dosequence leg2M: S_ptr =S_2M :GOTO dosequence leg2B: S_ptr =S_2B :GOTO dosequence '------------------------------------ leg3: branchL tox,[leg3F,leg3M,leg3B] leg3F: S_ptr =S_3F :GOTO dosequence leg3M: S_ptr =S_3M :GOTO dosequence leg3B: S_ptr =S_3B :GOTO dosequence '------------------------------------ leg4: branchL tox,[leg4F,leg4M,leg4B] leg4F: S_ptr =S_4F :GOTO dosequence leg4M: S_ptr =S_4M :GOTO dosequence leg4B: S_ptr =S_4B :GOTO dosequence '------------------------------------ leg5: branchL tox,[leg5F,leg5M,leg5B] leg5F: S_ptr =S_5F :GOTO dosequence leg5M: S_ptr =S_5M :GOTO dosequence leg5B: S_ptr =S_5B :GOTO dosequence '------------------------------------ leg6: branchL tox,[leg6F,leg6M,leg6B] leg6F: S_ptr =S_6F :GOTO dosequence leg6M: S_ptr =S_6M :GOTO dosequence leg6B: S_ptr =S_6B :GOTO dosequence '-------------- Wave Gait -- this makes much more code ---- 'body: lookdown2 tox,[S_0F,S_0M,S_0B],S_ptr 'DEBUG "tox=",#tox ' goto dosequence '------------------------------------ 'leg1: lookdown2 tox,[S_1F,S_1M,S_1B],S_ptr ' goto dosequence '------------------------------------ 'leg2: lookdown2 tox,[S_2F,S_2M,S_2B],S_ptr ' goto dosequence '------------------------------------ 'leg3: lookdown2 tox,[S_3F,S_3M,S_3B],S_ptr ' goto dosequence '------------------------------------ 'leg4: lookdown2 tox,[S_4F,S_4M,S_4B],S_ptr ' goto dosequence '------------------------------------ 'leg5: lookdown2 tox,[S_5F,S_5M,S_5B],S_ptr ' goto dosequence '------------------------------------ 'leg6: lookdown2 tox,[S_6F,S_6M,S_6B],S_ptr ' goto dosequence '------------------------------------ dosequence:'sequence->S_ptr[dosequence] 'DEBUG"DOSEQ",CR '-------- start section get sequence -------- 'a bit monolithic because of limited gosub stack if power=1 then _DSQS_ptr pause 100 HIGH power 'make sure servo power is on _DSQS_ptr: 'DEBUG "S_ptr=",hex4 S_ptr," S_null=",hex4 S_null,13 IF S_ptr<$F000 THEN readSdata 'DEBUG"lookupS" GOSUB lookupS GOTO doneSdata readSdata: 'DEBUG"readSdata" READ S_ptr,P_ptr S_ptr =S_ptr +1 doneSdata: IF P_ptr=0 THEN getcmnd '------- start section dopose ------- getpose:'P_ptr[getmove]-> update relevant to 'DEBUG"GETPOSE",CR IF P_ptr<$F000 THEN readPdata0 GOSUB lookupP 'read value into tox servo =tox GOTO donePdata0 readPdata0: READ P_ptr,servo P_ptr =P_ptr +1 donePdata0: IF servo=15 THEN qhalt 'got pose so wait till done IF P_ptr<$F000 THEN readPdata1 GOSUB lookupP 'read value into tox GOTO donePdata1 readPdata1: READ P_ptr,tox nxtP: P_ptr =P_ptr +1 donePdata1: IF tox>1 THEN qs0 IF Gaittype="T" THEN dosequence 'ignore pose-cmnds If T IF tox=1 THEN setfly Gaittype ="q" 'quick - no waits between poses GOTO dosequence setfly: Gaittype ="f" 'flyby wait between poses GOTO dosequence 'now qs0: IF servo<>0 THEN posecalcs LOOKDOWN tox,[sLf,sLb],lfootpos posecalcs: 'even numbered servos do fd/bk(0,2..), odd ones up and down(1,3..) 'modify fd/bk by swing 'modify up/down by legdown and lift 'on a byte Parallax(2-3 min 0)=255 so make sure we don't go through 0 or >255! IF servo.LOWBIT<>1 THEN swings 'even servos swing, odds lift lifts: READ D_sneutral+servo,lupx IF servo>5 THEN Rside Lside: IF tox=smin THEN ldn llift: tox =((lupx MAX legdown) -legdown +lift) MIN 255 GOTO putval ldn: tox =(lupx MAX legdown) -legdown GOTO putval Rside: IF tox=smax THEN rdn rlift: tox =(((lupx +legdown) MIN 255) MAX lift) -lift GOTO putval rdn: tox =(lupx +legdown) MIN 255 GOTO putval swings: READ D_sneutral+servo,midswingx IF tox<>smid THEN swingsums 'test against logical midswing tox =midswingx IF servo<>0 THEN putval flyby =midswingx 'not easy to set as don't know leg fromx GOTO putval swingsums: IF tox>midswingx THEN adds tox =(midswingx MAX swing) -swing IF servo<>0 THEN putval flyby =tox +lift GOTO putval adds: tox =(midswingx +swing) MIN 255 IF servo<>0 THEN putval flyby =(tox MAX lift) -lift putval: GOSUB seroutval 'put value to servo GOTO getpose 'get rest of pose data '------- qhalt:'DEBUG"QHALT",CR IF haltingf=1 THEN qdone serin2 cmndrx,baud,100,noser,[cmnd] noser:'DEBUG"NOSER",CR 'DEBUG"CMNDHALT=",#CMNDHALT,CR IF cmndhalt=1 OR cmnd="H" THEN HALT 'test halt pin and serial qdone:'DEBUG"QDONE",CR IF Gaittype="q" THEN dosequence 'load next pose NOW qT: IF Gaittype="T" OR Gaittype="Q" THEN qflags 'else 'gaittype must be 'f' GOSUB qflyby IF flyby=255 THEN flybyend 'set Gaittype='f' back to 'Q', load next pose qflags:'DEBUG"QFLAGS",CR GOSUB checkdone 'DEBUG bin16 movedflags,cr IF (movedflags & $FFF) <> $FFF THEN qhalt 'wait until all legs done IF Gaittype="T" THEN dosequence flybyend: Gaittype ="Q" GOTO dosequence '-----end section dopose --------- '-------- sub routines ------------ seroutval: tox =(tox MIN smax) MAX smin 'don't overtravel servos movedflags.LOWBIT(servo) =0 GOSUB servmap command =Sspeed<<4 +servset +cpSaddress 'DEBUG IBIN8 COMMAND," ",#TOX serout2 txx\flowx,i96n,[command,tox] RETURN '--------------------------------- checkdone: FOR servo =0 TO 12 STEP 3 GOSUB servmap askdone:'DEBUG" checking",#servo serout2 txx\flowx,i96n,[Qstatus,0] '?status serin2 rxx,i96n,50,askdone,[Sstatus] 'DEBUG IBIN8 sSTATUS,CR FOR cpSaddress =0 TO 2 movedflags.LOWBIT(servo+cpSaddress) =Sstatus.LOWBIT(cpSaddress) NEXT NEXT m15f =1 'set movedflags.bit15=1 so all done=$FFFF RETURN '--------------------------------- checkSdone: GOSUB servmap askSdone:'DEBUG" checking",#servo serout2 txx\flowx,i96n,[Qstatus,0] '?status serin2 rxx,i96n,50,askSdone,[Sstatus] 'DEBUG IBIN8 sSTATUS,CR movedflags.LOWBIT(servo) =Sstatus.LOWBIT(cpSaddress) RETURN '--------------------------------- qflyby:'only on servo 0 servo =0 GOSUB getspos branchL lfootpos,[qfbF,qfbB] qfbF: IF tox>=flyby THEN qfbdone RETURN qfbB: IF tox<=flyby THEN qfbdone RETURN qfbdone: flyby =255 RETURN '--------------------------------- servmap: LOOKUP servo,[2,1,0,2,1,0,0,1,2,0,1,2,2,1,0],cpSaddress LOOKUP servo,[10,10,10,8,8,8,0,0,0,12,12,12,4,4,4],txx LOOKUP servo,[11,11,11,9,9,9,1,1,1,13,13,13,5,5,5],flowx RETURN '-------------------------- getspos: GOSUB servmap command =servread +cpSaddress ask1: 'DEBUG"willask ",#servo," " serout2 txx\flowx,i96n,[command,tox] serin2 rxx,i96n,50,ask1,[tox] 'DEBUG "getspos=",#tox,cr RETURN '-------------------------- servoff: GOSUB servmap command =srelax +cpSaddress serout2 txx\flowx,i96n,[command,0] RETURN '------------- Sequence data for Wave Gait --------- lookupS: waveseq =S_ptr -256 -$F000 branchL waveseq,[LS_0F,LS_0M,LS_0B,_ LS_1F,LS_1M,LS_1B,_ LS_2F,LS_2M,LS_2B,_ LS_3F,LS_3M,LS_3B,_ LS_4F,LS_4M,LS_4B,_ LS_5F,LS_5M,LS_5B,_ LS_6F,LS_6M,LS_6B] P_ptr =0 RETURN LS_0F: lookup2 wavSindx,[P_stand,P_0F,0],P_ptr wavPindx =0 wavSindx =wavSindx +1 RETURN LS_0M: lookup2 wavSindx,[P_stand,P_0M,0],P_ptr wavPindx =0 wavSindx =wavSindx +1 RETURN LS_0B: lookup2 wavSindx,[P_stand,P_0B,0],P_ptr wavPindx =0 wavSindx =wavSindx +1 RETURN '----------- LS_1F: lookup2 wavSindx,[P_Qnowait,P_1uw,P_Qfbwait,P_1f,P_stand,0],P_ptr wavPindx =0 wavSindx =wavSindx +1 RETURN LS_1M: lookup2 wavSindx,[P_Qnowait,P_1uw,P_Qfbwait,P_1m,P_stand,0],P_ptr wavPindx =0 wavSindx =wavSindx +1 RETURN LS_1B: lookup2 wavSindx,[P_Qnowait,P_1uw,P_Qfbwait,P_1b,P_stand,0],P_ptr wavPindx =0 wavSindx =wavSindx +1 RETURN '----------- LS_2F: lookup2 wavSindx,[P_Qnowait,P_2uw,P_Qfbwait,P_2f,P_stand,0],P_ptr wavPindx =0 wavSindx =wavSindx +1 RETURN LS_2M: lookup2 wavSindx,[P_Qnowait,P_2uw,P_Qfbwait,P_2m,P_stand,0],P_ptr wavPindx =0 wavSindx =wavSindx +1 RETURN LS_2B: lookup2 wavSindx,[P_Qnowait,P_2uw,P_Qfbwait,P_2b,P_stand,0],P_ptr wavPindx =0 wavSindx =wavSindx +1 RETURN '----------- LS_3F: lookup2 wavSindx,[P_Qnowait,P_3uw,P_Qfbwait,P_3f,P_stand,0],P_ptr wavPindx =0 wavSindx =wavSindx +1 RETURN LS_3M: lookup2 wavSindx,[P_Qnowait,P_3uw,P_Qfbwait,P_3m,P_stand,0],P_ptr wavPindx =0 wavSindx =wavSindx +1 RETURN LS_3B: lookup2 wavSindx,[P_Qnowait,P_3uw,P_Qfbwait,P_3b,P_stand,0],P_ptr wavPindx =0 wavSindx =wavSindx +1 RETURN '----------- LS_4F: lookup2 wavSindx,[P_Qnowait,P_4uw,P_Qfbwait,P_4f,P_stand,0],P_ptr wavPindx =0 wavSindx =wavSindx +1 RETURN LS_4M: lookup2 wavSindx,[P_Qnowait,P_4uw,P_Qfbwait,P_4m,P_stand,0],P_ptr wavPindx =0 wavSindx =wavSindx +1 RETURN LS_4B: lookup2 wavSindx,[P_Qnowait,P_4uw,P_Qfbwait,P_4b,P_stand,0],P_ptr wavPindx =0 wavSindx =wavSindx +1 RETURN '----------- LS_5F: lookup2 wavSindx,[P_Qnowait,P_5uw,P_Qfbwait,P_5f,P_stand,0],P_ptr wavPindx =0 wavSindx =wavSindx +1 RETURN LS_5M: lookup2 wavSindx,[P_Qnowait,P_5uw,P_Qfbwait,P_5m,P_stand,0],P_ptr wavPindx =0 wavSindx =wavSindx +1 RETURN LS_5B: lookup2 wavSindx,[P_Qnowait,P_5uw,P_Qfbwait,P_5b,P_stand,0],P_ptr wavPindx =0 wavSindx =wavSindx +1 RETURN '----------- LS_6F: lookup2 wavSindx,[P_Qnowait,P_6uw,P_Qfbwait,P_6f,P_stand,0],P_ptr wavPindx =0 wavSindx =wavSindx +1 RETURN LS_6M: lookup2 wavSindx,[P_Qnowait,P_6uw,P_Qfbwait,P_6m,P_stand,0],P_ptr wavPindx =0 wavSindx =wavSindx +1 RETURN LS_6B: lookup2 wavSindx,[P_Qnowait,P_6uw,P_Qfbwait,P_6b,P_stand,0],P_ptr wavPindx =0 wavSindx =wavSindx +1 RETURN '------------- Pose data for Wave Gait --------- lookupP: wavepose =P_ptr -256 -$F000 branchL wavepose,[LP_0F,LP_0M,LP_0B,_ LP_1uw,LP_1f,LP_1m,LP_1b,_ LP_2uw,LP_2f,LP_2m,LP_2b,_ LP_3uw,LP_3f,LP_3m,LP_3b,_ LP_4uw,LP_4f,LP_4m,LP_4b,_ LP_5uw,LP_5f,LP_5m,LP_5b,_ LP_6uw,LP_6f,LP_6m,LP_6b] servo =15 RETURN LP_0F: lookup2 wavPindx,[0,sLb,2,sLb,4,sLb,6,sRb,8,sRb,10,sRb,15],tox wavPindx =wavPindx +1 RETURN LP_0M: lookup2 wavPindx,[0,sLm,2,sLm,4,sLm,6,sRm,8,sRm,10,sRm,15],tox wavPindx =wavPindx +1 RETURN LP_0B: lookup2 wavPindx,[0,sLf,2,sLf,4,sLf,6,sRf,8,sRf,10,sRf,15],tox wavPindx =wavPindx +1 RETURN LP_1uw: lookup2 wavPindx,[1,sLu,15],tox wavPindx =wavPindx +1 RETURN LP_1f: lookup2 wavPindx,[0,sLf,15],tox wavPindx =wavPindx +1 RETURN LP_1m: lookup2 wavPindx,[0,sLm,15],tox wavPindx =wavPindx +1 RETURN LP_1b: lookup2 wavPindx,[0,sLb,15],tox wavPindx =wavPindx +1 RETURN LP_2uw: lookup2 wavPindx,[7,sRu,15],tox wavPindx =wavPindx +1 RETURN LP_2f: lookup2 wavPindx,[6,sRf,15],tox wavPindx =wavPindx +1 RETURN LP_2m: lookup2 wavPindx,[6,sRm,15],tox wavPindx =wavPindx +1 RETURN LP_2b: lookup2 wavPindx,[6,sRb,15],tox wavPindx =wavPindx +1 RETURN LP_3uw: lookup2 wavPindx,[3,sLu,15],tox wavPindx =wavPindx +1 RETURN LP_3f: lookup2 wavPindx,[2,sLf,15],tox wavPindx =wavPindx +1 RETURN LP_3m: lookup2 wavPindx,[2,sLm,15],tox wavPindx =wavPindx +1 RETURN LP_3b: lookup2 wavPindx,[2,sLb,15],tox wavPindx =wavPindx +1 RETURN LP_4uw: lookup2 wavPindx,[9,sRu,15],tox wavPindx =wavPindx +1 RETURN LP_4f: lookup2 wavPindx,[8,sRf,15],tox wavPindx =wavPindx +1 RETURN LP_4m: lookup2 wavPindx,[8,sRm,15],tox wavPindx =wavPindx +1 RETURN LP_4b: lookup2 wavPindx,[8,sRb,15],tox wavPindx =wavPindx +1 RETURN LP_5uw: lookup2 wavPindx,[5,sLu,15],tox wavPindx =wavPindx +1 RETURN LP_5f: lookup2 wavPindx,[4,sLf,15],tox wavPindx =wavPindx +1 RETURN LP_5m: lookup2 wavPindx,[4,sLm,15],tox wavPindx =wavPindx +1 RETURN LP_5b: lookup2 wavPindx,[4,sLb,15],tox wavPindx =wavPindx +1 RETURN LP_6uw: lookup2 wavPindx,[11,sRu,15],tox wavPindx =wavPindx +1 RETURN LP_6f: lookup2 wavPindx,[10,sRf,15],tox wavPindx =wavPindx +1 RETURN LP_6m: lookup2 wavPindx,[10,sRm,15],tox wavPindx =wavPindx +1 RETURN LP_6b: lookup2 wavPindx,[10,sRb,15],tox wavPindx =wavPindx +1 RETURN '=================== Direct control ===================================== ' turns on servo power and exits with servo power on ' $speed;$servo - high nibble and low nibble respectively ' 0,$speed;$servo,position ' 1,$XX,$servo,X ->P actual servo position ' 2,$XX,$servo,X ->S servo status, bit1-on bit0-done ' 3,X,X ->hiF,then loF two bytes all servo moved status ' 4,X,X ->hiR,then loR two bytes all servo on/off status ' 5,$XX;$servo,X stop pulsing servo CONTROL: serin2 cmndrx,baud,100,getcmnd,[action,tox] HIGH POWER servo =(action & $F) MIN 14 Sspeed =action >>4 branchL cmnd,[sendval,askval,askstatus,askallmoved,askallpulsing,setSoff] '-------------------------- askallpulsing: FOR servo =0 TO 12 STEP 3 GOSUB servmap _qp:serout2 txx\flowx,i96n,[Qstatus,0] '?status serin2 rxx,i96n,50,_qp,[Sstatus] FOR cpSaddress =0 TO 2 movedflags.LOWBIT(servo+cpSaddress) =Sstatus.LOWBIT(cpSaddress+4) NEXT NEXT GOTO sayflags '-------------------------- askallmoved: GOSUB checkdone GOTO sayflags '-------------------------- sendval: GOSUB seroutval GOTO getcmnd '-------------------------- askval: GOSUB getspos serout2 cmndtx,baud,[tox] GOTO getcmnd '-------------------------- askstatus: GOSUB servmap _qs: serout2 txx\flowx,i96n,[Qstatus,0] '?status serin2 rxx,i96n,50,_qs,[Sstatus] tox.BIT0 =Sstatus.LOWBIT(cpSaddress) tox.BIT1 =Sstatus.LOWBIT(cpSaddress<<4) serout2 cmndtx,baud,[tox] GOTO getcmnd '-------------------------- setSoff: GOSUB servoff GOTO getcmnd '-------------------------- sayflags: serout2 cmndtx,baud,[movedflags.HIGHBYTE,movedflags.LOWBYTE] GOTO getcmnd '------------------- End Program --------------------------------------