Hexapod RC code = get it here

Lynxmotion Electronics & Software
1

I searched high and low for some example RC code for the lynxmotion hex’s and never did turn up anything that worked really well if at all, so I hacked the heck out of the PS2 demo code for it and here it is, 3 channel (steering, throttle, switch) all is incorporated into the basic code for the hex, the only thing necessary is to modify the “Offsets” portion to match your crawler. it runs at full speed with absolutely zero lag on the standard basic atom 28 pin module. I will post pictures of the wiring tonight.

(</code
FF
/code>)

the steering/throttle values are between -120 and +120 and the switch is a high/low value (1 or 0).

2

Is this code for a 3DOF or a 2DOF hexapod? Looking forward to seeing it.

3

My bad, the “CoderQuotes” plugin for firefox that i use on the fedora forums doesn’t work here.

its the 3 DOF.

this is the code for reading the RC signals

[code]stmaxvalue var word ; maximum raw value for the steering
stminvalue var word ; minimum raw value for the steering
strange var word ; steering range, positive integer
stcentershift var word ; center point for steering channel if only positive intergers (raw values) are used
thrmaxvalue var word ; maximum raw value for the throttle
thrminvalue var word ; minimum raw value for the throttle
thrrange var word ; the range of the throttle, positive integer
thrcentershift var word ; center point for throttle channel if only positive integers (raw Values) are used
testnum var word ; testnumber for the calibration procedure = or > the calibratetime value
thr var sword ;raw input value for throttle
st var sword ;raw input value for steering
aux var sword ; raw input value for auxilary
xspeed var sword ; recommend keeping as a sword, uses this value repeatedly for filtering, mod only if space is an issue
steering var sword ; recommend keeping as a sword, uses this value repeatedly for filtering, mod only if space is an issue
rcbutton var sword ; third channel if you make your own filter then this can be a signed number, for now its a 1 or 0

rcstaticdeadband con 10 ;the deadband around center (divide by 128 for a percentage, i.e. 10 deadband is 8.33_%)
thrin con P1 ;pin number for the throttle channel
stin con P2 ;pin number for the steering channel
auxin con p0 ;pin number for the Aux in channel
spk con p9 ; botboard II has the speaker on pin 9 - only used for the sound to let you know its calibrating
calibratetime con 128 ; number of tests before saving/stopping calibration

stmaxvalue = 0 ;if the bot drifts left/right/
stminvalue = 2000 ;forward/back then change the 2000’s to 8000’s
thrmaxvalue = 0
thrminvalue = 2000

xspeed = 0 ; this is the forward speed, Why was X chosen for speed? shouldn’t it be Y?
steering = 0 ; Steering rate

;########################################
;Calibration procedure, will beep for a couple seconds
;whilst you move all channels to their extremes
;a few times auto generates max and min values for each channel

for testnum = 1 to calibratetime ;count from 1 to the value of calibrate time, the current value is stored in testnum
high spk ; tie the speaker pin to positive - makes a click sound
pulsin thrin,1,thr ; read the minor pulse length of the throttle signal from the receiver (the short part of the pulse)
pulsin stin,1,st ; same for steering, the 1 here makes this process run 8 times faster than if it were a 0

	if thr > thrmaxvalue then ; if the current maximum value for throttle is greater than the highest stored one, then
		thrmaxvalue = thr ; store the current (highest) value in place of the (lower) max value
		elseif thr < thrminvalue ; if the current throttle value is lower than the lowest ever seen then
		thrminvalue = thr ; store the new lowest value in its (the last low value) place
	endif

	if st > stmaxvalue then ; if the current maximum value for steering is greater than the highest stored one, then
		stmaxvalue = st ; store the current (highest) value in place of the (lower) max value
		elseif st < stminvalue ; if the current throttle value is lower than the lowest ever seen then
		stminvalue = st ; store the new lowest value in its (the last low value) place
	endif
	


low spk ; ground the speaker - makes a click sound

pause 10 ; added a pause so the calibration procedure gives you enough time to react and move the controls
next

thrrange = thrmaxvalue - thrminvalue ; set the throttles range equal to the difference between its max and min values
thrcentershift = thrrange / 2 ; set the center point of the throttle halfway between the two extremes
strange = stmaxvalue - stminvalue ; set the steerings range equal to the difference between its max and min values
stcentershift = strange / 2 ; set the center point of the steering halfway between the two extremes

top

; the guts

;Throttle
; returns about -118 to +118
Pulsin thrin,1,thr ; read the current value of the throttle channel

xspeed = thr - thrminvalue - thrcentershift ; set xspeed equal to a positive or negative value proportional to its extremes
xspeed = xspeed / 3 ; divide xspeed by 3 so it fits within the +/- 120 specs for the hexapod

;hard to explain this one, read it a few times, if the value is bouncing around center, then this will filter it out and set it to 0 instead
if xspeed > rcstaticdeadband then
xspeed = xspeed
elseif xspeed < 0 - rcstaticdeadband
xspeed = xspeed
else
xspeed = 0
endif

;#################################################
;Steering - tweaked for -19 to +19 range according to the comments in the orignal
;hex code, but in actual use seems to work better set for giving a +120 to -120 range

Pulsin stin,1,st

steering = st - stminvalue - stcentershift ; set steering equal to a positive or negative value proportional to its extremes
steering = steering / 3 ;19 here gives a +/-19 range and 3 gives a +/- 118 range, the later seems to work better.

;hard to explain this one, read it a few times, if the value is bouncing around center, then this will filter it out and set it to 0 instead
if steering > rcstaticdeadband then
steering = steering
elseif steering < 0 - rcstaticdeadband
steering = steering
else
steering = 0
endif

;#################################################

goto top[/code]

4

and this is the full program (its broken, don’t touch, leave as is, etc. you’ve been warned.)

[code]; Use Basic Micro ATOM IDE 05.3.0.0
; *** not suitable for IDE 02.2.1.1 (there’s a specific version for this IDE)
; *** not suitable for PRO IDE 08.0.1.7 (there’s a specific version for this IDE)
; Use Basic Atom 28

;************************************************
;*** Basic Atom with SSC-32 and PS2 DualShock ***
;----------------- 1.43 3DOF-C ------------------
;--------------- PS2 Controller -----------------
;-------------- Round Body (H3-R) ---------------
;********** Bot Board Buzzer support ************
;***** Little Gripper / Pan & Tilt support ******
;**************** Deck support ******************
;************************************************
;** Programmer: Laurent Gay, lynxrios-at######### **
;************************************************
;
; 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 14
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-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)

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 20 ;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)

Height var Sbyte
LastHeight var Sbyte
HeightLimit var Byte
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

stmaxvalue var word
stminvalue var word
strange var word
stcentershift var word
aumaxvalue var word
auminvalue var word
aurange var word
aucentershift var word
thrmaxvalue var word
thrminvalue var word
thrrange var word
thrcentershift var word
testnum var word
thr var sword
st var sword
au var sword
Xspeed var sword
steering var sword
rcbutton var sword

rcstaticdeadband con 10
thrin con P1
stin con P2
auin con p0
spk con p9
calibratetime con 127

;--------------------------------------------------------------------
;***************
;*** Program ***
;***************

;-------------Init
clear
auminvalue = 2000
aumaxvalue = 0
stmaxvalue = 0
stminvalue = 2000
thrmaxvalue = 0
thrminvalue = 2000
xspeed = 0
steering = 0
rcbutton = 1

calibrate
pause 3000
for testnum = 1 to 127
high spk
pulsin thrin,1,thr
pulsin stin,1,st

	if thr > thrmaxvalue then
		thrmaxvalue = thr
		elseif thr < thrminvalue 
		thrminvalue = thr
	endif

	if st > stmaxvalue then
		stmaxvalue = st
		elseif st < stminvalue
		stminvalue = st
	endif

	if au > aumaxvalue then
		aumaxvalue = au
		elseif st < auminvalue
		auminvalue = au
	endif
	


low spk

pause 10
next

thrrange = thrmaxvalue - thrminvalue
thrcentershift = thrrange / 2
strange = stmaxvalue - stminvalue
stcentershift = strange / 2
aurange = aumaxvalue - auminvalue
aucentershift = aurange / 2

Sound 9,[100000\8235, 200000\2435]

programstart

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,I8N1_38400,"#", |
RRHH,RRHH2,“po-40 #”,RRHV,RRHV2,“po7 #”,RRK,RRK2,“po-7 #”, |
MRHH,MRHH2,“po-31 #”,MRHV,MRHV2,“po-48 #”,MRK,MRK2,“po-12 #”, |
FRHH,FRHH2,“po-2 #”,FRHV,FRHV2,“po-12 #”,FRK,FRK2,“po5 #”, |
RLHH,RLHH2,“po-26 #”,RLHV,RLHV2,“po-33 #”,RLK,RLK2,“po-22 #”, |
MLHH,MLHH2,“po-40 #”,MLHV,MLHV2,“po45 #”,MLK,MLK2,“po10 #”, |
FLHH,FLHH2,“po5 #”,FLHV,FLHV2,“po16 #”,FLK,FLK2,“po-10”,13]

;SSC-32 -> H3 engine
gosub H3Init
Sound 9,[100000\4235, 200000\4435]
;--------------------------------------------------------------------
;-------------Main loop
main

Height = 5 ; tall grass
LegUpShift = 30
GaitSpeed = 3
HeightAdjust = 0
LegUpShift = (LegUpShift max LegUpShiftMax) min LegUpShiftMin
GaitKind = LegUpShift > 45
GaitSpeedTmp = GaitSpeed max 20
MovesDelay = 8
NoPresets
;################################################
;NOTE: X AND Y ARE FLIPPED, THEY ARE WRONG IN ORIGINAL CODE
;Throttle
; returns about -118 to +118
Pulsin thrin,1,thr

xspeed = thr - thrminvalue - thrcentershift
xspeed = xspeed / 3

if xspeed > rcstaticdeadband then
xspeed = xspeed
elseif xspeed < 0 - rcstaticdeadband
xspeed = xspeed
else
xspeed = 0
endif

;#################################################
;Steering - tweaked for +/-118 range

Pulsin stin,1,st

steering = st - stminvalue - stcentershift
steering = steering / 3

if steering > rcstaticdeadband then
steering = steering
elseif steering < 0 - rcstaticdeadband
steering = steering
else
steering = 0
endif

if rcbutton = 0 then ;rcbutton tesT
gosub hold
endif

;@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@

GaitSpeed = GaitSpeedTmp min (3 + GaitKind)

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 * 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
		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,I8N1_38400,"#",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,"T180",13]
else
	pause 144
	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,I8N1_38400,"#",DEC Index,"P1500"]
next
serout SSC32,I8N1_38400,"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,I8N1_38400,"#",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,I8N1_38400,"#",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
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

;--------------------------------------------------------------------
;-------------Sub All1500
hold
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,I8N1_38400,"#",DEC Index,"P1500"]
next
serout SSC32,I8N1_38400,"T576",13]

pause 576

Steps = NbSteps
return
; then Flat Body[/code]
5

Can anybody host the .bas files instead of making them posts? Until then I can email people the .bas files so the tabs, etc. are still there. PM me if you want them.

At this point the code is 98% functional, two main issues, when standing still, it paces (you’ll understand if you run it) and AUX channel has issues(my priority right now, its returning unexplained values, ill have to trace it).

My setup is the Spektrum DX3.0 transmitter and the old style, gray box receiver with the button on it, a 2S1P A123 battery (freaking awesome for robotics, kick lipo’s butt, charges in 10 mins and runs for hours and is pretty small and light too) and the BotBoard II with the standard basic atom and SSC-32 servo sequencer on the CH3-R and the standard HS-475HB servos

Pinout
P0 - AUX from RX
P1 - THR from RX
P2 - STR from RX
P4 - being used for RX power, tied to input voltage (spektrum take up to 9v)
P8 - serial out to SSC32

the SSC-32 pin out is the same as the default so no work is necessary on it.

Regards,
Matt.

PS, I know it needs to be cleaned up and tweaked a bit.

6

I have new polished code to be posted tonight, fully functional including standing still (no more pacing). And lots of speed tweaks so that 5 channels work now with no stutter during steps (you just need to copy/paste the “read” snippets of code for each channel and same for the variables), The actual code im posting is for 3 channels however as that is what i am using.

Also modified a setting in one of the serial outs called “T180” to “T130”, it walks a fair bit faster now. Im figuring its a command for regulating the update speed for the SSC-32.

7

You can host code here.
otherrobots.com/Downloads/tabid/56/Default.aspx

8

Sorry guys, the library is blocking me from downloading the .bas files from my email but I am going to be in a hotel in Edmonton tomorrow for business and i’ll have a company laptop I can use to post the code at the hotel.

Thanks for that link.