Hi guys,
I’m coming here in hopes of getting some guidance, as I seem to have jumped a bit further into the deep end than I had planned to…
I’m a hardware guy. I’m pretty good with electronics but I can’t program my way out of a wet paper bag. I’ve got an ARC-32 that I am trying to use to control a biped SCOUT kit.
I have downloaded Zenta’s biped arc-32 program (viewtopic.php?f=7&t=6342&start=90) but I’m having trouble with that too. I can’t seem to get it to do anything. I’ve checked that it is seeing input from my PS2 controller and it is (Ran a program that dumps the data to the terminal), but when I turn the bot on it won’t respond to any commands from the controller. I’m not sure what I’m missing here, hoping someone can help me out. Here’s the cfg and ps2 portions of the code:
[code];Project Lynxmotion Phoenix
;Description: Phoenix, configuration file.
; All Hardware connections (excl controls) and body dimensions
; are configurated in this file. Can be used with V2.0 and above
;Configuration version: V2.0
;Date: 06-10-2009
;Programmer: Jeroen Janssen (aka Xan)
; ARC32 Version: Kurt Eckhardt(kurte)
;
;Hardware setup: ABB2 with ATOM 28 Pro, SSC32 V2, (See further for connections)
;
;NEW IN V2.0
; - Conversion to Arc32
;
;--------------------------------------------------------------------
USEPS2 con 1 ; Do we use the PS2?$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$<—CHANGED
;USEXBEE con 0 ; and/or do we use XBEE?
UseReversedKnee con 1
;--------------------------------------------------------------------
;[BB2 PIN NUMBERS]
cEyesPin con P25
;--------------------------------------------------------------------
;[ARC32 PIN NUMBERS]
; Using group 1 and 3 for servos
; Some pins are changed!
#IFDEF UseReversedKnee$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$<—CHANGED PIN NUMBERS BELOW
cLHipYawPin con P15 ;Right Leg Hip Rotate
cLHipRollPin con P11 ;Right Leg Hip Sway
cLFemurPin con P8 ;Right Leg Femur
cLTibiaPin con P7 ;Right Leg Knee
cLAnklePitchPin con P3 ;Right Leg Ankle
cLAnkleRollPin con P0 ;Right Leg Ankle Sway
cRHipYawPin con P31 ;Left Leg Hip Rotate
cRHipRollPin con P27 ;Left Leg Hip Sway
cRFemurPin con P24 ;Left Leg Femur
cRTibiaPin con P23 ;Left Leg Knee
cRAnklePitchPin con P19 ;Left Leg Ankle
cRAnkleRollPin con P16 ;Left Leg Ankle Sway
#ELSE
cRHipYawPin con P0 ;Right Leg Hip Rotate
cRHipRollPin con P1 ;Right Leg Hip Sway
cRFemurPin con P7 ;Right Leg Femur
cRTibiaPin con P3 ;Right Leg Knee
cRAnklePitchPin con P2 ;Right Leg Ankle
cRAnkleRollPin con P5 ;Right Leg Ankle Sway
cLHipYawPin con P16 ;Left Leg Hip Rotate
cLHipRollPin con P17 ;Left Leg Hip Sway
cLFemurPin con P23 ;Left Leg Femur
cLTibiaPin con P20 ;Left Leg Knee
cLAnklePitchPin con P18 ;Left Leg Ankle
cLAnkleRollPin con P19 ;Left Leg Ankle Sway
#ENDIF
cCogShifterpin con P6
;
cSound con P30 ; lets try output sound on plug in speaker
cStatusLED con P44
cBattLED con P45
;--------------------------------------------------------------------
;[MIN/MAX ANGLES]
cRHipYawMin1 con -900 ;Mechanical limits of the Right Leg, decimals = 1
cRHipYawMax1 con 900
cRHipRollMin1 con -900
cRHipRollMax1 con 900
cRFemurMin1 con -900
cRFemurMax1 con 900
cRTibiaMin1 con -900
cRTibiaMax1 con 900
cRAnklePitchMin1 con -900
cRAnklePitchMax1 con 900
cRAnkleRollMin1 con -900
cRAnkleRollMax1 con 900
cLHipYawMin1 con -900 ;Mechanical limits of the Right Leg, decimals = 1
cLHipYawMax1 con 900
cLHipRollMin1 con -900
cLHipRollMax1 con 900
cLFemurMin1 con -900
cLFemurMax1 con 900
cLTibiaMin1 con -900
cLTibiaMax1 con 900
cLAnklePitchMin1 con -900
cLAnklePitchMax1 con 900
cLAnkleRollMin1 con -900
cLAnkleRollMax1 con 900
;--------------------------------------------------------------------
;[BODY DIMENSIONS]
cHipVertLength con 29
cCoxaLength con 29 ;Length of the Coxa [mm]
cFemurLength con 75 ;Length of the Femur [mm]
cTibiaLength con 70 ;Lenght of the Tibia [mm]
cROffsetX con -42 ;Distance X from center of the body to the Right Hip
cROffsetZ con 0 ;Distance Z from center of the body to the Right Hip
cLOffsetX con 42 ;Distance X from center of the body to the Left Hip
cLOffsetZ con 0 ;Distance Z from center of the body to the Left Hip
;--------------------------------------------------------------------
;[Joint offsets]
cFemurOffset con 450
cAnklePitchOffset con 300
;--------------------------------------------------------------------
;[START POSITIONS FEET]
CHexInitY con 75 'temp
cRInitPosX con 0 ;Start positions of the Right leg
cRInitPosY con 75
cRInitPosZ con -10
cLInitPosX con 0 ;Start positions of the Left leg
cLInitPosY con 75
cLInitPosZ con -10
;--------------------------------------------------------------------
;[ICS, Internal Cog Shifter]
cSliderHalfMaxMinDeg con 245 ;Thats the 1/2 of max/min range for the slider servo = 24,5 deg
cSCBPX con 18 ;Division factor for calculating the SCBodyPosX
cMaxOppositeTravel con 20 ;A limitation for how far the oposite leg can travel while doing sidewalking
;(when walking sideways to the right the left leg is the oposite)[/code]
and
[code]
#ifdef USEPS2
;DEBUG_PS2 con 1
;Project Lynxmotion Phoenix
;Description: Phoenix, control file.
; The control input subroutine for the phoenix software is placed in this file.
; Can be used with V2.0 and above
;Configuration version: V1.0
;Date: 25-10-2009
;Programmer: Jeroen Janssen (aka Xan)
;
;Hardware setup: PS2 version
;
;NEW IN V1.0
; - First Release
;
; Walk method 1:
; - Left Stick Walk/Strafe
; - Right Stick Rotate
;
; Walk method 2:
; - Left Stick Disable
; - Right Stick Walk/Rotate
;
;
;PS2 CONTROLS:
; [Common Controls]
; - Start Turn on/off the bot
; - L1 Toggle Shift mode
; - L2 Toggle Rotate mode
; - Circle Toggle Single leg mode
; - Square Toggle Balance mode
; - Triangle Move body to 35 mm from the ground (walk pos)
; and back to the ground
; - D-Pad up Body up 10 mm
; - D-Pad down Body down 10 mm
; - D-Pad left decrease speed with 50mS
; - D-Pad right increase speed with 50mS
;
; [Walk Controls]
; - select Switch gaits
; - Left Stick (Walk mode 1) Walk/Strafe
; (Walk mode 2) Disable
; - Right Stick (Walk mode 1) Rotate,
; (Walk mode 2) Walk/Rotate
; - R1 Toggle Double gait travel speed
; - R2 Toggle Double gait travel length
;
; [Shift Controls]
; - Left Stick Shift body X/Z
; - Right Stick Shift body Y and rotate body Y
;
; [Rotate Controls]
; - Left Stick Rotate body X/Z
; - Right Stick Rotate body Y
;
; [Single leg Controls]
; - select Switch legs
; - Left Stick Move Leg X/Z (relative)
; - Right Stick Move Leg Y (absolute)
; - R2 Hold/release leg position
;
; [GP Player Controls]
; - select Switch Sequences
; - R2 Start Sequence
;
;====================================================================
;[CONSTANTS]
WalkMode con 0
TranslateMode con 1
RotateMode con 2
SingleLegMode con 3
GPPlayerMode con 4
;--------------------------------------------------------------------
;[PS2 Controller Constants]
PS2DAT con P40 ;PS2 Controller DAT (Brown)
PS2CMD con P41 ;PS2 controller CMD (Orange)
PS2SEL con P42 ;PS2 Controller SEL (Blue)
PS2CLK con P43 ;PS2 Controller CLK (White)
#if 1
PadMode con $73
#else
PadMode con $79
#endif
;--------------------------------------------------------------------
;[Ps2 Controller Variables]
DualShock var Byte(7)
LastButton var Byte(2)
DS2Mode var Byte
PS2Index var byte
BodyYOffset var sword
BodyYShift var sbyte
ControlMode var nib
DoubleHeightOn var bit
DoubleTravelOn var bit
WalkMethod var bit
#ifdef DEBUG_PS2
DSPrev var byte(7)
fDSChanged var bit
iDS var byte
#endif
;--------------------------------------------------------------------
;[InitController] Initialize the PS2 controller
; Warning: this version has been updated to allow both XBee and
; PS2 to control the robot… Which ever one issues a start
; will take over until it does a stop…
;--------------------------------------------------------------------
#ifdef USEXBEE
InitPS2Controller:
#else
InitController:
#endif
;PS2 controller
RetryPs2TestInit:
high PS2CLK
LastButton(0) = 255
LastButton(1) = 255
BodyYOffset = 0
BodyYShift = 0
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,$1\8]
shiftin PS2DAT,PS2CLK,FASTLSBPOST,[DS2Mode\8]
high PS2SEL
pause 1
if DS2Mode <> PadMode THEN
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,$1\8,$43\8,$0\8,$1\8,$0\8] ;CONFIG_MODE_ENTER
high PS2SEL
pause 1
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,$01\8,$44\8,$00\8,$01\8,$03\8,$00\8,$00\8,$00\8,$00\8] ;SET_MODE_AND_LOCK
high PS2SEL
pause 1
#if 0
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,$01\8,$4F\8,$00\8,$FF\8,$FF\8,$03\8,$00\8,$00\8,$00\8] ;SET_DS2_NATIVE_MODE
high PS2SEL
pause 1
#endif
low PS2SEL
shiftout PS2CMD,PS2CLK,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 PS2SEL
pause 1
#if 0
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,$01\8,$43\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8] ;CONFIG_MODE_EXIT
high PS2SEL
#endif
pause 1
sound cSOUND,[100\4000, 100\4500, 100\5000]
if (wDebugLevel and DBG_LVL_CONTROL) then
hserout "PS2: ", hex DS2Mode," ", hex PUCR5, 13];
endif
goto RetryPs2TestInit ;Check if the remote is initialized correctly
ENDIF
InputTimeDelay = 128 ;In PS2 mode I’m using this as a speedcontrol since Speedcontrol has another purpose in the gait section
return
;--------------------------------------------------------------------
;[ControlInput] reads the input data from the PS2 controller and processes the
;data to the parameters.
;
; Updated to handle both XBEE and PS2
;--------------------------------------------------------------------
#ifdef USEXBEE
ControlPS2Input:
#else
ControlInput:
#endif
; In case the controler gets out of whack double check to see if we are still in the right
high PS2CLK ;init clk state
low PS2SEL
shiftout PS2CMD,PS2CLK,LSBPRE,$1\8]
shiftin PS2DAT,PS2CLK,LSBPOST,[DS2Mode\8]
high PS2SEL
pause 1
if(DS2Mode <> PadMode)then
gosub RetryPs2TestInit ; use secondary name so does not depend on if XBee is also defined…
endif
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,$1\8,$42\8]
shiftin PS2DAT,PS2CLK,FASTLSBPOST,[DualShock(0)\8, DualShock(1)\8, DualShock(2)\8, DualShock(3)\8, |
DualShock(4)\8, DualShock(5)\8, DualShock(6)\8]
high PS2SEL
;pause 10
#ifdef DEBUG_PS2
if (wDebugLevel and DBG_LVL_CONTROL) then
fDSChanged = 0
for iDS = 0 to 6
if DualShock(iDS) <> DSPrev(iDS) then
DSPrev(iDS) = DualShock(iDS)
fDSChanged = 1
endif
next
if fDSChanged then
hserout "DS: ", hex dsPrev(0), " ",hex dsPrev(1), " ",hex dsPrev(2), " ",hex dsPrev(3), " ",|
hex dsPrev(4), " ",hex dsPrev(5), " ",hex dsPrev(6), 13]
endif
endif
#endif
; Switch bot on/off
IF (DualShock(1).bit3 = 0) and LastButton(0).bit3 THEN ;Start Button test
IF(HexOn) THEN
'Turn off
BodyPosX = 0
BodyPosY = 10
BodyPosZ = 0
BodyRotX1 = 0
BodyRotY1 = 0
BodyRotZ1 = 0
TravelLengthX = 0
TravelLengthZ = 0
TravelRotationY = 0
BodyYOffset = 0
BodyYShift = 0
SelectedLeg = 255
HexOn = 0
ELSE
'Turn on
HexOn = 1
ENDIF
ENDIF
IF HexOn THEN
;[SWITCH MODES]
;Translate mode
IF (DualShock(2).bit2 = 0) and LastButton(1).bit2 THEN ;L1 Button test
sound cSOUND, [50\4000]
IF ControlMode <> TranslateMode THEN
ControlMode = TranslateMode
ELSE
IF (SelectedLeg=255) THEN
ControlMode = WalkMode
ELSE
ControlMode = SingleLegMode
ENDIF
ENDIF
ENDIF
;Rotate mode
IF (DualShock(2).bit0 = 0) and LastButton(1).bit0 THEN ;L2 Button test
sound cSOUND, [50\4000]
IF ControlMode <> RotateMode THEN
ControlMode = RotateMode
ELSE
IF (SelectedLeg=255) THEN
ControlMode = WalkMode
ELSE
ControlMode = SingleLegMode
ENDIF
ENDIF
ENDIF
;Single leg mode
IF (DualShock(2).bit5 = 0) and LastButton(1).bit5 THEN ;Circle Button test
IF ABS(TravelLengthX)<cTravelDeadZone AND ABS(TravelLengthZ)<cTravelDeadZone AND ABS(TravelRotationY*2)<cTravelDeadZone THEN
Sound cSOUND,[50\4000]
IF (ControlMode <> SingleLegMode) THEN
ControlMode = SingleLegMode
IF (SelectedLeg = 255) THEN ;Select leg if none is selected
SelectedLeg=cRightLeg ;Startleg
ENDIF
ELSE
ControlMode = WalkMode
SelectedLeg=255
ENDIF
ENDIF
ENDIF
;GP Player mode
IF (DualShock(2).bit6 = 0) and LastButton(1).bit6 THEN ;Cross Button test
Sound cSOUND,[50\4000]
IF ControlMode <> GPPlayerMode THEN
ControlMode = GPPlayerMode
GPSeq=0
ELSE
ControlMode = WalkMode
ENDIF
ENDIF
;[Common functions]
;Switch Balance mode on/off
IF (DualShock(2).bit7 = 0) and LastButton(1).bit7 THEN ;Square Button test
BalanceMode = BalanceMode^1
IF BalanceMode THEN
sound cSOUND,[250\3000]
ELSE
sound cSOUND,[100\4000, 50\8000]
ENDIF
ENDIF
;Stand up, sit down
IF (DualShock(2).bit4 = 0) and LastButton(1).bit4 THEN ;Triangle Button test
IF (BodyYOffset>0) THEN
BodyYOffset = 0
ELSE
BodyYOffset = 35
ENDIF
ENDIF
IF (DualShock(1).bit4 = 0) and LastButton(0).bit4 THEN ;D-Up Button test
BodyYOffset = BodyYOffset+10
ENDIF
IF (DualShock(1).bit6 = 0) and LastButton(0).bit6 THEN ;D-Down Button test
BodyYOffset = BodyYOffset-10
ENDIF
IF (DualShock(1).bit5 = 0) and LastButton(0).bit5 THEN ;D-Right Button Increase speed
IF InputTimeDelay>25 THEN
InputTimeDelay = InputTimeDelay - 25
sound cSOUND, [50\4000]
ENDIF
ENDIF
IF (DualShock(1).bit7 = 0) and LastButton(0).bit7 THEN ;D-Left Button Decrease Speed
IF InputTimeDelay<255 THEN
InputTimeDelay = (InputTimeDelay + 25) MAX 255
sound cSOUND, [50\4000]
ENDIF
ENDIF
;[Walk functions]
IF (ControlMode=WALKMODE) THEN
;Switch gates
IF (DualShock(1).bit0 = 0) and LastButton(0).bit0 | ;Select Button test
AND ABS(TravelLengthX)<cTravelDeadZone | ;No movement
AND ABS(TravelLengthZ)<cTravelDeadZone |
AND ABS(TravelRotationY*2)<cTravelDeadZone THEN
IF GaitType<7 THEN
Sound cSOUND,[50\4000]
GaitType = 0;GaitType+1
ELSE
Sound cSOUND,[50\4000, 50\4500]
GaitType = 0
ENDIF
GOSUB GaitSelect
ENDIF
;Double leg lift height
IF (DualShock(2).bit3 = 0) and LastButton(1).bit3 THEN ;R1 Button test
sound cSOUND, [50\4000]
DoubleHeightOn = DoubleHeightOn^1
IF DoubleHeightOn THEN
LegLiftHeight = 80
ELSE
LegLiftHeight = 50
ENDIF
ENDIF
;Double Travel Length
IF (DualShock(2).bit1 = 0) and LastButton(1).bit1 THEN ;R2 Button test
sound cSOUND, [50\4000]
DoubleTravelOn = DoubleTravelOn^1
ENDIF
; Switch between Walk method 1 and Walk method 2
IF (DualShock(1).bit2 = 0) and LastButton(0).bit2 THEN ;R3 Button test
sound cSOUND, [50\4000]
WalkMethod = WalkMethod^1
ENDIF
;Walking
IF WalkMethod THEN ;(Walk Methode)
TravelLengthZ = (Dualshock(4) - 128)/2 ;Right Stick Up/Down
ELSE
TravelLengthX = -(Dualshock(5) - 128)/4
TravelLengthZ = (Dualshock(6) - 128)/2
ENDIF
IF DoubleTravelOn=0 THEN ;(Double travel length)
TravelLengthX = TravelLengthX/2
TravelLengthZ = TravelLengthZ/2
ENDIF
TravelRotationY = -(Dualshock(3) - 128)/6 ;Right Stick Left/Right
ENDIF
;[Translate functions]
;BodyYShift = 0
IF (ControlMode=TRANSLATEMODE) THEN
BodyPosX = (Dualshock(5) - 128)/2
BodyPosZ = -(Dualshock(6) - 128)/3
BodyRotY1 = (Dualshock(3) - 128)
BodyYShift = (-(Dualshock(4) - 128)/2)
ENDIF
;[Rotate functions]
IF (ControlMode=ROTATEMODE) THEN
BodyRotX1 = (Dualshock(6) - 128)
BodyRotY1 = (Dualshock(3) - 128)
BodyRotZ1 = (Dualshock(5) - 128)
BodyYShift = (-(Dualshock(4) - 128)/2)
ENDIF
;[Single leg functions]
IF (ControlMode=SINGLELEGMODE) THEN
;Switch leg for single leg control
IF (DualShock(1).bit0 = 0) and LastButton(0).bit0 THEN ;Select Button test
Sound cSOUND,[50\4000]
IF SelectedLeg=cRightLeg THEN
SelectedLeg = cLeftLeg
ELSE
SelectedLeg=cRightLeg
ENDIF
ENDIF
;Single Leg Mode
IF (ControlMode = SingleLegMode) THEN
SLLegX = (Dualshock(5) - 128)/2 ;Left Stick Right/Left
SLLegY = (Dualshock(4) - 128)/2 ;Right Stick Up/Down
SLLegZ = (Dualshock(6) - 128)/2 ;Left Stick Up/Down
ENDIF
; Hold single leg in place
IF (DualShock(2).bit1 = 0) and LastButton(1).bit1 THEN ;R2 Button test
sound cSOUND, [50\4000]
SLHold = SLHold^1
ENDIF
ENDIF
;[Single leg functions]
IF (ControlMode=GPPLAYERMODE) THEN
;Switch between sequences
IF (DualShock(1).bit0 = 0) and LastButton(0).bit0 THEN ;Select Button test
IF GPStart=0 THEN
IF GPSeq < 5 THEN ;Max sequence
sound cSOUND, [50\3000]
GPSeq = GPSeq+1
ELSE
Sound cSOUND,[50\4000, 50\4500]
GPSeq=0
ENDIF
ENDIF
ENDIF
;Start Sequence
IF (DualShock(2).bit1 = 0) and LastButton(1).bit1 THEN ;R2 Button test
GPStart=1
ENDIF
ENDIF
;Calculate walking time delay
;InputTimeDelay = 128 - (ABS((Dualshock(5) - 128)) MIN ABS((Dualshock(6) - 128))) MIN ABS((Dualshock(3) - 128))
ENDIF
;Calculate BodyPosY
BodyPosY = (BodyYOffset + BodyYShift)MIN 0
;Store previous state
LastButton(0) = DualShock(1)
LastButton(1) = DualShock(2)
return
;--------------------------------------------------------------------
#endif[/code]
Any help is appreciated.