'----[Quad_LCD_Appmod.bs2]------------------------------------------------ '{$STAMP BS2} '{$PBASIC 2.5} ' ' File....... Quad_LCD_Appmod.bs2 ' Purpose.... Control the QuadCrawler using the Parallax LCD AppMod. ' Select gaits using the LCD AppMod's push buttons. ' Receive feedback through the LCD display ' Author..... CrustCrawler Inc. (Mike Gebhard) ' E-mail..... support@crustcrawler.com ' Started.... 19 November 2004 ' Updated.... 05 December 2004 ' ' Hardware ' (1) QuadCrawler Robotic Kit ' (1) Parallax BOE ' (1) Parallax Basic Stamp II ' (1) Parallax PSC ' (1) Parallax LCD AppMod ' ' This program is a combibation of QuadWalker_Engine_Select_Gait.bs2 ' and several modified routines from Parallax's LCD Terminal AppMod ' documentation. Please refer to these programs for specific program ' operation. ' '========================================================================= ' To use this program ' 1. Use leg centers found in HomeQuad.bs2 '========================================================================= ' 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 ' '========================================================================= ' LCD Appmod Push Buttons '========================================================================= ' Button A ' Hold down button A to enter selection mode. The robot will stop moving. ' The LCD will display the current gait on line 1 and a simple ' navigation element on line 2. ' ' |--------------| ' | Currunt Gait | ' | | ' |--------------| ' A B C D ' ' Button B and C ' Press buttons B and C to navigate through 15 gaits. ' ' Button D ' Press button D accept gait selection. '========================================================================= ' gaitCode and gait text '========================================================================= ' gaitCode ' $00 - Home ' $01 - Spin Left ' $02 - Spin Right ' ' gaitCode gaitCode ' $10 - Forward Fast $20 - Forward ' $11 - Fast Forward Left $21 - Forward Left ' $12 - Fast Forward Right $22 - Forward Right ' ' LED gaitCode gaitCode ' $30 - Backward $40 - Fast Backward ' $31 - Backward Left $41 - Fast Backward Left ' $32 - Backward Right $42 - Fast Backward Right ' '------------------------------------------------------------------------- ' -----[ LCD I/O Definitions ]-------------------------------------------- E PIN 1 ' LCD Enable (1 = enabled) RW PIN 2 ' Read/Write RS PIN 3 ' Reg Select (1 = char) LcdDirs VAR DIRB ' dirs for I/O redirection LcdBusOut VAR OUTB LcdBusIn VAR INB LcdCls CON $01 ' clear the LCD LcdHome CON $02 ' move cursor home LcdCrsrL CON $10 ' move cursor left LcdCrsrR CON $14 ' move cursor right LcdDispL CON $18 ' shift chars left LcdDispR CON $1C ' shift chars right LcdDDRam CON $80 ' Display Data RAM control LcdCGRam CON $40 ' Character Generator RAM LcdLine1 CON $80 ' DDRAM address of line 1 LcdLine2 CON $C0 ' DDRAM address of line 2 LcdScrollTm CON 250 ' LCD scroll timing (ms) ' -----[ PSC I/O Definitions ]------------------------------------------------- PSC PIN 15 ' PSC module Baud CON 33164 ' 2400 baud '----- [Adjustable Ramp Values] ----------------------------------------------- 'Off fast >>>>> 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 ' Allow leg servos time to ' complete movement 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 ' -----[ Variables ]------------------------------------------------------- idx VAR Nib ' loop counter char VAR Byte ' character sent to LCD scan VAR Byte ' loop counter gaitCode VAR Byte ' Walking mode temp VAR Byte ' Temp pointer ptrEEPROM VAR Word ' EEPROM Pointer buttons VAR Nib btnA VAR buttons.BIT0 ' left-most button btnB VAR buttons.BIT1 btnC VAR buttons.BIT2 btnD VAR buttons.BIT3 ' right-most '---- [Walking Variables] ------------------------------------------------ servoAddr VAR Byte ' Servo addresses Index VAR Word ' Gait select servoPosition VAR Word ' Servo Position Ramp VAR Byte ' Ramp used in SEROUT RightRamp VAR Byte ' Right side ramp values LeftRamp VAR Byte ' Left side ramp values '---- [EEPROM data] ------------------------------------------------------ Msg1 DATA "Crust",0 Msg2 DATA " Crawler",0 Msg3 DATA "Press ",0 Msg4 DATA " Reset ",0 Nav DATA "",0 Ir DATA " GO! ",0 GaitTable DATA $00,"Home ",0, $01,"L Spin ",0, $02,"R Spin ",0, $10,"FForward",0, $11,"FF Left ",0, $12,"FF Right",0, $20,"Forward ",0, $21,"F Left ",0, $22,"F Right ",0, $30,"Backup ",0, $31,"B Left ",0, $32,"B Right ",0, $40,"F Backup",0, $41,"FB Left ",0, $42,"FB Right",0 ' 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] ----------------- 'Set default mode to fast forward gaitCode = $10 'Initialize LCD GOSUB Initialize_LCD ' Initialize LCD GOSUB CrustCrawler_Logo ' Display CrustCrawler GOTO Initialize_Mode ' Wait for button press END '---- [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 = $08) THEN Ramp = LeftRamp ENDIF ENDIF GOSUB Write_PSC LOOP IF ptrEEPROM = adjust+36 THEN GOTO Home_Legs ENDIF GOTO Check_btnA_Press '----- [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 '-----[ Subroutines ]----------------------------------------------------- LCD_Get_Buttons: LcdDirs = %0000 ' make LCD bus inputs buttons = %1111 ' assume all pressed FOR scan = 1 TO 10 buttons = buttons & LcdBusIn ' make sure button held PAUSE 5 ' debounce 10 x 5 ms NEXT LcdDirs = %1111 ' return bus to outputs RETURN 'Insert LCD Navigation string on line 2 Write_LCD_Nav_String: char = LcdLine2+1 ' Line 2 + 1 character GOSUB Write_LCD_Command ptrEEPROM = Nav ' Write to LCD GOSUB LCD_Put_String RETURN 'Write "GO!" on line 2 Write_IR_Str: char = LcdLine2+0 GOSUB Write_LCD_Command ptrEEPROM = Ir GOSUB LCD_Put_String RETURN 'Find the gait code position in EEPROM 'Immediately following is the gait code text. Get_Gait_Code: FOR ptrEEPROM = GaitTable TO GaitTable+150 STEP 10 READ ptrEEPROM, temp IF gaitCode = temp THEN EXIT ENDIF NEXT RETURN 'Write EEPROM string to LCD display Update_LCD_Gait_Display: char = LcdLine1+0 ' LCD line 1 GOSUB Write_LCD_Command ptrEEPROM = ptrEEPROM+1 ' point to EEPROM text GOSUB LCD_Put_String ' Wite to LCD ptrEEPROM = ptrEEPROM-9 ' Reset pointer to gaitCode RETURN ' EEPROM address 'Check button A state Check_btnA_Press: GOSUB LCD_Get_Buttons ' Check button press IF btnA = 1 THEN ' Button A pressed? Initialize_Mode: char = LcdCls ' Clear LCD GOSUB Write_LCD_Command PAUSE 1000 ' Wait 1 sec GOSUB Write_LCD_Nav_String ' Write "" to LCD GOSUB Get_Gait_Code ' Get current gaitCode GOSUB Update_LCD_Gait_Display ' Display current gait text GOSUB Button_Select_Gait ' Use B and C buttons to ENDIF ' select a new gait GOTO Parse_GaitCode ' Parse new gaitCode '-------------------------------------------- 'Press Button A to enter gait selections 'Press buttons B and C to select gait 'Press button D to execute gait '-------------------------------------------- Button_Select_Gait: ' Do while btnD not DO WHILE btnD = 0 ' pressed GOSUB LCD_Get_Buttons ' Check button press FOR idx = 1 TO 2 ' Check btnB and btnC IF buttons.LOWBIT(idx) THEN IF idx = 1 THEN ' btnB pressed IF ptrEEPROM = GaitTable THEN ptrEEPROM = GaitTable + 140 ' Goto end of GaitTable ELSE ptrEEPROM = ptrEEPROM - 10 ' Next pointer location ENDIF ENDIF IF idx = 2 THEN ' btnC pressed IF ptrEEPROM = GaitTable + 140 THEN ptrEEPROM = GaitTable ELSE ptrEEPROM = ptrEEPROM + 10 ENDIF ENDIF ENDIF READ ptrEEPROM, gaitCode GOSUB Update_LCD_Gait_Display ' Display gait selected NEXT LOOP GOSUB Write_IR_Str ' Write IR string RETURN Write_LCD_Command: ' Low RS = LCD directive like LOW RS ' move the cursor to line 2 LcdBusOut = char.HIGHNIB ' Place HIGHNIB on LCD bus PULSOUT E, 3 ' Strobe the Enable line LcdBusOut = char.LOWNIB ' Place LOWNIB on LCD bus PULSOUT E, 3 ' Strobe the Enable line 'HIGH RS RETURN LCD_Put_String: DO READ ptrEEPROM, char ' Read data at EEPROM address IF (char = 0) THEN EXIT ' 0 = End of EEPROM String GOSUB Write_LCD_Char ' Write character to LCD ptrEEPROM = ptrEEPROM + 1 ' Increment EEPROM pointer LOOP RETURN Write_LCD_Char: ' HIGH RS = Write a character HIGH RS ' to the LCD display LcdBusOut = char.HIGHNIB PULSOUT E, 3 LcdBusOut = char.LOWNIB PULSOUT E, 3 'HIGH RS RETURN Initialize_LCD: NAP 5 ' let LCD self-initialize DIRL = %11111110 ' setup pins for LCD LcdBusOut = %0011 ' 8-bit mode PULSOUT E, 3 : PAUSE 5 PULSOUT E, 3 : PAUSE 0 PULSOUT E, 3 : PAUSE 0 LcdBusOut = %0010 ' 4-bit mode PULSOUT E, 3 char = %00101000 ' 2-line mode GOSUB Write_LCD_Command char = %00001100 ' on, no crsr, no blink GOSUB Write_LCD_Command char = %00000110 ' inc crsr, no disp shift char = LcdCls ' Clear screen GOSUB Write_LCD_Command RETURN CrustCrawler_Logo: ' Dispaly CC logo ptrEEPROM = Msg1 ' Point to message 1 GOSUB LCD_Put_String ' Write string char = LcdLine2+0 ' Go to Line 2 GOSUB Write_LCD_Command ' Write command ptrEEPROM = Msg2 ' Point to message 2 GOSUB LCD_Put_String ' Write string PAUSE 2000 ' wait 2 secs RETURN Home_Legs: char = LcdCls ' Clear screen GOSUB Write_LCD_Command ptrEEPROM = Msg3 ' Write "Press Reset" GOSUB LCD_Put_String char = LcdLine2+0 GOSUB Write_LCD_Command ptrEEPROM = Msg4 GOSUB LCD_Put_String END