Powerpod output converted to run on Atom Pro

Lynxmotion Legged Robots
1

Edit: PowerPod now supports the Atom Pro. There is a lot of useful info in here. But it’s now possible to let PowerPod create your Atom Pro program for you.

Since I have sent this to several members, I will go ahead and post the code. This program was originally generated by the Powerpod and I then converted it to run on the Atom Pro. I have integrated two versions, both the Round and the inline version. I hope it is still working, but I make no promises!

[code];%CONFIG% BASICATOM28 $0 $0 $0 $0 $0 $0 $0 $0 $0 $0 $0 $0 $0 $0 $0 $0 $0 $0
; 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)
; 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.36 3DOF-C ------------------
;--------------- PS2 Controller -----------------
ROUND_BODY con 1
#if ROUND_BODY=1
;-------------- Round Body (H3-R) ---------------
#else
;-------------- Inline Body (H3) ----------------
#endif
;********** 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
#if ROUND_BODY=1
; 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
#endif
;
; 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
;DualShock
DAT con P12
CMD con P13
SEL con P14
CLK con P15

DeadZone con 28
PadMode con $79

; serial port
SER_POUT con P11
SER_PIN con P10
SER_MODE con i38400

;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 0 ;range is -20 (interior) to 20 (exterior), 0 is vertical, this is ‘all time’ tibia angle

;ACos
acosTbl 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 “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”

#if ROUND_BODY=1
; Additional Tilt servo for 180� Deck
DeckTilt Con “2”
DeckTilt2 Con “8”
#endif
;--------------------------------------------------------------------
;-------------Variables
Index var Byte
#if ROUND_BODY=0
Index2 var Byte
#endif
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
#if ROUND_BODY=0
SteeringLimit var Byte
#endif

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
WFSSCCnt 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
#if ROUND_BODY=1
DeckTilt_Pulse var Word
#endif

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 SER_POUT, SER_MODE,"#", |
RRHH,RRHH2,“po-69 #”,RRHV,RRHV2,“po17 #”,RRK,RRK2,“po9 #”, |
MRHH,MRHH2,“po2 #”,MRHV,MRHV2,“po0 #”,MRK,MRK2,“po0 #”, |
FRHH,FRHH2,“po-28 #”,FRHV,FRHV2,“po0 #”,FRK,FRK2,“po-18 #”, |
RLHH,RLHH2,“po-47 #”,RLHV,RLHV2,“po-21 #”,RLK,RLK2,“po35 #”, |
MLHH,MLHH2,“po-31 #”,MLHV,MLHV2,“po-40 #”,MLK,MLK2,“po62 #”, |
FLHH,FLHH2,“po-1 #”,FLHV,FLHV2,“po8 #”,FLK,FLK2,“po25”,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
	pause 144   ;  nap 3

	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
		pause 144   ;  nap 3
		for Index = 0 to 31
			serout SER_POUT, SER_MODE,"#",DEC Index,"P0"]
		next
		serout SER_POUT, SER_MODE,[13]
	else
		gosub H3Init
	endif
endif

if FlagOff then
	pause 144   ;  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 SER_POUT, SER_MODE,"#",MRHH,MRHH2,"P1800#",MRHV,MRHV2,"P1700#",MRK,MRK2,"P1700#", |
			MLHH,MLHH2,"P1200#",MLHV,MLHV2,"P1300#",MLK,MLK2,"P1300T288",13]
		;pause 288   ;  nap 4
		gosub WaitForSSC
		
		serout SER_POUT, SER_MODE,"#",MRHV,MRHV2,"P1500#",MRK,MRK2,"P1500#",MLHV,MLHV2,"P1500#", |
			MLK,MLK2,"P1500T288",13]
		;pause 288   ;  nap 4
		
		gosub WaitForSSC

#if ROUND_BODY=1
serout SER_POUT, SER_MODE,"#",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]
#else
serout SER_POUT, SER_MODE,"#",RRHV,RRHV2,“P1800#”,RRK,RRK2,“P1800#”,MRHV,MRHV2,“P1400#”, |
MRK,MRK2,“P1400#”,FRHH,FRHH2,“P2250#”,FRHV,FRHV2,“P2100#”,RLHV,RLHV2,“P1200#”, |
RLK,RLK2,“P1200#”,MLHV,MLHV2,“P1600#”,MLK,MLK2,“P1600#”,FLHH,FLHH2,“P750#”, |
FLHV,FLHV2,“P900T576”,13]

#endif
;pause 288 ; nap 4
for Index = 1 to 10
gosub WaitForSSC
serout SER_POUT, SER_MODE,"#",FRHV,FRHV2,“P2000#”,FRK,FRK2,“P1600#”,FLHV,FLHV2,“P900#”, |
FLK,FLK2,“P1600T144”,13]
;pause 144 ; nap 3

			gosub WaitForSSC
			serout SER_POUT, SER_MODE,"#",FRHV,FRHV2,"P2100#",FRK,FRK2,"P1400#",FLHV,FLHV2,"P1000#", |
				FLK,FLK2,"P1400T144",13]
			;pause 144   ;  nap 3
		next
		
		gosub WaitForSSC

#if ROUND_BODY=1
serout SER_POUT, SER_MODE,"#",FRHH,FRHH2,“P1500#”,FRK,FRK2,“P1500#”,FLHH,FLHH2,“P1500#”, |
FLHV,FLHV2,“P900#”,FLK,FLK2,“P1500T288”,13]
#else
serout SER_POUT, SER_MODE,"#",FRHH,FRHH2,“P1500#”,FRK,FRK2,“P1500#”,FLHH,FLHH2,“P1500#”, |
FLHV,FLHV2,“P900#”,FLK,FLK2,“P1500T576”,13]
#endif
;pause 288 ; nap 4

		gosub WaitForSSC
		serout SER_POUT, SER_MODE,"#",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   ;  nap 5
		
		gosub WaitForSSC
		serout SER_POUT, SER_MODE,"#",MRHH,MRHH2,"P1500#",MRHV,MRHV2,"P1700#",MRK,MRK2,"P1700#", |
			MLHH,MLHH2,"P1500#",MLHV,MLHV2,"P1300#",MLK,MLK2,"P1300T288",13]
		;pause 288   ;  nap 4
		
		gosub WaitForSSC
		serout SER_POUT, SER_MODE,"#",MRHV,MRHV2,"P1500#",MRK,MRK2,"P1500#",MLHV,MLHV2,"P1500#", |
			MLK,MLK2,"P1500T288",13]
		;pause 288   ;  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
		
			gosub WaitForSSC
			serout SER_POUT, SER_MODE,"#",MRHV,MRHV2,"P1900#",MRK,MRK2,"P1800#",MLHV,MLHV2,"P1100#", |
				MLK,MLK2,"P1200T288",13]
		
			gosub WaitForSSC
			serout SER_POUT, SER_MODE,"#",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   ;  nap 4
		
			gosub WaitForSSC
			serout SER_POUT, SER_MODE,"#",MRK,MRK2,"P1200#",MLK,MLK2,"P1800T288",13]
			;pause 288   ;  nap 4
		
			gosub WaitForSSC
			serout SER_POUT, SER_MODE,"#",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   ;  nap 5
		next
		
		gosub WaitForSSC
		serout SER_POUT, SER_MODE,"#",MRHV,MRHV2,"P1800#",MRK,MRK2,"P1800#",MLHV,MLHV2,"P1200#", |
			MLK,MLK2,"P1200T288",13]
		;pause 288   ;  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

#if ROUND_BODY=1
XSpeed = DualShock(4) - 128 ; Right Stick Vertical
#else
if (YSpeed < 0) then ; **** due to bug in Basic ****
YSpeed = -((-YSpeed)3/5)
else
YSpeed = YSpeed3/5
endif

XSpeed = (DualShock(4) - 128)   ; Right Stick Vertical 
if (XSpeed < 0) then   ; **** due to bug in Basic **** 
	XSpeed = -((-XSpeed)*4/5) 
else 
	XSpeed = XSpeed*4/5 
endif

#endif

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) & 0x7fff
	if DCoord = 0 then
		TmpCos = 0
	else
		TmpCos = XSpeed * 127 / DCoord
	endif
	
	gosub ACos
	DAngle = TmpAngle
	if YSpeed > 0 then
		DAngle = 256 - DAngle
	endif

	DCoord = DCoord max (128 - ABS(Height) - ABS(TibiaAngle * 2))

#if ROUND_BODY=1
; 180� Deck Code
if (Dangle < 64 or Dangle > 191) and (DCoord > 0) then
DeckTilt_Pulse = 1680 - Dcoord
else
DeckTilt_Pulse = 1380 + Dcoord
endif
#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

#if ROUND_BODY=1
HeightShift(0) = Roll/10
#else
HeightShift(0) = Roll/5
#endif
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)

#if ROUND_BODY=1
HeightShift(1) = HeightShift(0) + HeightShift(2)
HeightShift(4) = HeightShift(3) + HeightShift(5)
#else
HeightShift(1) = (HeightShift(0) + HeightShift(2))/2
HeightShift(4) = (HeightShift(3) + HeightShift(5))/2
#endif
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

#if ROUND_BODY=1
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
#else
SteeringLimit = 128 - ((ABS(XSpeed) + ABS(YSpeed))/2)
Steering = (Steering max SteeringLimit) min -SteeringLimit

   	if (Steering < 0) then 
    	Steering = -((-Steering) / 5)   ; **** due to bug in Basic **** 
   	else 
      	Steering = Steering / 5 
   	endif 
    
   	for Index2 = 0 to 1 
      	for Index = 0 to 2 
         	XPos2(Index + Index2*3) = -(DCoord * COS(DAngle + 64 + Index2*128) / 300) - ((Index - 1) * Steering) 
         	ZPos2(Index + Index2*3) = -(DCoord * SIN(DAngle + 64 + Index2*128) / 300) 
      	next 
	next

#endif
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) & 0x7fff )* 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)  & 0x7fff

		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

		
	gosub WaitForSSC
	serout SER_POUT, SER_MODE,"#",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), |

#if ROUND_BODY=1
“#”,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
“#”,DeckP,DeckP2,“P”,DEC Deck_Pulse,"#",LGripOC,LGripOC2,“P”,DEC LittleGripOCPulse, |
“#”,LGripLR,LGripLR2,“P”,DEC LittleGripLRPulse,“T190”,13]
#endif

else
	pause 144   ;  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
#if ROUND_BODY=1
for Index = 0 to 27 ;preserve servo 28 (Deck Tilt),29 and 30 (Little Gripper),31 (Deck)
#else
for Index = 0 to 27 ;preserve servo 28 (free),29 and 30 (Little Gripper),31 (Deck)
#endif
serout SER_POUT, SER_MODE,"#",DEC Index,“P1500”]
next
serout SER_POUT, SER_MODE,“T576”,13]

pause 576   ;  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

gosub WaitForSSC
serout SER_POUT, SER_MODE,"#",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   ;  nap 5
		
gosub WaitForSSC
serout SER_POUT, SER_MODE,"#",RRHH,RRHH2,"P1500#",MRHH,MRHH2,"P1500#",FRHH,FRHH2,"P1500#", |
	RLHH,RLHH2,"P1500#",MLHH,MLHH2,"P1500#",FLHH,FLHH2,"P1500T576",13]
	

return

;-------------Sub Arc Cosinus
ACos
TmpCos = (TmpCos max 127) min -127
if TmpCos < 0 then
TmpAngle = 127 - acosTbl(-TmpCos)
else
TmpAngle = acosTbl(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

#if ROUND_BODY=1
Rotate(Index) = Rotate(Index) + (Steering/5 - Rotate(Index))/StepFlag
#else
Rotate(Index) = Rotate(Index) + (Steering/2 - Rotate(Index))/StepFlag
#endif
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

#if ROUND_BODY=1
Rotate(Index) = Rotate(Index) + (-Steering/5 - Rotate(Index))/StepFlag
#else
Rotate(Index) = Rotate(Index) + (-Steering/2 - Rotate(Index))/StepFlag
#endif
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

;--------------- Simple function to try to see if the SSC32 has completed its previous move
sscBusy var byte

WaitForSSC
WFSSCCnt = 0
WFSSCLoop
; output a Q command if the SSC32 thinks it is done.
serout SER_POUT, SER_MODE,“Q”,13]

serin SER_PIN, SER_MODE, [sscBusy]

if sscBusy = "+" then
	pause 5
	WFSSCCnt = WFSSCCnt + 1
	if WFSSCCnt < 255 then		; Build in some recovery if they stop talking...
		goto WFSSCLoop
	endif
endif

return

;--------------------------------------------------------------------
[/code]

2

Hooray for Kurte!!! Thanks buddy!!! :smiley:

3

You are welcome :smiley:

4

Does this work with the standard P8 -> SSC32 connection or are different/more connections required for this work?

5

this coded is generally for the ps2 controller interface to the atom pro with the atom pro connected to the ssc-32

Although this code defaults the serout and serin to specific pins, you can change them in the “constant” section to redefine it to whatever you wish.

I personally removed/replaced the serin/serout command with hserin/hserout command which is on P14 and P15 of the Atom Pro

A little modification of the code is necessary to “inject” coordinates into the code since the PS2 controller “injects” the control for the H3-R.

6

I haven’t had any time to clean up the code further, but below is the cut and paste of the gutted-code that is edited from Kurte’s modded code to run on the Atom Pro 28.

I am running SSC-32 v1, with the Atom Pro 28 on ABB v1:

#include "servo_def.bas"

enablehserial
sethserial h38400,h8databits,hnoparity,h1stopbits

;-------------Constants

;Deck
Deck_PulseMin con 600
Deck_PulseMax con 2400  

;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 0         ;range is -20 (interior) to 20 (exterior), 0 is vertical, this is 'all time' tibia angle

;ACos
acosTbl 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 "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"

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
WFSSCCnt 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

HeightLimit = 25 - ABS(TibiaAngle / 2)

pause 500

gosub H3Init
Sound 9,[100\4235, 200\4435]
;--------------------------------------------------------------------
;-------------Main loop
main

;>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
	; ***************
	; Steering (Rotation):
	; ***************
	; Range 120 (Right) to -120 (Left)
	; 
	; ***************
	; XSpeed (translation):
	; ***************
	; Range -120 (Forward) to 120 (Backward) (translation)
	;
	; ***************
	; YSpeed (translation):
	; ***************
	; Range 120 (Right) to -120 (Left)
	;
	; ***************
	; Height :
	; ***************
	; Range 25 (Body Down) to -25 (Body Up)
	; 
	; ***************
	; LegUpShift :
	; ***************
	; Range 20 (Close to Floor) to 70 (About 2.5 inches from floor)
	; if LegUpShift > 45 the tripod Gait is modified to allow extra vertical move and the GaitSpeed is limited to 8-4 range
	; if LegUpShift is close to 70 it's recommended to use a 8-5 range for GaitSpeed 
	;	(a too fast gait loop won't allow extra vertical move)
	; If LegUpShift > 45, you'll have to move the Body Higher 
	;	(Setting Height to -15 or -20 for example) to allow extra vertical move
	;
	; ***************
	; GaitSpeed :
	; ***************
	; range 20 (Slow gait loop) to 3 (Fast gait loop)
	; Note, gaitSpeed is limited to 20-4 range if LegUpShift > 45
	;
	; ***************
	; LockLegs	
	; ***************
	; LockLegs = 0 => normal walk
	; LockLegs = 1 => body moves
	;
	; ***************
	; LeftStickMode	
	; ***************
	; if locklegs = 1 => body moves mode :
	; 	- LeftStickMode = 0 => Variable Steering and Height 
	;		are used for vertical axis rotation (Yaw) and translation (Height)
	;		---------------------------------------------
	; 	- LeftStickMode = 1 => Variable Steering and Height 
	;		are used for roll (right or left) and vertical axis translation (Height)
	;		---------------------------------------------
	; 	- LeftStickMode = 2 => Variable Steering and Height 
	;		are used for vertical axis rotation (Yaw) and pitch (forward or backward)
	;		---------------------------------------------
	; 	- LeftStickMode = 3 => Variable Steering and Height 
	;		are used for roll (right or left) and pitch (forward or backward)
	

Height = 0         ; Default preset
LockLegs = 0
LegUpShift = 35
GaitSpeed = 4	
Xspeed = -120
Yspeed = 0
FlipFlap = 0

Xspeed = Xspeed*4/5
Yspeed = Yspeed*3/5
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) & 0x7fff
      if DCoord = 0 then
         TmpCos = 0
      else
         TmpCos = XSpeed * 127 / DCoord
      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
         Distance = (SQR((XPos(Index) * XPos(Index) + ZPos(Index) * ZPos(Index)) / 4) & 0x7fff )* 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)  & 0x7fff

         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

         
       
      hserout"#",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), "T200",13]
        
      pause 200
       
   else
      pause 144   ;  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)
      hserout"#",DEC Index,"P1500"]
   next
   hserout"T576",13]

   pause 576   ;  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
         
   
   hserout"#",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   ;  nap 5
         
   
   hserout"#",RRHH,RRHH2,"P1500#",MRHH,MRHH2,"P1500#",FRHH,FRHH2,"P1500#", |
      RLHH,RLHH2,"P1500#",MLHH,MLHH2,"P1500#",FLHH,FLHH2,"P1500T576",13]
      

   return

;-------------Sub Arc Cosinus
ACos
   TmpCos = (TmpCos max 127) min -127
   if TmpCos < 0 then
      TmpAngle = 127 - acosTbl(-TmpCos)
   else
      TmpAngle = acosTbl(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

Ok, the explanation in this code is the variables that I’m setting or have previously referred to as “injecting” after the “main” section.

I am essentially “injecting” values into those variables in order to start the IK. One can easily take this code snippet and incorporate it and have it dynamically set the variables depending on telemetry.

Please note that this code is FAR from being cleaned up, there are EXTRANEOUS things that does NOTHING on my CH3-R setup.

I am currently trying to find the time to clean it up further, by re-organizing the code and renaming the variable to make more sense to me, as well as gutting out more sections that I deem extraneous for my setup. The next code snippet will probably be tailored to the Phoenix, as I think the Powerpod generated IK code is applicable to it as well, since each legs are still 60 degrees from each other, and it’s just a matter of changing the femur and tibia length.

Please enjoy and play with this code at your own discretion. Oh yeah, the code refers to the servodef.bas which defines my servo setup:

;servo_def.bas
;
;This file defines the servos, as well as their centering
;This file also defines many constants
;
;02/08/2008
;
;by Tom Chang

;----------------------------------------------------------------------------------------------------------------
;Constants generated by Powerpod

;Deck
Deck_PulseMin con 600
Deck_PulseMax con 2400
;Legs
;Hip Horizontal Max Min of Angle and Pulse
HipH_AngleMin con 21	;30°
HipH_AngleMax con 107	;150°
HipH_PulseMin con 910
HipH_PulseMax con 2090
;Hip Vertical Max Min of Angle and Pulse
HipV_AngleMin con 25	;35°
HipV_AngleMax con 103	;145°
HipV_PulseMin con 960
HipV_PulseMax con 2040
;Knee Max Min of Angle and Pulse
Knee_AngleMin con 36	;50°
Knee_AngleMax con 107	;150°
Knee_PulseMin con 1107
Knee_PulseMax con 2090
; 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 0		;range is -20 (interior) to 20 (exterior), 0 is vertical, this is 'all time' tibia angle
					
;end constants generated by powerpod
;----------------------------------------------------------------------------------------------------------------

; GROUP 1
; Right Front Hip Horizontal
RFHH_CH con 8		;Channel
RFHH_CTR con 1510	;Center
; Right Front Hip Vertical
RFHV_CH con 9		;Channel
RFHV_CTR con 1520	;Center
; Right Front Knee
RFK_CH con 10		;Channel
RFK_CTR con 1480	;Center

; GROUP 2
; Right Middle Hip Horizontal
RMHH_CH con 4		;Channel
RMHH_CTR con 1480	;Center
; Right Middle Hip Vertical
RMHV_CH con 5		;Channel
RMHV_CTR con 1470	;Center
; Right Middle Knee
RMK_CH con 6		;Channel
RMK_CTR con 1500	;Center

; GROUP 3
; Right Back Hip Horizontal
RBHH_CH con 0		;Channel
RBHH_CTR con 1630	;Center
; Right Back Hip Vertical
RBHV_CH con 1		;Channel
RBHV_CTR con 1580	;Center
; Right Back Knee
RBK_CH con 2		;Channel
RBK_CTR con 1480	;Center

; GROUP 4
; Left Back Hip Horizontal
LBHH_CH con 16		;Channel
LBHH_CTR con 1450	;Center
; Left Back Hip Vertical
LBHV_CH con 17		;Channel
LBHV_CTR con 1570	;Center
; Left Back Knee
LBK_CH con 18		;Channel
LBK_CTR con 1580	;Center

; GROUP 5
; Left Middle Hip Horizontal
LMHH_CH con 20		;Channel
LMHH_CTR con 1540	;Center
; Left Middle Hip Vertical
LMHV_CH con 21		;Channel
LMHV_CTR con 1420	;Center
; Left Middle Knee
LMK_CH con 22		;Channel
LMK_CTR con 1530	;Center

; GROUP 6
; Left Front Hip Horizontal
LFHH_CH con 24		;Channel
LFHH_CTR con 1500	;Center
; Left Front Hip Vertical
LFHV_CH con 25		;Channel
LFHV_CTR con 1480	;Center
; Left Front Knee
LFK_CH con 26		;Channel
LFK_CTR con 1560	;Center

The “servodef.bas” isn’t really necessary, since there are very little, if any, connection to the actual IK-code…

The servodef.bas is used as a reminder for myself, and will be incorporated as part of the on-going cleanup effort that I am applying to this code…

7

hi guys

i copied kerts program and loaded it into the atompro 8017 program, i then downloaded that via program into my atom pro conected to an ssc32 ver2. it wrote just fine and appeared to verify fine. but then i have no response from the phoenix hexapod i have lynx ps2 controler and when i power up the blinkng light on the ps2 reciever stops and goes steady, leving me to believe the controler and reviever are talking. i have the ssc set to 38.4 baud rate. have followed the cable conections via the build 99c tut.

so what am i doing wrong? :frowning:

i tried toms code and keep getting errors something about the hsin/hsout

so went back to kurtes which loaded fine.

i tried to do a debug and things ran fine untill it got to the clear at the program lines then it just stops??

and that is where i am at this point???

i am also using the version 2 ssc 32

larry in alaska

8

Well, you probably want to try it with kurte’s code, since mine have certain portions of the PS2 controller sections ripped out (I don’t use the ps2 controller).

I don’t have any experience of interfacing the ps2 controller with the atom pro, but first, have you looked at the H3-r tutorial that’s on the .com?

Second, check out this thread in the atom pro section:

lynxmotion.net/phpbb/viewtopic.php?t=159

The Atom Pro requires some pullup resistors I believe (probably due to an open collector/drain).

Hopefully, when kurte gets back online, he can help you. I know that he was able to get the code up and running…

9

I am online…

I probably need more information to help out much. For example I know you are using an Atom Pro 28 from you description and that you are using the Atom Pro IDE 8.0.1.7 which is good. Since you are mentioning build 99C tutorial I am assuming you are using the newer Bot Board 2. In this case I believe that the pull-up resistors are allready built in to the board. You will probably need to remove the jumpers for the Leds/switches as shown on BB2 tutorial Page 3.

Also my code was setup to have the communications between the the BB2 and SSC32 using both input and output pins and I was using a different IO pin then what is now standard in the tutorial.

So if you were wired up like the tutorial and have P8 connected to the SSC-32, then you should change the code:
SER_POUT con P8 ; Was P11 in my code.

Also my code relies on being able to query the SSC-32 to see if it completed the previous command. This requires a second wire between the two boards (From the TX pin on the SSC-32 to another IO pin on the BB2). Once you wire this, you should set:
SER_PIN con P?? ; To the IO pin you just put in.

I hope this helps.

Kurt

10

thanks kurte

i now have my pod standing and he will do the atack mode thing but when i try and tell it to walk foward it makes a couple of individual leg movements then goes dead, droping flat to the floor. the lights are still on but there is absolutly no more control from the ps2 controler. turn the bot off and back on and it recycles and is ready to do something.

so where might i be needing to look for this problem.

we are making headway with this puppy, errrrrr spidiething. :laughing:

11

Just guessing, but maybe battery problem. Do you have the same battery running the Servos as is doing the logic? You might be getting a brown out on the logic. Try running the SSC32 and AtomPro logic sections off a a 9v battery and see if it gets better.

Kurt

12

This is the classic signs of the SSC-32 resetting due to the processor browning out. You are probably sharing power for the VL from the servo supply. Lift the robot off the floor and suspend it so the legs don’t touch the ground and see if it will walk without flaking out. If so that clinches it.

There are many ways to fix this. What type of battery are you using now?

If I knew more about your power delivery system I could answer this easily. But because I don’t I will have to type many more words. :frowning:

Remove the VS1=VL jumper and install the 9vdc battery clip into the VL terminal of the SSC-32. This isolates the two supplies. So even if the servos draw too much current for the battery the logic in the servo controller will remain operational. The servos will not be effected much.

Tell me more about the batteries and wires etc and I can tell you more.

13

Hi guys, I know this is a long shot but I’ll ask anyway.
Does anyone happen to have a modified serial version of the powerpod generated code for the basic atom pro 28 ?

14

I don’t know if anyone has done it completely. But it would not be hard.

Simply run powerpod twice to output two files. One for PS2 control and one for serial. Then do a difference between the two files. (One of my old editors VS.EXE has a diff function for example). Then simply apply these changes to Atom Pro version that I posted here earlier.

The majority of the work will be to simply remove a bunch of lines and add a serin at the beginning of the loop. You will need to modifiy the serin command to the Atom Pro syntax…

Good Luck
Kurt

15

Hi Kurte

Thanks for the reply but I think that this is way over my head. :open_mouth:
I sure hope that the new powerpod for atom pro gets finished soon. :wink:

16

I make no promises here, but here is a quick edit. I think I hit most everything except for a couple of lines of timing that appeared to be slightly different between the two.

As always your millage may vary

Serial control version

[code]; Basic Atom Pro hacked up version of Powerpod for serial control

;************************************************
;*** Basic Atom with SSC-32 and PS2 DualShock ***
;----------------- 1.36 3DOF-C ------------------
;--------------- PS2 Controller -----------------
ROUND_BODY con 1
#if ROUND_BODY=1
;-------------- Round Body (H3-R) ---------------
#else
;-------------- Inline Body (H3) ----------------
#endif
;********** 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
#if ROUND_BODY=1
; 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
#endif
;
; Start button toggles between the ‘knee angle shift’ and ‘normal’ mode
;
; R3 button : Horn
;
;*********************************************
;
;
;--------------------------------------------------------------------
;-------------Constants
DeadZone con 8

; serial port
SER_POUT con P11
SER_PIN con P10
SER_MODE con i38400

;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 0 ;range is -20 (interior) to 20 (exterior), 0 is vertical, this is ‘all time’ tibia angle

;ACos
acosTbl 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 “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”

#if ROUND_BODY=1
; Additional Tilt servo for 180? Deck
DeckTilt Con “2”
DeckTilt2 Con “8”
#endif
;--------------------------------------------------------------------
;-------------Variables
Index var Byte
#if ROUND_BODY=0
Index2 var Byte
#endif
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
#if ROUND_BODY=0
SteeringLimit var Byte
#endif

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
WFSSCCnt 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
#if ROUND_BODY=1
DeckTilt_Pulse var Word
#endif

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 SER_POUT, SER_MODE,"#", |
RRHH,RRHH2,“po-69 #”,RRHV,RRHV2,“po17 #”,RRK,RRK2,“po9 #”, |
MRHH,MRHH2,“po2 #”,MRHV,MRHV2,“po0 #”,MRK,MRK2,“po0 #”, |
FRHH,FRHH2,“po-28 #”,FRHV,FRHV2,“po0 #”,FRK,FRK2,“po-18 #”, |
RLHH,RLHH2,“po-47 #”,RLHV,RLHV2,“po-21 #”,RLK,RLK2,“po35 #”, |
MLHH,MLHH2,“po-31 #”,MLHV,MLHV2,“po-40 #”,MLK,MLK2,“po62 #”, |
FLHH,FLHH2,“po-1 #”,FLHV,FLHV2,“po8 #”,FLK,FLK2,“po25”,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,100,NoData,[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 ;Triangle Button test
FlagOff = FlagOff ^ 1
if FlagOff then
gosub All1500
Gosub InitPos
pause 144 ; nap 3
for Index = 0 to 31
serout SER_POUT, SER_MODE,"#",DEC Index,“P0”]
next
serout SER_POUT, SER_MODE,[13]
else
gosub H3Init
endif
endif

if FlagOff then
pause 144 ; 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 SER_POUT, SER_MODE,"#",MRHH,MRHH2,"P1800#",MRHV,MRHV2,"P1700#",MRK,MRK2,"P1700#", | 
        MLHH,MLHH2,"P1200#",MLHV,MLHV2,"P1300#",MLK,MLK2,"P1300T288",13] 
     ;pause 288   ;  nap 4 
     gosub WaitForSSC 
      
     serout SER_POUT, SER_MODE,"#",MRHV,MRHV2,"P1500#",MRK,MRK2,"P1500#",MLHV,MLHV2,"P1500#", | 
        MLK,MLK2,"P1500T288",13] 
     ;pause 288   ;  nap 4 
      
     gosub WaitForSSC

#if ROUND_BODY=1
serout SER_POUT, SER_MODE,"#",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]
#else
serout SER_POUT, SER_MODE,"#",RRHV,RRHV2,“P1800#”,RRK,RRK2,“P1800#”,MRHV,MRHV2,“P1400#”, |
MRK,MRK2,“P1400#”,FRHH,FRHH2,“P2250#”,FRHV,FRHV2,“P2100#”,RLHV,RLHV2,“P1200#”, |
RLK,RLK2,“P1200#”,MLHV,MLHV2,“P1600#”,MLK,MLK2,“P1600#”,FLHH,FLHH2,“P750#”, |
FLHV,FLHV2,“P900T576”,13]

#endif
;pause 288 ; nap 4
for Index = 1 to 10
gosub WaitForSSC
serout SER_POUT, SER_MODE,"#",FRHV,FRHV2,“P2000#”,FRK,FRK2,“P1600#”,FLHV,FLHV2,“P900#”, |
FLK,FLK2,“P1600T144”,13]
;pause 144 ; nap 3

        gosub WaitForSSC 
        serout SER_POUT, SER_MODE,"#",FRHV,FRHV2,"P2100#",FRK,FRK2,"P1400#",FLHV,FLHV2,"P1000#", | 
           FLK,FLK2,"P1400T144",13] 
        ;pause 144   ;  nap 3 
     next 
      
     gosub WaitForSSC

#if ROUND_BODY=1
serout SER_POUT, SER_MODE,"#",FRHH,FRHH2,“P1500#”,FRK,FRK2,“P1500#”,FLHH,FLHH2,“P1500#”, |
FLHV,FLHV2,“P900#”,FLK,FLK2,“P1500T288”,13]
#else
serout SER_POUT, SER_MODE,"#",FRHH,FRHH2,“P1500#”,FRK,FRK2,“P1500#”,FLHH,FLHH2,“P1500#”, |
FLHV,FLHV2,“P900#”,FLK,FLK2,“P1500T576”,13]
#endif
;pause 288 ; nap 4

     gosub WaitForSSC 
     serout SER_POUT, SER_MODE,"#",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   ;  nap 5 
      
     gosub WaitForSSC 
     serout SER_POUT, SER_MODE,"#",MRHH,MRHH2,"P1500#",MRHV,MRHV2,"P1700#",MRK,MRK2,"P1700#", | 
        MLHH,MLHH2,"P1500#",MLHV,MLHV2,"P1300#",MLK,MLK2,"P1300T288",13] 
     ;pause 288   ;  nap 4 
      
     gosub WaitForSSC 
     serout SER_POUT, SER_MODE,"#",MRHV,MRHV2,"P1500#",MRK,MRK2,"P1500#",MLHV,MLHV2,"P1500#", | 
        MLK,MLK2,"P1500T288",13] 
     ;pause 288   ;  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 
      
        gosub WaitForSSC 
        serout SER_POUT, SER_MODE,"#",MRHV,MRHV2,"P1900#",MRK,MRK2,"P1800#",MLHV,MLHV2,"P1100#", | 
           MLK,MLK2,"P1200T288",13] 
      
        gosub WaitForSSC 
        serout SER_POUT, SER_MODE,"#",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   ;  nap 4 
      
        gosub WaitForSSC 
        serout SER_POUT, SER_MODE,"#",MRK,MRK2,"P1200#",MLK,MLK2,"P1800T288",13] 
        ;pause 288   ;  nap 4 
      
        gosub WaitForSSC 
        serout SER_POUT, SER_MODE,"#",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   ;  nap 5 
     next 
      
     gosub WaitForSSC 
     serout SER_POUT, SER_MODE,"#",MRHV,MRHV2,"P1800#",MRK,MRK2,"P1800#",MLHV,MLHV2,"P1200#", | 
        MLK,MLK2,"P1200T288",13] 
     ;pause 288   ;  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
#if ROUND_BODY=1
XSpeed = DualShock(4) - 128 ; Right Stick Vertical
#else
if (YSpeed < 0) then ; **** due to bug in Basic ****
YSpeed = -((-YSpeed)3/5)
else
YSpeed = YSpeed3/5
endif

XSpeed = (DualShock(4) - 128)   ; Right Stick Vertical 
if (XSpeed < 0) then   ; **** due to bug in Basic **** 
   XSpeed = -((-XSpeed)*4/5) 
else 
   XSpeed = XSpeed*4/5 
endif

#endif

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) & 0x7fff
if DCoord = 0 then
TmpCos = 0
else
TmpCos = XSpeed * 127 / DCoord
endif

  gosub ACos 
  DAngle = TmpAngle 
  if YSpeed > 0 then 
     DAngle = 256 - DAngle 
  endif 

  DCoord = DCoord max (128 - ABS(Height) - ABS(TibiaAngle * 2))

#if ROUND_BODY=1
; 180? Deck Code
if (Dangle < 64 or Dangle > 191) and (DCoord > 0) then
DeckTilt_Pulse = 1680 - Dcoord
else
DeckTilt_Pulse = 1380 + Dcoord
endif
#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

#if ROUND_BODY=1
HeightShift(0) = Roll/10
#else
HeightShift(0) = Roll/5
#endif
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)

#if ROUND_BODY=1
HeightShift(1) = HeightShift(0) + HeightShift(2)
HeightShift(4) = HeightShift(3) + HeightShift(5)
#else
HeightShift(1) = (HeightShift(0) + HeightShift(2))/2
HeightShift(4) = (HeightShift(3) + HeightShift(5))/2
#endif
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

#if ROUND_BODY=1
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
#else
SteeringLimit = 128 - ((ABS(XSpeed) + ABS(YSpeed))/2)
Steering = (Steering max SteeringLimit) min -SteeringLimit

      if (Steering < 0) then 
       Steering = -((-Steering) / 5)   ; **** due to bug in Basic **** 
      else 
         Steering = Steering / 5 
      endif 
    
      for Index2 = 0 to 1 
         for Index = 0 to 2 
            XPos2(Index + Index2*3) = -(DCoord * COS(DAngle + 64 + Index2*128) / 300) - ((Index - 1) * Steering) 
            ZPos2(Index + Index2*3) = -(DCoord * SIN(DAngle + 64 + Index2*128) / 300) 
         next 
  next

#endif
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) & 0x7fff )* 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)  & 0x7fff 

     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 

      
  gosub WaitForSSC 
  serout SER_POUT, SER_MODE,"#",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), |

#if ROUND_BODY=1
“#”,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
“#”,DeckP,DeckP2,“P”,DEC Deck_Pulse,"#",LGripOC,LGripOC2,“P”,DEC LittleGripOCPulse, |
“#”,LGripLR,LGripLR2,“P”,DEC LittleGripLRPulse,“T190”,13]
#endif

else
pause 144 ; 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
#if ROUND_BODY=1
for Index = 0 to 27 ;preserve servo 28 (Deck Tilt),29 and 30 (Little Gripper),31 (Deck)
#else
for Index = 0 to 27 ;preserve servo 28 (free),29 and 30 (Little Gripper),31 (Deck)
#endif
serout SER_POUT, SER_MODE,"#",DEC Index,“P1500”]
next
serout SER_POUT, SER_MODE,“T576”,13]

pause 576 ; 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

gosub WaitForSSC
serout SER_POUT, SER_MODE,"#",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 ; nap 5

gosub WaitForSSC
serout SER_POUT, SER_MODE,"#",RRHH,RRHH2,“P1500#”,MRHH,MRHH2,“P1500#”,FRHH,FRHH2,“P1500#”, |
RLHH,RLHH2,“P1500#”,MLHH,MLHH2,“P1500#”,FLHH,FLHH2,“P1500T576”,13]

return

;-------------Sub Arc Cosinus
ACos
TmpCos = (TmpCos max 127) min -127
if TmpCos < 0 then
TmpAngle = 127 - acosTbl(-TmpCos)
else
TmpAngle = acosTbl(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

#if ROUND_BODY=1
Rotate(Index) = Rotate(Index) + (Steering/5 - Rotate(Index))/StepFlag
#else
Rotate(Index) = Rotate(Index) + (Steering/2 - Rotate(Index))/StepFlag
#endif
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

#if ROUND_BODY=1
Rotate(Index) = Rotate(Index) + (-Steering/5 - Rotate(Index))/StepFlag
#else
Rotate(Index) = Rotate(Index) + (-Steering/2 - Rotate(Index))/StepFlag
#endif
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

;--------------- Simple function to try to see if the SSC32 has completed its previous move
sscBusy var byte

WaitForSSC
WFSSCCnt = 0
WFSSCLoop
; output a Q command if the SSC32 thinks it is done.
serout SER_POUT, SER_MODE,“Q”,13]

serin SER_PIN, SER_MODE, [sscBusy]

if sscBusy = “+” then
pause 5
WFSSCCnt = WFSSCCnt + 1
if WFSSCCnt < 255 then ; Build in some recovery if they stop talking…
goto WFSSCLoop
endif
endif

return

;-------------------------------------------------------------------- [/code]

17

Like I said in a previous post, you guys are unbelievable. Thanks a million.:smiley:
I’ll try it out and let you know.