'********************************************************** '* main 0 program for walker - 2000 - benz * '* posted on the web 5/00. Please note that this code * '* is for example purposes only and while the routines * '* for walking are functional in themselves, the program * '* itself will not work in this form. * '* included files are: * '* slowmo15 * '********************************************************** '************** declare type & inclusions '{$STAMP BS2SX, slomo15} '************** assign I/O *********** dirs = %0111111111111111 'I/O pin 15 = input, rest = outputs sscpin con 11 'pin for SSCII pie con 12 'piezo out blueLED con 13 'pin for LED redLED con 14 'pin for LED sonyIn con 15 'IR remote pin '************** constants ************ snddelay con 125 'timout for valid IR code IRbaud con 1021 'IR remote serial rate (2400) serBaud con 16624 'SSC II serial speed limit1 con 10 'leg movement limiter limit2 con 235 'leg movement limiter lift con 130 '130 - can be up to 150 stride con 88 'stride length - must be even number Hstride con stride/2 'half stride goodDelay con 124 '124 delay between motions roundLean con 30 'for leaning roundLean2 con 30 'ditto '************** scratchpad definitions ***************** '0 to 7 lastser 0 thru 7 '8 to 15 serdif 0 thru 7 '61 data for where to go in subprograms '62 initialization flag for startup/reset serdifMod con 8 '************** variables ************ i var byte 'scratch byte x var byte 'for-next scratch variable IRdata var byte 'IR data in lastIR var byte 'last IR value to prevent repeating serpos var byte(8) 'target servo position serDoneFlag var bit z var nib lastSer var byte 'current servo position serDif var byte 'difference between last & target positions y var nib '**for next - for step loops left/right isMoving var bit theLeg var nib 'leg being acted upon stopFlag var bit 'flag for stopping an action set in IR delay var byte 'delay between movements speed var byte 'speed setting '************** jump from subProgram check *********** get 62,z if z <> 0 then start 'z = 0 on startup/reset '************** initialization ******* speed = 22 gosub zoomtwo 'startup noise & lights gosub homepos 'startup position for x = 0 to 7 'set up lastSer in scratchpad RAM put x, serpos(x) next gosub sout 'move servos isMoving = 0 delay = goodDelay put 62,1 'scratchpad flag for startup/reset '************** main loop & functions start: low redLED stopFlag = 0 gosub checkIR if IRdata = lastIR then IRsame lastIR = IRdata 'if IRdata <> lastIR isMoving = 0 IRsame: if IRdata = 3 then lookRight 'rewind button if IRdata = 4 then lookLeft 'fast-forward button if IRdata = 6 then jumpSlo 'button 1 if IRdata = 7 then jumpSlo 'button 2 if IRdata = 8 then jumpSlo 'button 3 if IRdata = 9 then jumpSlo 'button 4 if IRdata = 10 then jumpSlo 'button 5 if IRdata = 11 then jumpSlo 'button 6 if IRdata = 12 then jumpSlo 'button 7 if IRdata = 13 then jumpSlo 'button 8 if IRdata = 14 then jumpSlo 'button 9 if IRdata = 16 then goForward 'channel up button if IRdata = 17 then goBackward 'channel down button if IRdata = 18 then turnRight 'volume down button if IRdata = 19 then turnLeft 'volume up button if IRdata = 20 then homeposgo 'mute button goto start jumpSlo: put 61,IRdata 'put in target routine run 1 'make jump - bye! goto start homeposGo gosub homepos gosub sout goto start '************** check IR ************* checkIR: IRdata = 255 serin sonyIn,IRbaud,snddelay,noData,[IRdata] pause 10 lookdown IRdata,[88,89,90,91,92,106,128,129,130,131,132,133,134,135,136,137,144,145,146,147,148],IRdata return 'stop,pause,play,rewnd,ffwd,tv/vcr,1,2,3,4,5,6,7,8,9,0,ch up,ch dn,volUp,volDn,mute noData: return '************** servo out **************** sout: if stopflag = 1 then aftermake stopFlag = 0 afterMake: 'check to make sure gosub checkIR 'command hasn't changed if IRdata = lastIR then goOn: 'and if it has then stopFlag = 1 'raise the stopFlag high redLED '********* optional redLight goOn for x = 0 to 7 serpos(x) = serpos(x) MIN limit1 'make sure servos don't overextend serpos(x) = serpos(x) MAX limit2 serout sscpin,serBaud,[255,x,serpos(x)] ' debug " ",dec serpos(x) '********** optional debug next ' debug 13 '**********carriage return to debug pause delay return '************** movement main routines *********** LookLeft: serpos(0) = serpos(0) + 3 serpos(2) = serpos(2) + 3 serpos(4) = serpos(4) + 3 serpos(6) = serpos(6) + 3 gosub sout if IRdata = lastIR then lookLeft 'uses data from last sout goto start lookRight: serpos(0) = serpos(0) - 3 serpos(2) = serpos(2) - 3 serpos(4) = serpos(4) - 3 serpos(6) = serpos(6) - 3 gosub sout if IRdata = lastIR then lookRight 'uses data from last sout goto start goForward: if isMoving = 1 then doGoAlready isMoving = 1 'set premove position 'FRONTS serpos(0) = 127 '77 '127 - 50 right front H (+ forward) serpos(4) = 107 '157 '127 + 30 left front H (- forward) 'BACKS serpos(2) = 117 '87 '127 - 40 right rear H (+ forward) serpos(6) = 139 '169 '129 + 40 left rear H (- forward) gosub sout doGoAlready: serpos(3) = serpos(3) - lift serpos(2) = serpos(2) + stride gosub doHstride gosub sout serpos(3) = serpos(3) + lift serpos(2) = serpos(2) + stride gosub doHstride gosub sout serpos(1) = serpos(1) + lift serpos(0) = serpos(0) + stride gosub doHstride gosub sout serpos(1) = serpos(1) - lift serpos(0) = serpos(0) + stride gosub doHstride gosub sout serpos(7) = serpos(7) + lift serpos(6) = serpos(6) - stride gosub doHstride gosub sout serpos(7) = serpos(7) - lift serpos(6) = serpos(6) - stride gosub doHstride gosub sout serpos(5) = serpos(5) - lift serpos(4) = serpos(4) - stride gosub doHstride gosub sout serpos(5) = serpos(5) + lift serpos(4) = serpos(4) - stride gosub doHstride gosub sout if stopFlag = 0 then doGoAlready goto start doHstride: serpos(0) = serpos(0) - (Hstride/2) serpos(2) = serpos(2) - (Hstride/2) serpos(4) = serpos(4) + (Hstride/2) serpos(6) = serpos(6) + (Hstride/2) return goBackward: if isMoving = 1 then doGoBackward isMoving = 1 'set premove position 'FRONTS serpos(0) = 137 '127 '127 - 50 right front H (+ forward) serpos(4) = 117 '107 '127 + 30 left front H (- forward) 'BACKS serpos(2) = 107 '117 '127 - 40 right rear H (+ forward) serpos(6) = 129 '139 '129 + 40 left rear H (- forward) gosub sout doGoBackward: serpos(5) = serpos(5) - lift serpos(4) = serpos(4) + stride gosub doBstride gosub sout serpos(5) = serpos(5) + lift serpos(4) = serpos(4) + stride gosub doBstride gosub sout serpos(7) = serpos(7) + lift serpos(6) = serpos(6) + stride gosub doBstride gosub sout serpos(7) = serpos(7) - lift serpos(6) = serpos(6) + stride gosub doBstride gosub sout serpos(1) = serpos(1) + lift serpos(0) = serpos(0) - stride gosub doBstride gosub sout serpos(1) = serpos(1) - lift serpos(0) = serpos(0) - stride gosub doBstride gosub sout serpos(3) = serpos(3) - lift serpos(2) = serpos(2) - stride gosub doBstride gosub sout serpos(3) = serpos(3) + lift serpos(2) = serpos(2) - stride gosub doBstride gosub sout goto start doBstride: serpos(0) = serpos(0) + (Hstride/2) serpos(2) = serpos(2) + (Hstride/2) serpos(4) = serpos(4) - (Hstride/2) serpos(6) = serpos(6) - (Hstride/2) return turnLeft: if isMoving = 1 then goTurnLeft isMoving = 1 gosub preturn 'set premove position gosub sout goTurnLeft: for y = 0 to 3 'forNext loop does it! lookup y,[2,0,4,6],theLeg lookup y,[95,160,95,160],i serpos(theLeg+1) = serpos(theLeg+1) + i serpos(theLeg) = serpos(theLeg) + stride gosub sout serpos(0) = serpos(0) - Hstride serpos(2) = serpos(2) - Hstride serpos(4) = serpos(4) - Hstride serpos(6) = serpos(6) - Hstride serpos(theLeg) = serpos(theLeg) + stride serpos(theLeg+1) = serpos(theLeg+1) - i gosub sout if stopFlag = 1 then start 'new command coming now next if IRdata = lastIR then goTurnLeft 'uses data from last sout goto start turnRight: if isMoving = 1 then goTurnRight isMoving = 1 gosub preturn 'set premove position gosub sout goTurnRight: for y = 0 to 3 'forNext loop does it! lookup y,[6,4,0,2],theLeg lookup y,[160,95,160,95],i serpos(theLeg+1) = serpos(theLeg+1) + i serpos(theLeg) = serpos(theLeg) - stride gosub sout serpos(0) = serpos(0) + Hstride serpos(2) = serpos(2) + Hstride serpos(4) = serpos(4) + Hstride serpos(6) = serpos(6) + Hstride serpos(theLeg) = serpos(theLeg) - stride serpos(theLeg+1) = serpos(theLeg+1) - i gosub sout if stopFlag = 1 then start 'new command coming now next if IRdata = lastIR then goTurnRight 'uses data from last sout goto start preturn: serpos(0) = 57 '97 '127 right front H (+ forward) serpos(2) = 67 '107 '127 right rear H (+ forward) serpos(4) = 157 '157 '127 left front H (- forward) serpos(6) = 149 '149 '127 left rear H (- forward) return '*************** load home position ************** homepos: for x = 0 to 7 lookup x,[127,55,127,225,127,200,129,65],lastSer serpos(x) = lastSer next ' 0 = 127 nom:127 right front horizontal (+ forward) ' 1 = 55 nom:0 right front vertical (+ lift) ' 2 = 127 nom:127 right rear horizontal (+ forward) ' 3 = 225 nom:255 right rear vertical (- lift) ' 4 = 127 nom:127 left front horizontal (- forward) ' 5 = 200 nom:255 left front vertical (- lift) ' 6 = 129 nom:127 left rear horizontal (- forward) ' 7 = 65 nom:0 left rear vertical (+ lift) return '************* sound routines ************ zoomTwo: 'zoom two i = 0 freqout pie,225,320,317 for x = 1 to 29 i = i + 8 high blueLED freqout pie,3,(800+i),(560-x) low blueLED freqout pie,(0+i),104+(i*2) next high redLED pause 125 freqout pie,200,600,597 freqout pie,125,320,317 low redLED return