📄 quad_led_button_rc.bs2
字号:
idx VAR Nib ' digit index
btnIndx VAR Nib ' 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
stickXPos VAR Word ' Left/Right (X) joystick posn
stickYPos VAR Word ' Up/Down(Y) joystick 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
IF gaitCode = $03 THEN
GOTO Get_Stick
ENDIF
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 Get_GaitCode ' Select gait code
ELSE
GOTO Get_Stick
ENDIF
Continue:
ptrEEPROM = ptrEEPROM - 48 ' Reset EEPROM pointer
GOTO Walking_Engine ' Continue walking
'----- [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] ------
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
' Convert Stick positions to gaitcode
Get_Stick:
PULSIN StickXPin, 1, stickXPos ' Read joystick positions
PULSIN StickYPin, 1, stickYPos ' from transmitter P13, P12
IF (stickXPos & stickYPos) < 500 THEN ' Is the RC Tx off
GOTO Continue ' Continue walking
ELSE
SELECT stickYPos
CASE 580 TO 650
gaitCode = $10
CASE 651 TO 730
gaitCode = $20
CASE 731 TO 800
gaitCode = $00
CASE 801 TO 870
gaitCode = $30
CASE 871 TO 960
gaitCode = $40
ENDSELECT
SELECT stickXPos
CASE 500 TO 700
gaitCode = gaitCode | $01
CASE 701 TO 820
gaitCode = gaitCode | $00
IF gaitCode = 0 THEN
gaitCode = $03
ENDIF
CASE 821 TO 999
gaitCode = gaitCode | $02
ENDSELECT
GOTO Parse_GaitCode
ENDIF⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -