Here is the complete code. This code gives my a lot of errors when I press the debug button. It compiles just fine and loads into the atom with no problems.
[code];
; Use Basic Micro ATOM PRO IDE 08.0.1.7
; *** not suitable for IDE 02.2.1.1 (there’s a specific version for this IDE)
; *** not suitable for IDE 05.3.0.0 (there’s a specific version for this IDE)
; Use Basic Atom PRO 28
;My version 1.02 “Hector 1”
;************************************************
;****** Basic Atom with SSC-32, PC control ******
;----------------- 3.42 3DOF-A ------------------
;---- Serial Port Control (Bot Board / PC) ------
;-------------- 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 17, Hip Vertical : pin 16, Knee : pin 15
; Middle Right leg : Hip Horizontal : pin 20, Hip Vertical : pin 19, Knee : pin 18
; Front Right leg : Hip Horizontal : pin 23, Hip Vertical : pin 22, Knee : pin 21
;
; 3 left legs connections :
; Rear Left leg : Hip Horizontal : pin 2, Hip Vertical : pin 1, Knee : pin 0
; Middle Left leg : Hip Horizontal : pin 5, Hip Vertical : pin 4, Knee : pin 3
; Front Left leg : Hip Horizontal : pin 8, Hip Vertical : pin 7, Knee : pin 6
;
; 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 : Horn
;
;*********************************************
;
;
;--------------------------------------------------------------------
;-------------Constants
;
DeadZone con 8
;*** SSC-32 card communication on pin 15 ***
SSC32 con p15
;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
ArcCos Bytetable 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 “1” ;Rear Right leg : Hip Horizontal : pin 17
RRHH2 con “7”
RRHV con “1” ;Rear Right leg : Hip Vertical : pin 16
RRHV2 con “6”
RRK con “1” ;Rear Right leg : Knee : pin 15
RRK2 con “5”
MRHH con “2” ;Middle Right leg : Hip Horizontal : pin 20
MRHH2 con “0”
MRHV con “1” ;Middle Right leg : Hip Vertical : pin 19
MRHV2 con “9”
MRK con “1” ;Middle Right leg : Knee : pin 18
MRK2 con “8”
FRHH con “2” ;Front Right leg : Hip Horizontal : pin 23
FRHH2 con “3”
FRHV con “2” ;Front Right leg : Hip Vertical : pin 22
FRHV2 con “2”
FRK con “2” ;Front Right leg : Knee : pin 21
FRK2 con “1”
RLHH con “0” ;Rear Left leg : Hip Horizontal : pin 2
RLHH2 con “2”
RLHV con “0” ;Rear Left leg : Hip Vertical : pin 1
RLHV2 con “1”
RLK con “0” ;Rear Left leg : Knee : pin 0
RLK2 con “0”
MLHH con “0” ;Middle Left leg : Hip Horizontal : pin 5
MLHH2 con “5”
MLHV con “0” ;Middle Left leg : Hip Vertical : pin 4
MLHV2 con “4”
MLK con “0” ;Middle Left leg : Knee : pin 3
MLK2 con “3”
FLHH con “0” ;Front Left leg : Hip Horizontal : pin 8
FLHH2 con “8”
FLHV con “0” ;Front Left leg : Hip Vertical : pin 7
FLHV2 con “7”
FLK con “0” ;Front Left leg : Knee : pin 6
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)
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
CheckSum var Byte
;--------------------------------------------------------------------
;***************
;*** Program ***
;***************
;-------------Init
clear
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
;SerialQuery
serout S_OUT,i9600,“RD”] ; ‘Ready’ to recieve data now
serin S_OUT,i9600,200000,NoData,[str DualShock(0)\7]
;serin S_OUT,i9600,[str DualShock(0)\7]
CheckSum = DualShock(1) ^ DualShock(2) ^ DualShock(3) ^ |
DualShock(4) ^ DualShock(5) ^ DualShock(6)
;serout S_OUT,i9600,[CheckSum]
if CheckSum <> DualShock(0) then
Sound 9,[50\2000]
NoData
DualShock(1) = $FF
DualShock(2) = $FF
for Index = 3 to 6
DualShock(Index) = $80
next
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 (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
Sound 9,[100\4235, 200\4435] ;Triangle Button test
FlagOff = FlagOff ^ 1
if FlagOff then
gosub All1500
Gosub InitPos
pause 144
for Index = 0 to 31
serout SSC32,i38400,"#",DEC Index,"P0"]
next
serout SSC32,i38400,[13]
else
gosub H3Init
endif
endif
if FlagOff then
pause 144
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
Sound 9,[100\4235, 200\4435]
serout S_OUT,i9600,"AT"] ;send "AT" to serial port to indicate that routine is active (by me)
high p3;led on (by me)
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]
pause 288
serout SSC32,i38400,"#",MRHV,MRHV2,"P1500#",MRK,MRK2,"P1500#",MLHV,MLHV2,"P1500#", |
MLK,MLK2,"P1500T288",13]
pause 288
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]
pause 288
for Index = 1 to 10
serout SSC32,i38400,"#",FRHV,FRHV2,"P2000#",FRK,FRK2,"P1600#",FLHV,FLHV2,"P900#", |
FLK,FLK2,"P1600T144",13]
pause 144
serout SSC32,i38400,"#",FRHV,FRHV2,"P2100#",FRK,FRK2,"P1400#",FLHV,FLHV2,"P1000#", |
FLK,FLK2,"P1400T144",13]
pause 144
next
serout SSC32,i38400,"#",FRHH,FRHH2,"P1500#",FRK,FRK2,"P1500#",FLHH,FLHH2,"P1500#", |
FLHV,FLHV2,"P900#",FLK,FLK2,"P1500T288",13]
pause 288
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]
pause 576
serout SSC32,i38400,"#",MRHH,MRHH2,"P1500#",MRHV,MRHV2,"P1700#",MRK,MRK2,"P1700#", |
MLHH,MLHH2,"P1500#",MLHV,MLHV2,"P1300#",MLK,MLK2,"P1300T288",13]
pause 288
serout SSC32,i38400,"#",MRHV,MRHV2,"P1500#",MRK,MRK2,"P1500#",MLHV,MLHV2,"P1500#", |
MLK,MLK2,"P1500T288",13]
pause 288
low p3;led off (by me)
endif
if (DualShock(2).bit7 = 0) and LastButton(1).bit7 then ;Square Button test
Sound 9,[100\3235, 100\3435]
serout S_OUT,i9600,"FL"] ;send "FL" to serial port to indicate that routine is active (by me)
high p4;led on (by me)
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]
pause 288
serout SSC32,i38400,"#",MRK,MRK2,"P1200#",MLK,MLK2,"P1800T288",13]
pause 288
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]
pause 576
next
serout SSC32,i38400,"#",MRHV,MRHV2,"P1800#",MRK,MRK2,"P1800#",MLHV,MLHV2,"P1200#", |
MLK,MLK2,"P1200T288",13]
pause 288
gosub All1500
low p4; led off (by me)
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)
TmpCos = XSpeed * 127 / DCoord
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 * SIN(DAngle + (Index * 43 + 21) + 64) / 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
Distance = SQR((XPos(Index) * XPos(Index) + ZPos(Index) * ZPos(Index)) / 4) * 2
TmpCos = ZPos(Index) * 127 / Distance
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
TmpZPos = ZPos(Index) * 127
TmpDistance = ((Distance - HipV_HipH) * 127) min 0
TmpSEWSEW = (TmpYPos * TmpYPos + TmpDistance * TmpDistance) / 16129
TmpSEW = SQR(TmpSEWSEW)
TmpCos = (Femur_Length * Femur_Length + Tibia_Length * Tibia_Length - TmpSEWSEW)
TmpCos = TmpCos * 127 / (2 * Femur_Length * Tibia_Length)
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)
TmpCos = TmpCos * 127 / (2 * Femur_Length * TmpSEW)
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,"T210",13]
else
pause 144
if MovesDelay < -15 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]
pause 576
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]
pause 576
serout SSC32,i38400,"#",RRHH,RRHH2,“P1500#”,MRHH,MRHH2,“P1500#”,FRHH,FRHH2,“P1500#”, |
RLHH,RLHH2,“P1500#”,MLHH,MLHH2,“P1500#”,FLHH,FLHH2,“P1500T576”,13]
pause 576
return
;-------------Sub Arc Cosinus
ACos
TmpCos = (TmpCos max 127) min -127
if TmpCos < 0 then
TmpAngle = 128 - ArcCos(-TmpCos)
else
TmpAngle = ArcCos(TmpCos)
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
;--------------------------------------------------------------------
[/code]
