Hi,
I have an AH3-R. Attached is both the SSC-32 and the Bot Board II. I also have a Lynxmotion controller.
I connected the SSc and Bot Board II as instructed in the assembly guide link below.
lynxmotion.com/images/html/build99c.htm
Using the BasicMicro IDE i uploaded the program below.
;
; Use Basic Micro ATOM IDE 02.2.1.1
; *** not suitable for IDE 05.3.0.0 (there’s a specific version for this IDE)
; Use Basic Atom 28
; *** not compatible with Basic Atom Pro (new version for BA Pro soon)
;************************************************
;*** Basic Atom with SSC-32 and PS2 DualShock ***
;----------------- 1.41 3DOF-A ------------------
;--------------- PS2 Controller -----------------
;-------------- Round Body (H3-R) ---------------
;********** Bot Board Buzzer support ************
;***** Little Gripper / Pan & Tilt support ******
;**************** Deck support ******************
;************************************************
;** Programmer: Laurent Gay, [email protected] **
;************************************************
;
; let’s say that the 2 switches holes of the H3/H3R is the ‘rear’ of the robot
;
; *** using 3 ‘right’ legs and 3 ‘left’ legs ***
; put the SSC-32 card with the PC serial port looking to ‘Front’ (opposite to H3R switches)
;
; 3 right legs connections :
; Rear Right leg : Hip Horizontal : pin 0, Hip Vertical : pin 1, Knee : pin 2
; Middle Right leg : Hip Horizontal : pin 4, Hip Vertical : pin 5, Knee : pin 6
; Front Right leg : Hip Horizontal : pin 8, Hip Vertical : pin 9, Knee : pin 10
;
; 3 left legs connections :
; Rear Left leg : Hip Horizontal : pin 16, Hip Vertical : pin 17, Knee : pin 18
; Middle Left leg : Hip Horizontal : pin 20, Hip Vertical : pin 21, Knee : pin 22
; Front Left leg : Hip Horizontal : pin 24, Hip Vertical : pin 25, Knee : pin 26
;
; Default mode :
; Move the robot (2 axis translation) with the right joystick
; Turn with the left joystick X axis
; move Up/Down body with the left joystick Y axis
; lock/unlock the height with the left joystick push button (L3)
;
; Body move mode :
; Push ‘select’ button to activate this mode (push ‘select’ again to return to default mode)
; move the body horizontally (2 axis translation) with the right joystick
; the left joystick push button (L3) has a new function now, it selects between 4 control modes for the left joystick:
; 1)- (by default) move the body vertically + rotate around the vertical axis
; 2)- (push L3 to activate) move the body vertically + roll
; 3)- (push L3 again to activate) pitch + rotate around the vertical axis
; 4)- (push L3 again to activate) pitch + roll the body
; pushing L3 again will return to mode 1 (cycling)
;
; go to standby position with the ‘Triangle’ button, press again to go to walk position
; Attack posture with the ‘Cross’ button (*** only possible if legs are not moving )
; All = 1500 posture with the ‘Circle’ button ( only possible if legs are not moving )
; Learning to fly posture with the ‘Square’ button (uh ? nutty ?..it CAN fly…) ( only possible if legs are not moving )
; Change the legs ‘up’ position with the digital Up/Down buttons
;
; Change the gait speed with digital right/left buttons
;
; Presets buttons (only with ‘Default’ mode):
; - R1 => Tall grass
; - L1 => Tile floor
; - R2 => Body low
; - L2 => Default preset
;
; Little Gripper support on Pin 29 (Left/Right) and 30 (Open/Close) (only in ‘Body move’ mode)
; - R1 => Open
; - L1 => Close
; - R2 => Rotate Right
; - L2 => Rotate Left
;
; Deck support on Pin 31 (panning servo)
; allow to handle a deck (for camera, sensors, lights)
; the panning servo will follow the direction it walk
; Additional tilt servo support for 180° Deck on pin 28 (Round body only)
; When using a 180° deck + 2 GP2D12 back to back on a tilt servo
; the tilt servo will incline the ‘usable’ GP2D12 (looking the walking direction) to floor
; when walking at low speed, the angle increase (looking near)
; when walking at full speed, the angle is close to horizontal (looking far)
; when going opposite direction, the angle is mirrored to vertical,
; making the opposite GP2D12 looking near or far
;
; Start button toggles between the ‘knee angle shift’ and ‘normal’ mode
;
; R3 button (right joystick push button) : Horn
;
; you may have to push the Analog Button on a MadCatz Wireless controller (if in sleep mode)
;
;*********************************************
;
;
;--------------------------------------------------------------------
;-------------Constants
;PS2 Controller / BotBoard II (PS2 controller => pin 12, 13, 14, 15)
DAT con P12
CMD con P13
SEL con P14
CLK con P15
;*** SSC-32 card communication on pin 8 ***
SSC32 con p8
DeadZone con 28
PadMode con $79
;Deck
Deck_PulseMin con 600
Deck_PulseMax con 2400
;Little Gripper
LGripLR_PulseMin con 750
LGripLR_PulseMax con 2250
LGripOC_PulseMin con 750
LGripOC_PulseMax con 2250
;Legs
HipH_AngleMin con 21 ;30°
HipH_AngleMax con 107 ;150°
HipH_PulseMin con 910
HipH_PulseMax con 2090
HipV_AngleMin con 25 ;35°
HipV_AngleMax con 103 ;145°
HipV_PulseMin con 960
HipV_PulseMax con 2040
Knee_AngleMin con 36 ;50°
Knee_AngleMax con 107 ;150°
Knee_PulseMin con 1107
Knee_PulseMax con 2090
;****************************************************
; 3DOF-A Leg Dimensions (TibiaAngle constant = 0)
;HipV_HipH con 38 ;1.50" = 38mm (1.50 * 25.4)
;Femur_Length con 57 ;2.25" = 57mm (2.25 * 25.4)
;Tibia_Length con 124 ;4.875" = 124mm (4.875 * 25.4)
; 3DOF-B Leg Dimensions (TibiaAngle constant = 0)
;HipV_HipH con 29 ;1.14" = 29mm (1.14 * 25.4)
;Femur_Length con 57 ;2.25" = 57mm (2.25 * 25.4)
;Tibia_Length con 108 ;4.25" = 108mm (4.25 * 25.4)
; 3DOF-C Leg Dimensions (TibiaAngle constant = 20)
;HipV_HipH con 29 ;1.14" = 29mm (1.14 * 25.4)
;Femur_Length con 57 ;2.25" = 57mm (2.25 * 25.4)
;Tibia_Length con 141 ;5.55" = 141mm (5.55 * 25.4)
; 3DOF-(Old) Leg Dimensions (TibiaAngle constant = 0)
;HipV_HipH con 32 ;1.25" = 32mm (1.25 * 25.4)
;Femur_Length con 70 ;2.75" = 70mm (2.75 * 25.4)
;Tibia_Length con 108 ;4.25" = 108mm (4.25 * 25.4)
;****************************************************
; 3DOF-A Leg Dimensions (TibiaAngle constant = 0)
HipV_HipH con 38 ;1.50" = 38mm (1.50 * 25.4)
Femur_Length con 57 ;2.25" = 57mm (2.25 * 25.4)
Tibia_Length con 124 ;4.875" = 124mm (4.875 * 25.4)
LegUpShiftMin con 30
LegUpShiftMax con 70
KneeShiftPulse con 200 ;range is 0 to 255 (activated by pushing start button, affect the legs when up)
TibiaAngle con 0 ;range is -20 (interior) to 20 (exterior), 0 is vertical, this is ‘all time’ tibia angle
;ACos
data 64,64,63,63,63,62,62,62,61,61,61,60,60,60,59,59,|
59,59,58,58,58,57,57,57,56,56,56,55,55,55,54,54,|
54,53,53,53,52,52,52,51,51,51,50,50,50,49,49,49,|
48,48,48,47,47,46,46,46,45,45,45,44,44,44,43,43,|
42,42,42,41,41,41,40,40,39,39,39,38,38,37,37,37,|
36,36,35,35,35,34,34,33,33,32,32,31,31,31,30,30,|
29,29,28,28,27,27,26,25,25,24,24,23,23,22,21,21,|
20,19,19,18,17,16,15,15,14,13,11,10,09,07,05,00
; Don’t use ByteTable instead, it takes too much memory
RRHH con “0” ;Rear Right leg : Hip Horizontal : pin 0
RRHH2 con “0”
RRHV con “0” ;Rear Right leg : Hip Vertical : pin 1
RRHV2 con “1”
RRK con “0” ;Rear Right leg : Knee : pin 2
RRK2 con “2”
MRHH con “0” ;Middle Right leg : Hip Horizontal : pin 4
MRHH2 con “4”
MRHV con “0” ;Middle Right leg : Hip Vertical : pin 5
MRHV2 con “5”
MRK con “0” ;Middle Right leg : Knee : pin 6
MRK2 con “6”
FRHH con “0” ;Front Right leg : Hip Horizontal : pin 8
FRHH2 con “8”
FRHV con “0” ;Front Right leg : Hip Vertical : pin 9
FRHV2 con “9”
FRK con “1” ;Front Right leg : Knee : pin 10
FRK2 con “0”
RLHH con “1” ;Rear Left leg : Hip Horizontal : pin 16
RLHH2 con “6”
RLHV con “1” ;Rear Left leg : Hip Vertical : pin 17
RLHV2 con “7”
RLK con “1” ;Rear Left leg : Knee : pin 18
RLK2 con “8”
MLHH con “2” ;Middle Left leg : Hip Horizontal : pin 20
MLHH2 con “0”
MLHV con “2” ;Middle Left leg : Hip Vertical : pin 21
MLHV2 con “1”
MLK con “2” ;Middle Left leg : Knee : pin 22
MLK2 con “2”
FLHH con “2” ;Front Left leg : Hip Horizontal : pin 24
FLHH2 con “4”
FLHV con “2” ;Front Left leg : Hip Vertical : pin 25
FLHV2 con “5”
FLK con “2” ;Front Left leg : Knee : pin 26
FLK2 con “6”
;Little Gripper Pin
LGripLR Con “2” ;Little Gripper Left/Right : pin 29
LGripLR2 Con “9”
LGripOC Con “3” ;Little Gripper Open/close : pin 30
LGripOC2 Con “0”
; Deck Pin
DeckP Con “3” ;Deck Panning servo : pin 31
DeckP2 Con “1”
; Additional Tilt servo for 180° Deck
DeckTilt Con “2”
DeckTilt2 Con “8”
;--------------------------------------------------------------------
;-------------Variables
Index var Byte
DualShock var Byte(7)
DS2Mode var Byte
LastButton var Byte(2)
Steps var Byte
FlipFlap var Bit
FlagOff var Bit
MovesDelay var Sbyte
HeightAdjust var Bit
HeightShift var Sbyte(6)
KneeShift var Byte(7)
Steering var Sbyte
Height var Sbyte
LastHeight var Sbyte
HeightLimit var Byte
XSpeed var Sbyte
YSpeed var Sbyte
Roll var Sbyte
Pitch var Sbyte
Rotate var Sbyte(6)
DCoord var Byte
DAngle var Byte
NbSteps var Byte
GaitSpeed var Byte
GaitSpeedTmp var Byte
StepFlag var Byte
TmpXPos var Long
TmpYPos var Long
TmpZPos var Long
TmpDistance var Long
TmpSEW var Word
TmpSEWSEW var Long
TmpCos var Long
TmpAngle var Sbyte
XPos var Sword(6)
YPos var Sword(6)
ZPos var Sword(6)
XPos2 var Sword(6)
YPos2 var Sword(6)
ZPos2 var Sword(6)
Distance var Sword
HipH_Angle var Sbyte
HipV_Angle var Sword
Knee_Angle var Sword
HipH_Pulse var Word(6)
HipV_Pulse var Word(6)
Knee_Pulse var Word(6)
LittleGripOCPulse var Word
LittleGripLRPulse var Word
Deck_Pulse var Word
DeckTilt_Pulse var Word
LegUpShift var Byte
GaitKind var Bit
Freeze var Bit
Tripod var Bit
LockLegs var Bit
LeftStickMode var Byte
;--------------------------------------------------------------------
;***************
;*** Program ***
;***************
;-------------Init
clear
high CLK
LastButton(0) = 255
LastButton(1) = 255
LittleGripOCPulse = 1500
LittleGripLRPulse = 1500
HeightLimit = 25 - ABS(TibiaAngle / 2)
pause 500
;-------------Init SSC-32 with pulse offsets
serout SSC32,i38400,"#", |
RRHH,RRHH2,“po0 #”,RRHV,RRHV2,“po0 #”,RRK,RRK2,“po0 #”, |
MRHH,MRHH2,“po0 #”,MRHV,MRHV2,“po0 #”,MRK,MRK2,“po0 #”, |
FRHH,FRHH2,“po0 #”,FRHV,FRHV2,“po0 #”,FRK,FRK2,“po0 #”, |
RLHH,RLHH2,“po0 #”,RLHV,RLHV2,“po0 #”,RLK,RLK2,“po0 #”, |
MLHH,MLHH2,“po0 #”,MLHV,MLHV2,“po0 #”,MLK,MLK2,“po0 #”, |
FLHH,FLHH2,“po0 #”,FLHV,FLHV2,“po0 #”,FLK,FLK2,“po0”,13]
;SSC-32 -> H3 engine
gosub H3Init
Sound 9,[100\4235, 200\4435]
;--------------------------------------------------------------------
;-------------Main loop
main
;Ps2Query
low SEL
shiftout CMD,CLK,FASTLSBPRE,$1\8]
shiftin DAT,CLK,FASTLSBPOST,[DS2Mode\8]
high SEL
pause 1
low SEL
shiftout CMD,CLK,FASTLSBPRE,$1\8,$42\8]
shiftin DAT,CLK,FASTLSBPOST,[DualShock(0)\8, DualShock(1)\8, DualShock(2)\8, DualShock(3)\8, |
DualShock(4)\8, DualShock(5)\8, DualShock(6)\8]
high SEL
pause 1
if DS2Mode <> PadMode then
low SEL
shiftout CMD,CLK,FASTLSBPRE,$1\8,$43\8,$0\8,$1\8,$0\8] ;CONFIG_MODE_ENTER
high SEL
pause 1
low SEL
shiftout CMD,CLK,FASTLSBPRE,$01\8,$44\8,$00\8,$01\8,$03\8,$00\8,$00\8,$00\8,$00\8] ;SET_MODE_AND_LOCK
high SEL
pause 1
;low SEL
;shiftout CMD,CLK,FASTLSBPRE,$01\8,$4D\8,$00\8,$00\8,$01\8,$FF\8,$FF\8,$FF\8,$FF\8] ;VIBRATION_ENABLE
;high SEL
;pause 1
low SEL
shiftout CMD,CLK,FASTLSBPRE,$01\8,$4F\8,$00\8,$FF\8,$FF\8,$03\8,$00\8,$00\8,$00\8] ;SET_DS2_NATIVE_MODE
high SEL
pause 1
low SEL
shiftout CMD,CLK,FASTLSBPRE,$01\8,$43\8,$00\8,$00\8,$5A\8,$5A\8,$5A\8,$5A\8,$5A\8] ;CONFIG_MODE_EXIT_DS2_NATIVE
high SEL
pause 1
low SEL
shiftout CMD,CLK,FASTLSBPRE,$01\8,$43\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8] ;CONFIG_MODE_EXIT
high SEL
nap 4
goto main
endif
;**********************************************************************
; R1-L1-R2-L2 presets code
;
; to change Body Height (range is -25 (body up) to 25 (body down)) :
;Height = 10 ; for example
;
; to change Leg Up Shift (range is 20 (close to floor) to 70 (about 2.5 inches from floor))
;LegUpShift = 30 ; for example
;
; to change Gait Speed (range is 3 (fast) to 20 (very slow))
;GaitSpeed = 4 ; for example
If LockLegs then NoPresets
if (DualShock(2).bit3 = 0) and LastButton(1).bit3 then ;R1 Button test
Height = -25 ; tall grass
LegUpShift = 70
GaitSpeed = 10
elseif (DualShock(2).bit2 = 0) and LastButton(1).bit2 ;L1 Button test
Height = 5 ; tile floor
LegUpShift = 30
GaitSpeed = 3
elseif (DualShock(2).bit1 = 0) and LastButton(1).bit1 ;R2 Button test
Height = 25 ; Body low
LegUpShift = 35
GaitSpeed = 4
elseif (DualShock(2).bit0 = 0) and LastButton(1).bit0 ;L2 Button test
Height = 0 ; Default preset
LegUpShift = 35
GaitSpeed = 4
else
goto NoPresets
endif
HeightAdjust = 0
LegUpShift = (LegUpShift max LegUpShiftMax) min LegUpShiftMin
GaitKind = LegUpShift > 45
GaitSpeedTmp = GaitSpeed max 20
MovesDelay = 8
NoPresets
;**********************************************************************
if (DualShock(2).bit4 = 0) and LastButton(1).bit4 then ;Triangle Button test
FlagOff = FlagOff ^ 1
if FlagOff then
gosub All1500
Gosub InitPos
Nap 3
for Index = 0 to 31
serout SSC32,i38400,"#",DEC Index,"P0"]
next
serout SSC32,i38400,[13]
else
gosub H3Init
endif
endif
if FlagOff then
nap 3
goto OffEnd
endif
if (MovesDelay <= 0) or LockLegs then
if (DualShock(2).bit5 = 0) and LastButton(1).bit5 then ;Circle Button test
gosub All1500
endif
if (DualShock(2).bit6 = 0) and LastButton(1).bit6 then ;Cross Button test
gosub All1500
;Attack posture
serout SSC32,i38400,"#",MRHH,MRHH2,"P1800#",MRHV,MRHV2,"P1700#",MRK,MRK2,"P1700#", |
MLHH,MLHH2,"P1200#",MLHV,MLHV2,"P1300#",MLK,MLK2,"P1300T288",13]
nap 4
serout SSC32,i38400,"#",MRHV,MRHV2,"P1500#",MRK,MRK2,"P1500#",MLHV,MLHV2,"P1500#", |
MLK,MLK2,"P1500T288",13]
nap 4
serout SSC32,i38400,"#",RRHV,RRHV2,"P1800#",RRK,RRK2,"P1800#",MRHV,MRHV2,"P1400#", |
MRK,MRK2,"P1400#",FRHH,FRHH2,"P1800#",FRHV,FRHV2,"P2100#",RLHV,RLHV2,"P1200#", |
RLK,RLK2,"P1200#",MLHV,MLHV2,"P1600#",MLK,MLK2,"P1600#",FLHH,FLHH2,"P1200#", |
FLHV,FLHV2,"P900T288",13]
nap 4
for Index = 1 to 10
serout SSC32,i38400,"#",FRHV,FRHV2,"P2000#",FRK,FRK2,"P1600#",FLHV,FLHV2,"P900#", |
FLK,FLK2,"P1600T144",13]
nap 3
serout SSC32,i38400,"#",FRHV,FRHV2,"P2100#",FRK,FRK2,"P1400#",FLHV,FLHV2,"P1000#", |
FLK,FLK2,"P1400T144",13]
nap 3
next
serout SSC32,i38400,"#",FRHH,FRHH2,"P1500#",FRK,FRK2,"P1500#",FLHH,FLHH2,"P1500#", |
FLHV,FLHV2,"P900#",FLK,FLK2,"P1500T288",13]
nap 4
serout SSC32,i38400,"#",RRHV,RRHV2,"P1500#",RRK,RRK2,"P1500#",MRHV,MRHV2,"P1500#", |
MRK,MRK2,"P1500#",FRHV,FRHV2,"P1500#",MLHV,MLHV2,"P1500#",MLK,MLK2,"P1500#", |
FLHV,FLHV2,"P1500#",RLHV,RLHV2,"P1500#",RLK,RLK2,"P1500T576",13]
nap 5
serout SSC32,i38400,"#",MRHH,MRHH2,"P1500#",MRHV,MRHV2,"P1700#",MRK,MRK2,"P1700#", |
MLHH,MLHH2,"P1500#",MLHV,MLHV2,"P1300#",MLK,MLK2,"P1300T288",13]
nap 4
serout SSC32,i38400,"#",MRHV,MRHV2,"P1500#",MRK,MRK2,"P1500#",MLHV,MLHV2,"P1500#", |
MLK,MLK2,"P1500T288",13]
nap 4
endif
if (DualShock(2).bit7 = 0) and LastButton(1).bit7 then ;Square Button test
gosub All1500
;Learning to fly posture (uh ? nutty ?...it CAN fly...)
for Index = 1 to 4
serout SSC32,i38400,"#",MRHV,MRHV2,"P1900#",MRK,MRK2,"P1800#",MLHV,MLHV2,"P1100#", |
MLK,MLK2,"P1200T288",13]
serout SSC32,i38400,"#",RRHV,RRHV2,"P1600#",RRK,RRK2,"P1600#",FRHV,FRHV2,"P1600#", |
FRK,FRK2,"P1600#",RLHV,RLHV2,"P1400#",RLK,RLK2,"P1400#",FLHV,FLHV2,"P1400#", |
FLK,FLK2,"P1400T576",13]
nap 4
serout SSC32,i38400,"#",MRK,MRK2,"P1200#",MLK,MLK2,"P1800T288",13]
nap 4
serout SSC32,i38400,"#",RRHV,RRHV2,"P1400#",RRK,RRK2,"P1400#",MRHV,MRHV2,"P1500#", |
FRHV,FRHV2,"P1400#",FRK,FRK2,"P1400#",RLHV,RLHV2,"P1600#",RLK,RLK2,"P1600#", |
MLHV,MLHV2,"P1500#",FLHV,FLHV2,"P1600#",FLK,FLK2,"P1600T576",13]
nap 5
next
serout SSC32,i38400,"#",MRHV,MRHV2,"P1800#",MRK,MRK2,"P1800#",MLHV,MLHV2,"P1200#", |
MLK,MLK2,"P1200T288",13]
nap 4
gosub All1500
endif
endif
if (DualShock(1).bit0 = 0) and LastButton(0).bit0 then ;Select Button test
LockLegs = LockLegs ^ 1
Sound 9,[100\(2000 + LockLegs * 1000)]
gosub FlatBody
endif
if (DualShock(1).bit3 = 0) and LastButton(0).bit3 then ;Start Button test
KneeShift(6) = KneeShift(6) ^ KneeShiftPulse
Sound 9,[100\(800 + KneeShift(6))]
endif
if (DualShock(1).bit1 = 0) and LastButton(0).bit1 then ;L3 Button test
if LockLegs then
LeftStickMode = (LeftStickMode + 1) & 3
gosub FlatBody
else
HeightAdjust = HeightAdjust ^ 1
endif
Sound 9,[100\3500]
endif
if (DualShock(1).bit2 = 0) and LastButton(0).bit2 then ;R3 Button test (Horn)
Sound 9,[800\392, 600\370, 200\0, 800\330, 600\294, 200\0, 300\330, 100\0, 300\330]
endif
if DualShock(1).bit4 = 0 then ;Up Button test
LegUpShift = (LegUpShift + 1) max LegUpShiftMax
elseif DualShock(1).bit6 = 0 ;Down Button test
LegUpShift = (LegUpShift - 1) min LegUpShiftMin
else
goto NoSound
endif
GaitKind = LegUpShift > 45
Sound 9,[20\(LegUpShift * 10 + 400 + GaitKind * 200)]
MovesDelay = 8
NoSound
if (DualShock(1).bit5 = 0) and LastButton(0).bit5 then ;Right Button test
GaitSpeedTmp = (GaitSpeedTmp + 1) max 20
elseif (DualShock(1).bit7 = 0) and LastButton(0).bit7 ;Left Button test
GaitSpeedTmp = (GaitSpeedTmp - 1) min 3
else
goto NoSound1
endif
Sound 9,[20\(50-GaitSpeedTmp) * 35 + 400]
MovesDelay = 8
NoSound1
GaitSpeed = GaitSpeedTmp min (3 + GaitKind)
for Index = 3 to 6
if DualShock(Index) > (128 + DeadZone) then
DualShock(Index) = DualShock(Index) - DeadZone/2
elseif DualShock(Index) < (128 - DeadZone)
DualShock(Index) = DualShock(Index) + DeadZone/2
else
DualShock(Index) = 128
endif
next
YSpeed = DualShock(3) - 128 ; Right Stick Horizontal
XSpeed = DualShock(4) - 128 ; Right Stick Vertical
Steering = DualShock(5) - 128 ; Left Stick Horizontal
StepFlag = (GaitSpeedTmp - 2) max 3
if HeightAdjust or LockLegs then
Height = LastHeight + (((DualShock(6) - 128) / 5) - LastHeight) / StepFlag; Left Stick Vertical
endif
Height = (Height Max HeightLimit) min - HeightLimit
;H3
if Steering or (Height <> LastHeight) or YSpeed or XSpeed or LockLegs then
MovesDelay = 8
endif
if MovesDelay > 0 then
LastHeight = Height
DCoord = SQR(XSpeed * XSpeed + YSpeed * YSpeed)
if XSpeed > 0 then
TmpCos = XSpeed * 127 / DCoord
else
TmpCos = -((-XSpeed) * 127 / DCoord) ; **** due to bug in Basic ****
endif
gosub ACos
DAngle = TmpAngle
if YSpeed > 0 then
DAngle = 256 - DAngle
endif
DCoord = DCoord max (128 - ABS(Height) - ABS(TibiaAngle * 2))
; 180° Deck Code
if (Dangle < 64 or Dangle > 191) and (DCoord > 0) then
DeckTilt_Pulse = 1680 - Dcoord
else
DeckTilt_Pulse = 1380 + Dcoord
endif
Deck_Pulse = ((Deck_PulseMax - Deck_PulseMin) * ((DAngle + 64 ) & $7F)) / 127 + Deck_PulseMin
; 360° Deck Code
;Deck_Pulse = ((Deck_PulseMax - Deck_PulseMin) * DAngle) / 255 + Deck_PulseMin
Steps = Steps + 1
if Steps > NbSteps then
Steps = 1
FlipFlap = FlipFlap ^ 1
NbSteps = GaitSpeed
endif
if LockLegs then
if LeftStickMode > 0 then ;Body roll and pitch
if (leftStickMode & 1) then
Roll = Roll + (Steering - Roll) / StepFlag
Steering = 0
else
Roll = 0
endif
if (leftStickMode & 2) then
Pitch = Pitch + (Height - Pitch) / StepFlag
Height = 0
else
Pitch = 0
endif
HeightShift(0) = Roll/10
HeightShift(2) = HeightShift(0)
HeightShift(3) = -HeightShift(0)
HeightShift(5) = HeightShift(3)
HeightShift(0) = ((HeightShift(0) + Pitch) max HeightLimit) min -HeightLimit
HeightShift(3) = -HeightShift(0)
HeightShift(2) = ((HeightShift(2) - Pitch) max HeightLimit) min -HeightLimit
HeightShift(5) = -HeightShift(2)
HeightShift(1) = HeightShift(0) + HeightShift(2)
HeightShift(4) = HeightShift(3) + HeightShift(5)
endif
LittleGripOCPulse = ((LittleGripOCPulse + (DualShock(2).bit3 * 50) - (DualShock(2).bit2 * 50)) |
min LGripOC_PulseMin) max LGripOC_PulseMax
LittleGripLRPulse = ((LittleGripLRPulse + (DualShock(2).bit1 * 50) - (DualShock(2).bit0 * 50)) |
min LGripLR_PulseMin) max LGripLR_PulseMax
GaitSpeedTmp = StepFlag + 2
else
StepFlag = NbSteps - Steps + 1
endif
for Index = 0 to 5
XPos2(Index) = -(DCoord * COS(DAngle + (Index * 43 + 21)) / 300) ; 43 => 60 degrees
ZPos2(Index) = -(DCoord * SIN(DAngle + (Index * 43 + 21)) / 300) ; 43 => 60 degrees
next
Tripod = FlipFlap
gosub AorB_Down
Freeze = Steps < (NbSteps - GaitKind)
Tripod = FlipFlap ^ 1
gosub AorB_Up
for Index = 0 to 5
;Distance and HipH_Angle with XZ
if Zpos(Index) > 0 then
Distance = SQR((XPos(Index) * XPos(Index) + ZPos(Index) * ZPos(Index)) / 4) * 2
else
Distance = SQR((XPos(Index) * XPos(Index) |
+ (-ZPos(Index)) * (-ZPos(Index))) / 4) * 2 ; **** due to bug in Basic ****
endif
if ZPos(Index) > 0 then
TmpCos = ZPos(Index) * 127 / Distance
else
TmpCos = -(-ZPos(Index) * 127 / Distance) ; **** due to bug in Basic ****
endif
Distance = Distance + TibiaAngle ; Tibia angle for special legs (3DOF-C)
Gosub ACos
HipH_Angle = (TmpAngle max HipH_AngleMax) min HipH_AngleMin
;----------
;Set Angle
TmpXPos = XPos(Index) * 127
TmpYPos = (-((-YPos(Index)) * 127)) max 0 ; **** due to bug in Basic ****
if ZPos(Index) > 0 then
TmpZPos = ZPos(Index) * 127
else
TmpZPos = -((-ZPos(Index)) * 127) ; **** due to bug in Basic ****
endif
TmpDistance = ((Distance - HipV_HipH) * 127) min 0
TmpSEWSEW = ((-TmpYPos) * (-TmpYPos) + TmpDistance * TmpDistance) / 16129 ; **** due to bug in Basic ****
TmpSEW = SQR(TmpSEWSEW)
TmpCos = (Femur_Length * Femur_Length + Tibia_Length * Tibia_Length - TmpSEWSEW)
if TmpCos > 0 then
TmpCos = TmpCos * 127 / (2 * Femur_Length * Tibia_Length)
else
TmpCos = -((-TmpCos) * 127 / (2 * Femur_Length * Tibia_Length)) ; **** due to bug in Basic ****
endif
gosub ACos
Knee_Angle = (TmpAngle max Knee_AngleMax) min Knee_AngleMin
TmpCos = -TmpYPos / TmpSew
gosub ACos
HipV_Angle = TmpAngle
TmpCos = (Femur_Length * Femur_Length - Tibia_Length * Tibia_Length + TmpSEWSEW)
if TmpCos > 0 then
TmpCos = TmpCos * 127 / (2 * Femur_Length * TmpSEW)
else
TmpCos = -((-TmpCos) * 127 / (2 * Femur_Length * TmpSEW)) ; **** due to bug in Basic ****
endif
gosub ACos
HipV_Angle = ((TmpAngle + HipV_Angle) max HipV_AngleMax) min HipV_AngleMin
;----------
HipH_Angle = HipH_Angle + Rotate(Index)
HipH_Pulse(Index) = (((HipH_PulseMax - HipH_PulseMin) * (HipH_Angle - HipH_AngleMin) |
/ (HipH_AngleMax - HipH_AngleMin) + HipH_PulseMin) max HipH_PulseMax) min HipH_PulseMin
HipV_Pulse(Index) = (((HipV_PulseMax - HipV_PulseMin) * (HipV_Angle - HipV_AngleMin) |
/ (HipV_AngleMax - HipV_AngleMin) + HipV_PulseMin) max HipV_PulseMax) min HipV_PulseMin
Knee_Pulse(Index) = (((Knee_PulseMax - Knee_PulseMin) * (Knee_Angle - Knee_AngleMin) |
/ (Knee_AngleMax - Knee_AngleMin) + Knee_PulseMin) max Knee_PulseMax) min Knee_PulseMin
Knee_Pulse(Index) = 3000 - Knee_Pulse(Index) - KneeShift(Index) * 2
next
serout SSC32,i38400,"#",RRHH,RRHH2,"P",DEC HipH_Pulse(0),"#",RRHV,RRHV2,"P",DEC HipV_Pulse(0), |
"#",RRK,RRK2,"P",DEC Knee_Pulse(0),"#",FRHH,FRHH2,"P",DEC HipH_Pulse(2), |
"#",FRHV,FRHV2,"P",DEC HipV_Pulse(2),"#",FRK,FRK2,"P",DEC Knee_Pulse(2), |
"#",MLHH,MLHH2,"P",DEC HipH_Pulse(4),"#",MLHV,MLHV2,"P",DEC 3000 - HipV_Pulse(4), |
"#",MLK,MLK2,"P",DEC 3000 - Knee_Pulse(4),"#",MRHH,MRHH2,"P",DEC HipH_Pulse(1), |
"#",MRHV,MRHV2,"P",DEC HipV_Pulse(1),"#",MRK,MRK2,"P",DEC Knee_Pulse(1), |
"#",RLHH,RLHH2,"P",DEC HipH_Pulse(5),"#",RLHV,RLHV2,"P",DEC 3000 - HipV_Pulse(5), |
"#",RLK,RLK2,"P",DEC 3000 - Knee_Pulse(5),"#",FLHH,FLHH2,"P",DEC HipH_Pulse(3), |
"#",FLHV,FLHV2,"P",DEC 3000 - HipV_Pulse(3),"#",FLK,FLK2,"P",DEC 3000 - Knee_Pulse(3), |
"#",DeckP,DeckP2,"P",DEC Deck_Pulse,"#",DeckTilt,DeckTilt2,"P",DEC DeckTilt_Pulse, |
"#",LGripOC,LGripOC2,"P",DEC LittleGripOCPulse,"#",LGripLR,LGripLR2,"P",DEC LittleGripLRPulse,"T190",13]
else
nap 3
if MovesDelay < -17 then
MovesDelay = (NbSteps - GaitKind) - Steps + 1
EndIf
endif
MovesDelay = MovesDelay - 1
OffEnd
LastButton(0) = DualShock(1)
LastButton(1) = DualShock(2)
goto main
;--------------------------------------------------------------------
;-------------Sub H3 Init
H3Init
LockLegs = 0
NbSteps = 4
GaitSpeedTmp = NbSteps
LegUpShift = 35
for Index = 0 to 5
XPos(Index) = HipV_HipH + Femur_Length
YPos(Index) = - Tibia_Length
ZPos(Index) = 0
next
Gosub InitPos
MovesDelay = 8
; then All1500
;-------------Sub All1500
All1500
HeightAdjust = 1
height = 0
LastHeight = 0
for Index = 0 to 27 ;preserve servo 28 (Deck Tilt),29 and 30 (Little Gripper),31 (Deck)
serout SSC32,i38400,"#",DEC Index,"P1500"]
next
serout SSC32,i38400,"T576",13]
nap 5
Steps = NbSteps
; then Flat Body
;-------------Sub Flat Body
FlatBody
Roll = 0
Pitch = 0
for Index = 0 to 5
HeightShift(Index) = 0
next
return
;-------------Sub InitPos
InitPos
serout SSC32,i38400,"#",RRHV,RRHV2,“P2100#”,RRK,RRK2,“P1800#”,MRHV,MRHV2,“P2100#”,MRK,MRK2,“P1800#”, |
FRHV,FRHV2,“P2100#”,FRK,FRK2,“P1800#”,RLHV,RLHV2,“P900#”,RLK,RLK2,“P1200#”,MLHV,MLHV2,“P900#”, |
MLK,MLK2,“P1200#”,FLHV,FLHV2,“P900#”,FLK,FLK2,“P1200T1152”,13]
nap 5
serout SSC32,i38400,"#",RRHH,RRHH2,“P1500#”,MRHH,MRHH2,“P1500#”,FRHH,FRHH2,“P1500#”, |
RLHH,RLHH2,“P1500#”,MLHH,MLHH2,“P1500#”,FLHH,FLHH2,“P1500T576”,13]
nap 5
return
;-------------Sub Arc Cosinus
ACos
TmpCos = (TmpCos max 127) min -127
if TmpCos < 0 then
read -TmpCos,TmpAngle
TmpAngle = 127 - TmpAngle
else
read TmpCos,TmpAngle
endif
return
;-------------Tripod ‘A’ (tripod = 0) or ‘B’ (Tripod = 1) Down
AorB_Down
for Index = 0 + Tripod to 4 + Tripod Step 2
KneeShift(Index) = 0
Rotate(Index) = Rotate(Index) + (Steering/5 - Rotate(Index))/StepFlag
YPos(Index) = -Tibia_Length + Height + HeightShift(Index)
XPos(Index) = XPos(Index) + (XPos2(Index) - (XPos(Index) - (HipV_HipH + Femur_Length)))/StepFlag
ZPos(Index) = ZPos(Index) + (ZPos2(Index) - ZPos(Index))/StepFlag
next
return
;-------------Tripod ‘A’ (tripod = 0) or ‘B’ (Tripod = 1) Up
AorB_Up
if LockLegs then AorB_Down ;Goto sub AorB_Down and return directly from it to main loop
if (Steps = NbSteps) and GaitKind then
StepFlag = NbSteps
goto AorB_Down ;Goto sub AorB_Down and return directly from it to main loop
else
StepFlag = StepFlag - GaitKind - 1
endif
for Index = 0 + Tripod to 4 + Tripod Step 2
if (Steps > 1) and (Steps < (NbSteps - GaitKind - 1)) then
KneeShift(Index) = KneeShift(6)
else
KneeShift(Index) = 0
endif
Rotate(Index) = Rotate(Index) + (-Steering/5 - Rotate(Index))/StepFlag
YPos(Index) = -Tibia_Length + Height + (LegUpShift * Freeze)
XPos(Index) = XPos(Index) + (-XPos2(Index) - (XPos(Index) - (HipV_HipH + Femur_Length)))/StepFlag
ZPos(Index) = ZPos(Index) + (-ZPos2(Index) - ZPos(Index))/StepFlag
next
return
;--------------------------------------------------------------------
It loaded with no errors. I applied power to the controller and i didn’t hear the few short beeps and the legs didn’t snap into position as was instructed in the assembly guide.
Why is this?
