quad_lcd_appmod.bs2

该程序是一个描述四角铁甲虫机器人调试过程的程序

'---- [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 <B C> on line 2
  Write_LCD_Nav_String:
    char = LcdLine2+1                           ' Line 2 + 1 character
    GOSUB Write_LCD_Command
    ptrEEPROM = Nav                             ' Write <B C> 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 "<B C>" 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