'------- [ Quad_LED_Button_IR.bs2 ] ----------------------- '{$STAMP BS2} '{$PBASIC 2.5} ' ' File....... Quad_LED_Button_IR.BS2 ' Purpose.... EEPROM tables, LED display, two button interface ' and SHARP GP2D12 infrared distance detector ' 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 ' '=====[ Updates ]========================================================= ' Updates: ' 1. Updated EEPROM tables. ' a. Aggresive stride ' b. Less servo strain ' 2. Programmatic horizontal leg adjustment ' 3. Updated comments ' '========================================================================= ' 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 ' '=====[ 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. ' '=====[ ADC0831 and GP2D12 Resources ]==================================== ' Please take the time to look through the ADC0831 datasheet ' http://www.crustcrawler.com/downloads/datasheet/dsadc0831.pdf ' ' SHARP GP2D12 datasheet ' http://www.crustcrawler.com/downloads/datasheet/gp2d12.pdf ' ' Voltage to distance calculation using the ' ADC0831 8 bit A/D and Sharp IR. ' http://www.acroname.com/robotics/info/articles/irlinear/irlinear.html ' This resource contains everythnig you need to understand ' the distance calculation in this program. ' ' October's issue of Nuts and Volts "Stamp Applications" ' "Measuring Up - Up to 80 cm, That is" ' by Jon Williams is another great resource. ' '========================================================================= ' ADC0831 to BOE and GP2D12 connections '========================================================================= ' Vdd(+5V) ' ____________ | ' |1 | | 3.3-10 micro F ' pin 9 -|CS A Vcc |-|------||-------| ' | D | Vdd(+5V) | ' GP2D12 -|Vin+ C CLK |- pin 11 | | ' | 0 | / Vss ' |---|Vin= 8 Dout|- pin 10 \ ' | | 3 | / 1k resistor ' |---|GND 1 Vref|-----------| ' | |____________| / ' | \ 2k resistor (ref) ' Vss / 2.74 V ' | ' Vss '========================================================================= ' GP2D12 to BOE and ADC0831 '========================================================================= ' ___________________ ' | GP2D12 | ' | Front | ' |_Vo_____GND____Vcc_| ' | | | ' | 33mf | | ' |--||--| | ' Vin+ Vss Vdd ' ADC0831 BOE BOE ' '========================================================================= ' 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 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 is responsible for aligning ' the LED display to the gaitCode. ' '========================================================================= ' 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 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. ' ' Adjustable values: ' [Adjustable Ramp Value section] ' Ramp is a prameter passed to the PSC with 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. Small values, 1-7, ' move fast while larger values, 8-15($F), move ' slow. ' ' Adjust the these values to increase/decrease ' servo speeds. Experiment to find the values ' that work best for your robot. ' '========================================================================= ' Walk Forward Diagram '========================================================================= ' ' **Right Side** | ' | ' F \ / | / \ ' O _\_ _/_ | _/_ _\_ ' R / 1 \____/ 2 | | / 1 \____/ 2 | ' W | | | | | ' A | ____ | | | ____ | ' R \_3_/ \_4_| | \_3_/ \_4_| ' D \ / | / \ ' \ / | / \ ' ' ' 7 Segement LED Display: ' 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 ' '------------------------------------------------------------------------- ' -----[ 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 '-----[ ADC0831 I/O ] ---------------------------------------------------- FrontIR CON 9 ' Front ADC0831 chip Select DIO_10 CON 10 ' Data I/O pin 10 CLK CON 11 ' Clock '-----[ Transform Function Constants ]------------------------------------ Slope CON 3200 y_Intercept CON 3 k CON 4 '---- [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] ------------------------------------------------ counter VAR Nib servoAddr VAR Byte ' Servo addresses gaitCode VAR Byte ' Current mode tempCode VAR Byte ' Temp gaitcode variable ramp VAR Byte ' Ramp used in SEROUT rightRamp VAR Byte ' Right side ramp values leftRamp VAR Byte ' Left side ramp values distTemp VAR Byte ' ADC0831 Reading distance VAR Byte ' Distance calculation 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 HIGH FrontIR ' ADC0831 off GOTO Select_GaitCode ' Select a gait code '---- [Gait Selection] --------------------------------------------------- Parse_GaitCode: ' Assign gait parameters SELECT gaitCode CASE $00 ' Adjust Legs 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 DO WHILE servoAddr <> $FF IF (servoAddr // 2) = 1 THEN ramp = LiftRamp 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 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 Select_GaitCode ' Select gait code ELSE GOTO Get_IR ENDIF '---- [Sub routines] ----------------------------------------------------- '----- [Serial Out EEPROM values to PSC] --------------------------------- Write_PSC: READ ptrEEPROM+1, servoPosition.LOWBYTE, servoPosition.HIGHBYTE SEROUT PSC,Baud,["!SC",ServoAddr, Ramp,servoPosition.LOWBYTE, servoPosition.HIGHBYTE, CR] ptrEEPROM = ptrEEPROM + 3 READ ptrEEPROM,servoAddr PAUSE delay RETURN '---- [Button Code Section] ---------------------------------------------- Select_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 '------[ IR Routines ]---------------------------------------------------- Get_IR: distTemp = 0 ' Initialize distTemp distance = 0 ' Initialize distance FOR counter = 1 TO 3 ' Average reading GOSUB ADC0831_Reading ' Get 8 bit data from ADC. distance = distance + (distTemp/3) PAUSE 50 NEXT GOSUB ADC0831_Distance_Calc ' Get ADC0831 distance SELECT distance ' Adjustable Distance decisions CASE < 15 ' Less than 15 cm tempCode = $40 ' Backup CASE < 25 ' Less than 25cm tempCode = $01 ' Spin left CASE < 50 ' Less than 50cm tempCode = $12 ' Gradual turn right CASE ELSE ' Else walk forward tempCode = $10 ENDSELECT IF tempCode = gaitCode THEN ' If no change in gait code ptrEEPROM = ptrEEPROM - 48 ' Reset EEPROM pointer GOTO Walking_Engine ' Continue same gait ELSE gaitCode = tempCode ' Get new gaitCode GOSUB Get_Segments GOTO Parse_GaitCode ' Set walking parameters ENDIF 'Get ADC0831 8-bit data ADC0831_Reading: LOW FrontIR ' Activate ADC. SHIFTIN DIO_10,CLK, ' Get data bits from. MSBPOST,[distTemp\9] ' ADC0831 HIGH FrontIR ' Deactivate ADC. RETURN 'Linearizing function to calculate 'GP2D12 distance reading ADC0831_Distance_Calc: distance = (Slope /(distance + y_Intercept)) - k RETURN 'Update LED display Get_Segments: btnIndx = Gait READ btnIndx, tempCode DO WHILE tempCode <> gaitCode btnIndx = btnIndx + 1 READ btnIndx, tempCode LOOP READ btnIndx-Gait, segments RETURN