'------- [ QuadWalker_LED_Button ] ----------------------- '{$STAMP BS2} '{$PBASIC 2.5} ' ' File....... QuadWalker_LED_Button.BS2 ' Purpose.... Use EEPROM tables, LED display, and two button interface ' to produce 15 different gaits. ' Author..... CustCrawler Inc. (Mike Gebhard) ' E-mail..... support@crustcrawler.com ' Started.... 16 April 2004 ' Updated.... 28 November 2004 ' Version.... 1.1 ' ' Hardware ' (1) QuadCrawler Kit ' '=====[ Updates ]========================================================= ' Updates: ' 1. Updated EEPROM tables. ' a. Aggresive stride ' b. Less servo strain ' 2. Programmatic horizontal leg adjustment ' 3. Updated comments '========================================================================= ' ' Button operation: ' Press either button during program execution to enter selection mode. ' Press the up and down buttons to select gaits. ' Press both buttons to accept selection ' ' Select zero to center and lower the QuadCrawler legs. ' Adjust the legs according to the quadCrawler assembly guide. ' Press the BOE reset button to restart. ' '========================================================================= ' Programmatic Adjustments ' Find the section of code below: '========================================================================= '-----[ Horizontal Leg Constants ]---------------------------------------- ' Center1 CON 750 ' Leg1 horizontal servo ' Center2 CON 750 ' Leg2 horizontal servo ' Center3 CON 750 ' Leg3 horizontal servo ' Center4 CON 750 ' Leg4 horizontal servo ' ' Replace these constants with the constants ' you found using the HomeQuad.bs2 program. ' ' Example Quad center constants yours will vary '-----[ Horizontal Leg Constants ]---------------------------------------- 'Center1 CON 725 'Center2 CON 800 'Center3 CON 785 'Center4 CON 750 ' '========================================================================= ' 7 Segement LED Display and gaitCode EEPROM data: '========================================================================= ' The 7 Segement LED data starts at the address labeled LED. ' gaitCode information starts at the address labeled Gait. ' '---- [7 Segment LED and gaitCode EEPROM Data Example] ----- ' Hex # 0 1 2 3 4 5 6 7 8 9 A B C D E 'LED DATA $7E,$18,$6D,$3D,$1B,$37,$77,$1C,$7F,$1F,$5F,$73,$66,$79,$67 'Gait DATA $00,$01,$02,$10,$11,$12,$20,$21,$22,$30,$31,$32,$40,$41,$42 ' ' btnIndx is a pointer to the LED EEPROM data. ' The two sets of EEPROM data form a parallel array of data. If ' btnIndx is pointing to EEPROM address 3, the segment display value ' is $3D. Making the segments variable equal to $3D causes a 3 to ' show on the LED display. Then, btnIndx + Gait is the gaitCode $10 or ' walk forward. ' ' The Get_GaitCode and Get_Segments sub routine are responsible for aligning ' the LED display to the gaitCode. ' '========================================================================= ' LED vs gaitCode Table '========================================================================= ' LED gaitCode ' (0) $00 - Home ' (1) $01 - Spin Left ' (2) $02 - Spin Right ' ' LED gaitCode LED gaitCode ' (3) $10 - Forward Fast (6) $20 - Forward ' (4) $11 - Fast Forward Left (7) $21 - Forward Left ' (5) $12 - Fast Forward Right (8) $22 - Forward Right ' ' LED gaitCode LED gaitCode ' (9) $30 - Backward (C) $40 - Fast Backward ' (A) $31 - Backward Left (d) $41 - Fast Backward Left ' (b) $32 - Backward Right (E) $42 - Fast Backward Right ' (F) Open '========================================================================= ' 7 Segement LED Display Table: '========================================================================= ' ' Segment map: .edc bafg HEX .edc bafg HEX ' (a) 0 %0111 1110 $7E 8 %0111 1111 $7F ' ----- 1 %0001 1000 $18 9 %0001 1111 $1F ' (f) | | (b) 2 %0110 1101 $6D A %0101 1111 $5F ' | (g) | 3 %0011 1101 $3D B %0111 0011 $73 ' ----- 4 %0001 1011 $1B C %0110 0110 $66 ' (e) | | (c) 5 %0011 0111 $37 D %0111 1001 $79 ' | | 6 %0111 0111 $77 E %0110 0111 $67 ' ----- 7 %0001 1100 $1C F %0100 0111 $47 - Open ' '========================================================================= ' Gait EEPROM Data: '========================================================================= ' Gait EEPROM data is stored in 3 byte sections; Servo address, ' LOWBYTE of servo position, and HIGHBYTE of servo position. ' Data is READ from EEPROM 3 bytes at a time then sent to the PSC. ' ' How does it work? ' Variables used; ptrEEPROM, servoAddr, rightRamp, and leftRamp ' ' ptrEEPROM is a pointer to EEPROM addresses. Assigning ' ptrEEPROM to the label "Forward" (ptrEEPROM = Forawrd) places ' the pointer at the starting EEPROM address for forward motion. ' ' rightRamp and leftRamp control servo speeds (ramp) on ' the left or right side of the QuadCrawler. ' Slowing the servos on one side of the robot causes ' the QuadCralwer to make gradual turns. ' '========================================================================= ' Sub Routines '========================================================================= ' Walking_Engine: ' The "Walking_Engine" sub routine READs the first of 3 bytes sections ' stored in the address "ptrEEPROM" and assigns servo speed (ramp). ' ' If Walking_Engine encounters an $FF it has reached the ' end of the Forward EEPROM data. ptrEEPROM is reset ' and the process repeats. ' ' If Walking_Engine READS an odd servo address a very ' fast ramp value is used to lift the leg quickly. ' ' If the servo address is even Walking_Engine assigns ramp ' values depending on leg location, left or right ' side of the robot. ' ' Walking_Engine then passes control and the ptrEEPROM variable ' to the Write_PSC sub routine. ' ' Write_PSC: ' The "Write_PSC" sub uses "ptrEEPROM" to READ the next 2 ' bytes of data stored in EEPROM. servo position LOWBYTE ' servo position HIGHBYTE. This data is written to the PSC ' with a SEROUT command. ' ' ptrEEPROM is updated (+3) to point to the next 3 byte chunck ' of EEPROM and control is returned to Walking_Engine. ' ' Adjusting servo rotation speed: ' Find the [Adjustable Ramp Value section] in the code below. ' Ramp is a prameter passed to the PSC in the SEROUT command. ' See your PSC instruction manual for more information. ' Ramp is the speed/time it takes for a servo to move ' to a new position. The larger the ramp the slower the ' servo will rotate. Ramp can range from 0 to 63 ($0 to $3F) ' ' Feel free to adjust the these values to increase/decrease ' servo speeds. You'll need to experiment to find the values ' that work best for your robot and application. ' '========================================================================= ' Walk Forward Diagram '========================================================================= ' ' **Right Side** | ' | ' F \ / | / \ ' O _\_ _/_ | _/_ _\_ ' R / 1 \____/ 2 | | / 1 \____/ 2 | ' W | | | | | ' A | ____ | | | ____ | ' R \_3_/ \_4_| | \_3_/ \_4_| ' D \ / | / \ ' \ / | / \ ' '------------------------------------------------------------------------- ' -----[ I/O Definitions ]------------------------------------------------ PSC PIN 15 ' PSC module ModeBtn PIN 8 ' select robot mode up StartBtn PIN 12 ' select robot mode down segments VAR OUTL ' output on pins 0 - 7 Baud CON 33164 ' 2400 baud '---- [Button Variables]-------------------------------------------------- btns VAR Nib ' button holder btn1 VAR btns.BIT0 ' debounced button value btn2 VAR btns.BIT1 ' deboucned button value idx VAR Nib ' digit index btnIndx VAR Byte ' current digit to display '---- [Button states] ---------------------------------------------------- Pressed CON 1 ' Button states NotPressed CON 0 '---- [Walking Variables] ------------------------------------------------ servoAddr VAR Byte ' Servo addresses gaitCode VAR Byte ' Current mode ramp VAR Byte ' Ramp used in SEROUT rightRamp VAR Byte ' Right side ramp values leftRamp VAR Byte ' Left side ramp values ptrEEPROM VAR Word ' Gait select servoPosition VAR Word ' Servo Position '----- [Adjustable Ramp Values] ------------------------------------------ '|----------------------| '|Off | fast to slow | '-----|-----------------| '| 0 | $1 -- $3F(63) | '|----------------------| LiftRamp CON $1 ' Vertical servo ramp VeryFast CON $8 ' Walk very fast Fast CON $A ' Walk fast '-----[ Horizontal Leg Constants ]---------------------------------------- 'Servo center Center1 CON 750 ' Leg1 horizontal servo Center2 CON 750 ' Leg2 horizontal servo Center3 CON 750 ' Leg3 horizontal servo Center4 CON 750 ' Leg4 horizontal servo '-----[ Stride Calculations ]--------------------------------------------- Stride CON 150 ' Stride units delay CON Stride/5 ' Delay as a ratio of stride Leg1Center CON Center1 Leg1Forward CON Center1+Stride ' Stride + Leg1 Center Leg1Back CON Center1-Stride Leg2Center CON Center2 Leg2Forward CON Center2+Stride Leg2Back CON Center2-Stride Leg3Center CON Center3 Leg3Forward CON Center3-Stride Leg3Back CON Center3+Stride Leg4Center CON Center4 Leg4Forward CON Center4-Stride Leg4Back CON Center4+Stride '----- [Adjustable vertical servo positions] ----------------------------- RaiseRight CON 300 ' Raised and lowered LowerRight CON 1200 ' vertical servo values RaiseLeft CON 1200 LowerLeft CON 300 '---- [7 Segment LED and gaitCode EEPROM Data] ----- ' Hex # 0 1 2 3 4 5 6 7 8 9 A B C D E LED DATA $7E,$18,$6D,$3D,$1B,$37,$77,$1C,$7F,$1F,$5F,$73,$66,$79,$67 Gait DATA $00,$01,$02,$10,$11,$12,$20,$21,$22,$30,$31,$32,$40,$41,$42 '---- [Servo Address and Position EEPROM data] ----------------- ' Walk Forward Forward DATA $01,Word RaiseRight, $07,Word RaiseLeft, $00,Word Leg1Forward, $02,Word Leg2Back, $04,Word Leg3Center, $06,Word Leg4Center, $01,Word LowerRight, $07,Word LowerLeft, $03,Word RaiseRight, $05,Word RaiseLeft, $00,Word Leg1Center, $02,Word Leg2Center, $04,Word Leg3Forward, $06,Word Leg4Back, $03,Word LowerRight, $05,Word LowerLeft, $FF ' end of forward ' Walk Backward Back DATA $01,Word RaiseRight, $07,Word RaiseLeft, $00,Word Leg1Center, $02,Word Leg2Center, $04,Word Leg3Forward, $06,Word Leg4Back, $01,Word LowerRight, $07,Word LowerLeft, $03,Word RaiseRight, $05,Word RaiseLeft, $00,Word Leg1Forward, $02,Word Leg2Back, $04,Word Leg3Center, $06,Word Leg4Center, $03,Word LowerRight, $05,Word LowerLeft, $FF 'end of back ' Left Turn EEPROM values LTurn DATA $01,Word RaiseRight, $07,Word RaiseLeft, $00,Word Leg1Forward+Stride, $06,Word Leg4Back+Stride, $04,Word Leg3Forward, $02,Word Leg2Back, $01,Word LowerRight, $07,Word LowerLeft, $03,Word RaiseRight, $05,Word RaiseLeft, $00,Word Leg1Center+Stride, $06,Word Leg4Center+Stride, $04,Word Leg3Forward, $02,Word Leg2Back, $03,Word LowerRight, $05,Word LowerLeft, $FF ' end of left turn ' Right turn EEPROM Values RTurn DATA $01,Word RaiseRight, $07,Word RaiseLeft, $00,Word Leg1Forward, $02,Word Leg2Center-Stride, $04,Word Leg3Center-Stride, $06,Word Leg4Back, $01,Word LowerRight, $07,Word LowerLeft, $03,Word RaiseRight, $05,Word RaiseLeft, $00,Word Leg1Forward, $02,Word Leg2Back-Stride, $04,Word Leg3Forward-Stride, $06,Word Leg4Back, $03,Word LowerRight, $05,Word LowerLeft, $FF ' end of right turn ' Raise Center Lower Adjust DATA $01,Word RaiseRight, $00,Word Center1, $01,Word LowerRight DATA $03,Word RaiseRight, $02,Word Center2, $03,Word LowerRight DATA $05,Word RaiseLeft, $04,Word Center3, $05,Word LowerLeft DATA $07,Word RaiseLeft, $06,Word Center4, $07,Word LowerLeft, $FF ' end of adjust legs '---- [End EEPROM Data] ----------------- '---- [Initialize] ----------------- DIRL = %01111111 ' P0 - P6 are outputs gaitCode = $10 ' Walk straight forward fast btnIndx = $03 ' Init button index segments = $3D ' Display a 3 on LED GOTO Get_GaitCode ' Select a gait code '---- [Gait Selection] --------------------------------------------------- Parse_GaitCode: ' Assign gait parameters SELECT gaitCode ' depending on gaitCode CASE $00 ' Adjust Legs if 0 is selected ptrEEPROM = Adjust rightRamp = Fast leftRamp = Fast GOTO Walking_Engine CASE $01 ptrEEPROM = LTurn ' Spin left rightRamp = VeryFast ' Assign ramp to leftRamp = VeryFast ' left and right sides GOTO Walking_Engine CASE $02 ptrEEPROM = RTurn ' Spin right rightRamp = VeryFast leftRamp = VeryFast GOTO Walking_Engine ENDSELECT ' HIGHNIB of gaitCode = direction ' Assign EEPROM pointer IF gaitCode.HIGHNIB <= $2 THEN ptrEEPROM = Forward ELSE ptrEEPROM = Back ENDIF Setup_Ramp_Vars: ' HIGHNIB of gaitCode = Speed ' Assign ramp (servo speed) IF (gaitCode.HIGHNIB = $1) OR (gaitCode.HIGHNIB = $4) THEN ramp = VeryFast ELSE ramp = Fast ENDIF ' Check LowNib to determine ' left and right leg ramps ' for gradual turns ' 0 = straight ' 1 = slow left side ' 2 = slow right side SELECT gaitCode.LOWNIB CASE $0 rightRamp = ramp leftRamp = rightRamp CASE $1 rightRamp = ramp leftRamp = (ramp+$2) CASE $2 leftRamp = ramp rightRamp = (ramp+$2) ENDSELECT '-----[ Main Walking Routine ]-------------------------------------------- Walking_Engine: READ ptrEEPROM,ServoAddr ' Read servo address DO WHILE servoAddr <> $FF ' $FF s the end of EEPROM data IF (servoAddr // 2) = 1 THEN ' O vdd servo address? ramp = LiftRamp ' Assign fast ramp ELSE IF (servoAddr = $00) OR (servoAddr = $02) THEN ramp = rightRamp ENDIF IF(servoAddr = $04) OR (servoAddr = $06) THEN ramp = leftRamp ENDIF ENDIF GOSUB Write_PSC LOOP ' If adjustment mode is selected -> END IF ptrEEPROM = Adjust + 36 THEN ' Adjust legs selected END ENDIF 'Check for a button press GOSUB Read_Buttons IF (btns.BIT0 = Pressed) OR (btns.BIT1 = Pressed) THEN GOTO Get_GaitCode ' Use button to select gait code ELSE ptrEEPROM = ptrEEPROM - 48 ' Reset EEPROM pointer GOTO Walking_Engine ' Continue walking ENDIF '----- [Serial Out EEPROM values to PSC] --------------------------------- Write_PSC: READ ptrEEPROM+1, servoPosition.LOWBYTE, ' Read servo address servoPosition.HIGHBYTE SEROUT PSC,Baud,["!SC",ServoAddr, ' Write servo address Ramp,servoPosition.LOWBYTE, ' and servo position servoPosition.HIGHBYTE, CR] ' to the PSC ptrEEPROM = ptrEEPROM + 3 ' Next 3 bytes of EEPROM READ ptrEEPROM,servoAddr ' Read next servo address PAUSE delay RETURN '---- [Button Code Section] ------ Get_GaitCode: DO WHILE (btns.BIT0 = NotPressed) OR (btns.BIT1 = NotPressed) IF (btns > %00) AND (btns < %11) THEN ' one or the other pressed? btnIndx = btnIndx + btn1 // 15 ' increment if Button1 = 1 btnIndx = btnIndx + (14 * btn2) // 15 ' decrement if Button2 = 1 READ btnIndx, segments ' Update LED display READ gait + btnIndx, gaitCode ' Update Selected gait Mode PAUSE 250 ' 1/4 sec between changes ENDIF GOSUB Read_Buttons LOOP GOTO Parse_GaitCode '----- [Read Buttons] --------------------------------------------------- Read_Buttons: btns = %0011 ' assume both pressed FOR idx = 1 TO 5 btns.BIT0 = btns.BIT0 & ModeBtn ' scan mode button btns.BIT1 = btns.BIT1 & StartBtn ' scan start/stop button PAUSE 5 ' debounce delay NEXT RETURN