Constant beeping

etwx · December 22, 2012, 8:46pm

After setting up my A-pod robot the botboard just continues to beep. I did the troubleshooting that the online manual point out but it still just beeps. this is my third robot, so I am some what familiar with the way it suppose to work. Does anyone have any other advice?

innerbreed · December 22, 2012, 11:34pm

I’d suggest checking your PS2 connections.

etwx · December 23, 2012, 12:05am

According to the online manual it’s connected okay. Unless something changed the one set of wires are on 12 thru 15 alone with the ground wire. The wires from the bot board in the tail are on pins 10 yellow black ground red pin 11.

innerbreed · December 23, 2012, 12:24am

Are there any LEDs on the receiver… what are they doing… flashing, steady, off?

Can you post a photo of the PS2 connections? This will help me identify the problem. Also check the code you are using to make sure the pin connections are defined correctly in that too.

etwx · December 23, 2012, 12:39am

On the receiver there is a green LED and a red LED. The green one is on constant and the red one is not on at all. I’m not near the robot at the moment to take a pic however I will post a pic tomorrow.

innerbreed · December 23, 2012, 1:01am

Ok yes a photo will help…
If the green light is on then you have power… so I can assume the two pin connection (red & black) on pin 15 are correct, with black wire facing to the outside of the board.

But I fear pins 12 through to 15 (signal) are backwards?
Did you use the (now redundant) cable adapter for your receiver or have you made up the connections using two servo extension cables, as mentioned in some tutorials.? If so check connections that end too.

If you hear a continuous beeping, the PS2 controller is not connected to the Atom properly. You can test the controller with a Playstation 2 to verify that it is working, but also check that the atom chip is properly inserted… all the way.
Usually if it’s not then the board will click, not beep, but it’s always worth checking…

etwx · December 23, 2012, 1:24am

The modified cable has three wires y b r. The controller end the TX is y and the RX is red ground is black. The bot board end connection is y & b on pin 10 and red on pin 11.

innerbreed · December 23, 2012, 1:52am

I’m not t walking about the bidirectional wire between the botboard and the SSC. This is not where your issue is.

I know you have the first PS2 addition. (V1)
I mean does your connection on the PS2 look like this…

If so check it with this…
lynxmotion.com/images/html/build162.htm

Or do you have the old connection (pre made).

etwx · December 23, 2012, 12:55pm

My ps2 controller don’t look like that one you showed me at all. The one I have is a black/greyish in color and is a two piece setup.

etwx · December 23, 2012, 1:06pm

also here is a pic of the bot board connection for the ps2

innerbreed · December 23, 2012, 1:51pm

Ok cool thanks for the photos. Can you post a link to the tutorial you used to setup and point me to the code you are using.
Your ps2 setup looks correct. I have heard of some issues with v1 receivers but id like to make sure the code is reading correctly.

etwx · December 23, 2012, 3:06pm

lynxmotion.com/images/html/build184.htm

This is the online information I used.

innerbreed · December 23, 2012, 6:52pm

my pc is giving me a lot of grief… if you could copy and paste just the PS2.bas code to a notepad file and attach that, it will save me having to install the Basic Micro Studio on a PC that keeps turning itself off.

etwx · December 23, 2012, 7:27pm

I think this is what you’re looking for.
Phoenix_Control_ps2.bas (23.1 KB)

innerbreed · December 23, 2012, 7:37pm

Yes, that’s the file but I cannot open it on my tablet as android haven’t yet licenced software for me to open it. I need you to copy and paste its contense to note pad or directly to this forum so I can view it. Sorry about this, but my current pc is about to gain another issue… fist!

etwx · December 23, 2012, 8:22pm

try this out!
;Project Lynxmotion A-Pod
;Description: A-Pod, control file.
; The control input subroutine for the phoenix software is placed in this file.
; Can be used with V2.1 and above
;Configuration version: V1.2
;Date: 09-01-2012
;Programmer: Jeroen Janssen (aka Xan)
;A-Pod code: Kåre Halvorsen (aka Zenta)
;
;Hardware setup: PS2 version
;
;NEW IN V1.1
; - added speaker constant
; - added variable for number of gaits in code
; - Changed BodyRot to 1 decimal percision
; - Added variable Center Point of Rotation for the body
;
; Walk method 1:
; - Left Stick Walk/Strafe
; - Right Stick Rotate
;
; Walk method 2:
; - Left Stick Disable
; - Right Stick Walk/Rotate
;
;NEW IN V1.2
; - A-Pod support
; - Extra control of Mandible/gripper w/FSR sensor, Head control, Abdomen/tail control
;
;PS2 CONTROLS:
; [Common Controls]
; - Start Turn on/off the bot
; - L1 A-Pod Mandibles/gripper Open (hold to move)
; - L2 A-Pod Mandibles/gripper Close (hold to move)
; - Circle Toggle Single leg mode, Removed from A-Pod
; - Square Toggle Balance mode
; - Triangle Move body to 35 mm from the ground (walk pos)
; and back to the ground
;- L2 + D-Pad left Decrease gripper torque
;- L2 + D-Pad right Increase gripper torque
; - D-Pad up Body up 10 mm
; - D-Pad down Body down 10 mm
; - R3 Toogle full/half head rotation range (hint push/click the right gimbal)
; - O (Circle) Toggle Rotate mode
; - X (Cross) Toggle Shift mode
;
; [Walk Controls]
; - select Switch gaits
; - Left Stick Walk/Strafe
;
; - Right Stick Rotate, body X rotate
;
; - D-Pad left decrease speed with 50mS (moved from common to walk only control)
; - D-Pad right increase speed with 50mS
; - R1 Toggle Double gait travel height
; - 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]
; - select Switch rotate function (Head tracking, fixed head, head only)
; - Left Stick L/R: Y rotate, U/D: Z Translate (Shift)
; - Right Stick L/R: Z rotate, U/D: X rotate
; - R1 Moves the Center Point of Rotation to the Head (Hold button)
; - R2 Moves the Center Point of Rotation to the Tail (Hold button)
; - D-Pad left Slower but more accurate indirect control
; - D-Pad right Faster respons on indirect control
; - L3 Reset body rotations, click the left gimbal for neutralizing the IndDualShock
;
;Not used on A-Pod:
; [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/Stop Sequence
;
;====================================================================
;[CONSTANTS]
WalkMode con 0
TranslateMode con 1
RotateMode con 2
SingleLegMode con 3
GPPlayerMode con 4
;--------------------------------------------------------------------
;[PS2 Controller Constants]
#ifndef PS2DAT ; Allow user to overwrite default values in config file
PS2DAT con P12 ;PS2 Controller DAT (Brown)
PS2CMD con P13 ;PS2 controller CMD (Orange)
PS2SEL con P14 ;PS2 Controller SEL (Blue)
PS2CLK con P15 ;PS2 Controller CLK (White)
#endif
PadMode con $79
;--------------------------------------------------------------------
;[Ps2 Controller Variables]
DualShock var Byte(7)
IndDualShock var sbyte(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
GPSpeedControl var bit
;--------------------------------------------------------------------
;[A-Pod variables]
CtrlMoveInp var sword ; Input for smooth control movement (ex joystick, pot value), using sword for beeing on the safe side
CtrlMoveOut var sword ;
CtrlDivider var nib ;

HeadRotOut1 var sword
HeadPanOut1 var sword
HeadTiltOut1 var sword
HeadRotInput1 var sword
HeadPanInput1 var sword
HeadTiltInput1 var sword
RotateFunction var nib
FullHeadRange var bit ;Set to 1 for full range on head rotation

;A-Pod mandible variables:
MandibleFSRInput var word
MandibleFSR_Activated var bit
RPOTSensePos var byte
RPOTCtrlPos var byte ; Controlled indirectly by the Circle and Cross button, inc/dec variable
TorqueLevel var word
TorqueSelect bytetable 0,64,128,192,255 ;Could be made much simpler, reusing stuff from DIY control
TorqueIndex var nib
MandibleOpen var bit
MandibleClose var bit

NeutralStick var bit(4) ;For use with indirect gimbal control, set high for reseting the gimbal. Controlled by L3.
IDSdivFactor var nib ;This factor determine if the indirect control is fast (low value) or accurate and slow (high value)

;--------------------------------------------------------------------
;[InitController] Initialize the PS2 controller
InitController:
;A-Pod init stuff:
IDSdivFactor = 7 ; Set Default value
RPOTCtrlPos = 50 ; Default Mandible position
TorqueIndex = 2
TorqueLevel = (cMandibleFSRminTorque + (TorqueSelect(TorqueIndex)*cTorqueMultiFactor2)/c2DEC) ;Middle torque value (128)
;—
#if PS2DAT = 0
PUCR5.bit0 = 1 ; Note these pull-ups may not be sufficient for all PS2 remotes.
#endif
#if PS2DAT = 16
PUCR1.bit1 = 0x1 ; 16 is on H8 P11 which has a pull-up
#endif

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

ReInitController:
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

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

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

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
pause 100
	
sound cSpeakerPin, [100\4000, 100\4500, 100\5000]

goto InitController ;Check if the remote is initialized correctly

ENDIF
return
;--------------------------------------------------------------------
;[ControlInput] reads the input data from the PS2 controller and processes the
;data to the parameters.
ControlInput:

; Check if the PS2 remote has timed out
low PS2SEL
shiftout PS2CMD,PS2CLK,LSBPRE,$1\8]
shiftin PS2DAT,PS2CLK,LSBPOST,[DS2Mode\8]
high PS2SEL
pause 1

if(DS2Mode <> PadMode)then
gosub ReInitController
endif

; Read controller input
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

; 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 = 0
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).bit6 = 0) and LastButton(1).bit6 THEN	;Cross Button, Not L1 as before
  sound cSpeakerPin, [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).bit5 = 0) and LastButton(1).bit5 THEN	;Circle Button, Not L2
  sound cSpeakerPin, [50\4000]
  IF ControlMode <> RotateMode THEN
    ControlMode = RotateMode
  ELSE
    IF (SelectedLeg=255) THEN
      ControlMode = WalkMode
    ELSE
      ControlMode = SingleLegMode
    ENDIF
  ENDIF
ENDIF

;Single leg mode, NOT USED ON A-Pod!

#ifndef cNoSL
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 cSpeakerPin, [50\4000]
IF (ControlMode <> SingleLegMode) THEN
ControlMode = SingleLegMode
IF (SelectedLeg = 255) THEN ;Select leg if none is selected
SelectedLeg=cRF ;Startleg
ENDIF
ELSE
ControlMode = WalkMode
SelectedLeg=255
ENDIF
ENDIF
ENDIF
#endif
;GP Player mode, NOT USED ON A-Pod!
#ifndef cNOGP
IF (DualShock(2).bit6 = 0) and LastButton(1).bit6 THEN ;Cross Button test
Sound cSpeakerPin, [50\4000]
IF ControlMode <> GPPlayerMode THEN
ControlMode = GPPlayerMode
GPSeq=0
ELSE
ControlMode = WalkMode
ENDIF
ENDIF
#endif

;[Common functions]
IF (DualShock(2).bit2 = 0)THEN; L1 Button , Mandible Open
RPOTCtrlPos = (RPOTCtrlPos + 5) MAX 255
ENDIF

IF (DualShock(2).bit0 = 0)THEN;L2 Button, Mandible Close
	IF NOT MandibleFSR_Activated THEN ; 
  		RPOTCtrlPos = (RPOTCtrlPos - 5) MIN 0; Don't decrease this value if FSR is activated
    ENDIF 
	
	;Code for adjusting the torque using the left/right D-Pad:
	IF (DualShock(1).bit5 = 0) and LastButton(0).bit5 THEN	;D-Right Button Increase torque
  	IF TorqueIndex<4 THEN
  		TorqueIndex = TorqueIndex +1
  		IF TorqueIndex=4 THEN
  		  sound cSpeakerPin, [40\6000]	;Just a little note that we have reached the limit
  		ELSE
  		  sound cSpeakerPin, [50\4000]
  		ENDIF
    	TorqueLevel = (cMandibleFSRminTorque + (TorqueSelect(TorqueIndex)*cTorqueMultiFactor2)/c2DEC)
  	ENDIF
	ENDIF
	
	IF (DualShock(1).bit7 = 0) and LastButton(0).bit7 THEN	;D-Left Button decrease torque
  	IF TorqueIndex>0 THEN
    	TorqueIndex = TorqueIndex -1
    	TorqueLevel = (cMandibleFSRminTorque + (TorqueSelect(TorqueIndex)*cTorqueMultiFactor2)/c2DEC)
    	IF TorqueIndex=0 THEN
  		  sound cSpeakerPin, [40\2000]	;Just a little note that we have reached the limit
  		ELSE
  		  sound cSpeakerPin, [50\4000]
  		ENDIF
  	ENDIF
	ENDIF	
ENDIF

GOSUB MandibleControl ; Call the mandible control sub

#IFDEF MonitorFSR
serout s_out, i57600, “RPOTCtrlPos:”, dec RPOTCtrlPos," MandRightPWM:", dec MandRightPWM," FSR: “,|
sdec MandibleFSRInput,” TorqueLevel = “, dec TorqueLevel, " TDF:”, dec cTorqueMultiFactor2, 13]
#ENDIF

;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 cSpeakerPin, [250\3000]	  
  ELSE	  
	sound cSpeakerPin, [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).bit2 = 0) and LastButton(0).bit2 THEN	;R3 Button, Toogle full/half head rotation range 
	FullHeadRange = ~FullHeadRange
    sound cSpeakerPin, [50\4000]
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<(NrOfGaits-1) THEN
	  Sound cSpeakerPin, [50\4000]
	  GaitType = GaitType+1
  	ELSE
	  Sound cSpeakerPin, [50\4000, 50\4500]
	  GaitType = 0
  	ENDIF
  	GOSUB GaitSelect					
  ENDIF
  
  ;Speed control:
  IF (DualShock(1).bit5 = 0) and LastButton(0).bit5 and NOT (DualShock(2).bit0 = 0) THEN	;D-Right Button test
  	IF SpeedControl>0 THEN
    	SpeedControl = SpeedControl - 50
    	sound cSpeakerPin, [50\4000]
  	ENDIF
  ENDIF	
	
	IF (DualShock(1).bit7 = 0) and LastButton(0).bit7 and NOT (DualShock(2).bit0 = 0) THEN	;D-Left Button test
  	IF SpeedControl<2000 THEN
    	SpeedControl = SpeedControl + 50
    	sound cSpeakerPin, [50\4000]	  
  	ENDIF
  ENDIF
  	
  ;Double leg lift height		
  IF (DualShock(2).bit3 = 0) and LastButton(1).bit3 THEN	;R1 Button test
    sound cSpeakerPin, [50\4000]
    DoubleHeightOn = DoubleHeightOn^1
    IF DoubleHeightOn THEN
      LegLiftHeight = 60; A-Pod	  
    ELSE	  	  
  	  LegLiftHeight = 30; Use lower on A-Pod
    ENDIF
  ENDIF
  	
  ;Double Travel Length
  IF (DualShock(2).bit1 = 0) and LastButton(1).bit1 THEN	;R2 Button test
    sound cSpeakerPin, [50\4000]
    DoubleTravelOn = DoubleTravelOn^1
  ENDIF
  
  
  	
  ;Walking	
  TravelLengthX = -(Dualshock(5) - 128)*2/3
  TravelLengthZ = (Dualshock(6) - 128)*2/3
  	  
  GOSUB SmoothControl -(Dualshock(3) - 128), BodyRotZ1, 2], BodyRotZ1
  GOSUB SmoothControl -(Dualshock(4) - 128), BodyRotX1, 2], BodyRotX1
 
  GOSUB HeadControl (Dualshock(3) - 128)/5, HeadPanInput1, -(Dualshock(4) - 128)] 
  HeadTiltAngle1 =  HeadTiltOut1
  HeadRotAngle1 =  HeadRotOut1
  
  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
;Switch Rotate Function
IF (DualShock(1).bit0 = 0) and LastButton(0).bit0 THEN ;Select Button test
IF RotateFunction<2 THEN
Sound cSpeakerPin, [50\4000]
RotateFunction = RotateFunction+1
ELSE
Sound cSpeakerPin, [50\4000, 50\4500]
RotateFunction = 0
ENDIF
ENDIF

  IF (DualShock(1).bit1 = 0) and LastButton(0).bit1 THEN	;L3 Button, Click the gimbal for neutralizing the IndDualShock 
    sound cSpeakerPin, [50\4000]
    NeutralStick(0) = 1
    NeutralStick(1) = 1	
    NeutralStick(2) = 1
    NeutralStick(3) = 1	  
  ENDIF
  ;Set the IDSdivFactor:
  IF (DualShock(1).bit5 = 0) and LastButton(0).bit5 and NOT (DualShock(2).bit0 = 0) THEN	;D-Right Button test
  	IF IDSdivFactor>4 THEN
    	IDSdivFactor = IDSdivFactor - 2						;Decrease value means faster indirect control
    	sound cSpeakerPin, [50\4000]
    ELSE
    	sound cSpeakerPin, [40\6000]	;Just a little note that we have already reached the limit					
  	ENDIF
  ENDIF	
	
	IF (DualShock(1).bit7 = 0) and LastButton(0).bit7 and NOT (DualShock(2).bit0 = 0) THEN	;D-Left Button test
  	IF IDSdivFactor<14 THEN
    	IDSdivFactor = IDSdivFactor + 2						;Increase value give a slower but more accurate indirect control
    	sound cSpeakerPin, [50\4000]
    ELSE
    	sound cSpeakerPin, [40\2000]	;Just a little note that we have already reached the limit	  
  	ENDIF
  ENDIF
  GOSUB IndirectDualShock
  GOSUB BranchRotateFunction
  ;BodyPosZ = -(Dualshock(6) - 128)/3
  BodyPosZ = -IndDualshock(6)/3
  ;BodyYShift = (-(Dualshock(4) - 128)/2)
  
  ;Shift Center Point of Rotation for the body
  IF(DualShock(2).bit3 = 0)THEN		;R1 Button, CP to the Head tilt joint
  	BodyRotOffsetZ = -165 
  ELSEIF(DualShock(2).bit1 = 0) 	;R2 Button, CP to the Abdomen/tail tilt joint
  	BodyRotOffsetZ = 165 
  ELSE
    BodyRotOffsetZ = 0
  ENDIF
  
ELSE
  BodyRotOffsetX = 0
  BodyRotOffsetY = 0
  BodyRotOffsetZ = 0
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 cSpeakerPin, [50\4000]
    IF SelectedLeg<5 THEN
      SelectedLeg = SelectedLeg+1
    ELSE
      SelectedLeg=0
    ENDIF
  ENDIF

  ;Single Leg Mode
  IF (ControlMode = SingleLegMode) THEN 
    SLLegX	= (Dualshock(5) - 128)/2 ;Left Stick Right/Left
    SLLegY	= (Dualshock(4) - 128)/10 ;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 cSpeakerPin, [50\4000]
    SLHold = SLHold^1
  ENDIF	  
ENDIF

#ifndef cNOGP
;[GP Player]
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 cSpeakerPin, [50\3000]	    
        GPSeq = GPSeq+1
      ELSE
        Sound cSpeakerPin, [50\4000, 50\4500]
        GPSeq=0
      ENDIF
    ENDIF
  ENDIF
  
  ;Start Sequence
  IF (DualShock(2).bit1 = 0) and LastButton(1).bit1 THEN   ;R2 Button test    
    IF GPStart = 0 THEN   ; See if we are running a sequence
      GPSpeedControl=0		; start off assuming no speed control
      GPStart=1    ; Nope then try to start it.
      GPSM = 100
    ELSE
      GPStart = 0xff  ; Yes - cancel it.
    ENDIF
  ENDIF
  IF GPStart = 2 then
    IF GPSpeedControl  or ((ABS(DualShock(4)-128)) >= cTravelDeadZone) THEN
      GPSM = ((DualShock(4) - 128)*25) / 16		; right stick UP/DOWN map to +-200 range.
      GPSpeedControl = 1
    ELSE
      GPSM = 100
    ENDIF
  ENDIF

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
;--------------------------------------------------------------------
;--------------------------------------------------------------------
;[ControlAllowInput] - Code to tell the controller to disable any async communications or interrupts
_fCAI var byte
ControlAllowInput[_fCAI]:
; PS2 does not need to do anything yet
RETURN
;
;Note about the gimbals:
;DualShock(3) ;Right stick horz
;DualShock(4) ;Right stick vert
;DualShock(5) ;Left stick horz
;DualShock(6) ;Left stick vert
;
;[IndirectDualShock] For an indirect control of A-Pod in rotation mode,
; necessary for obtaining a smooth and precise body control when using the PS2 controller
IndirectDualShock
For Index= 3 to 6
IF (NeutralStick(Index-3)=1) THEN ;simply resets the IndirectDualShock to 0 gradually

		IF IndDualShock(Index)>30 THEN
			IndDualShock(Index) = IndDualShock(Index)-30
		ELSEIF IndDualShock(Index)<-30
			IndDualShock(Index) = IndDualShock(Index)+30
		ELSE
			IndDualShock(Index) = 0
		ENDIF
		
		IF IndDualShock(Index) = 0 THEN
			NeutralStick(Index-3) = 0;Reset
		ENDIF
	ELSE
		IF ABS((Dualshock(Index) - 128)) > cTravelDeadZone THEN
			IndDualShock(Index) = ((IndDualShock(Index) + ((Dualshock(Index) - 128)/IDSdivFactor))MIN -128)MAX 127 
		ENDIF
	ENDIF

	;serout s_out, i57600, "IDS_",dec Index,":", sdec IndDualShock(Index)," "]
Next 
	;serout s_out, i57600, [13]

return

;-----------------------------------------------------------------------------------
; SmoothControl
; This function makes the body rotation and translation much smoother while walking
;
SmoothControl [CtrlMoveInp, CtrlMoveOut, CtrlDivider]
IF Walking THEN
IF (CtrlMoveOut < (CtrlMoveInp - 4)) THEN
CtrlMoveOut = CtrlMoveOut + ABS((CtrlMoveOut - CtrlMoveInp)/CtrlDivider)
ELSEIF (CtrlMoveOut > (CtrlMoveInp + 4))
CtrlMoveOut = CtrlMoveOut - ABS((CtrlMoveOut - CtrlMoveInp)/CtrlDivider)
ELSE
CtrlMoveOut = CtrlMoveInp
ENDIF
ELSE
CtrlMoveOut = CtrlMoveInp
ENDIF

return CtrlMoveOut
;-----------------------------------------------------------------------------------
;Branch RotateFunction,
;
BranchRotateFunction:
BRANCH RotateFunction,[HeadTracking, FixedHead, HeadOnly]

HeadTracking:
GOSUB HeadControl -IndDualShock(3), IndDualShock(5), -IndDualShock(4)]
BodyRotX1 = -IndDualShock(4)
BodyRotY1 = IndDualShock(5)
BodyRotZ1 = -IndDualShock(3)*2
HeadRotAngle1 = -HeadRotOut1
HeadPanAngle1 = HeadPanOut1
HeadTiltAngle1 = HeadTiltOut1

return

FixedHead:
BodyRotX1 = -IndDualShock(4)
BodyRotY1 = IndDualShock(5)
BodyRotZ1 = -IndDualShock(3)*2
HeadRotAngle1 = -BodyRotZ1 + HeadRotOut1
HeadPanAngle1 = BodyRotY1 + HeadPanOut1
HeadTiltAngle1 = -BodyRotX1 + HeadTiltOut1

return

HeadOnly:
;GOSUB HeadControl (Dualshock(3) - 128), (Dualshock(5) - 128), (Dualshock(4) - 128)]
GOSUB HeadControl [IndDualShock(3), IndDualShock(5), -IndDualShock(4)]
HeadRotAngle1 = -BodyRotZ1 + HeadRotOut1
HeadPanAngle1 = BodyRotY1 + HeadPanOut1
HeadTiltAngle1 = -BodyRotX1 + HeadTiltOut1

return
;--------------------------------------------------------------------
;Head control,
;
HeadControl [HeadRotInput1, HeadPanInput1, HeadTiltInput1]

GOSUB SmoothControl (-HeadRotInput1*7/(2-FullHeadRange)),HeadRotOut1,2], HeadRotOut1 '44,8 deg half range and 89,6 deg at full range

GOSUB SmoothControl (-HeadPanInput1*3),HeadPanOut1,2], HeadPanOut1 '38,4 deg

GOSUB SmoothControl [HeadTiltInput1*c2DEC/24,HeadTiltOut1,2], HeadTiltOut1 '53,3 deg

return
;--------------------------------------------------------------------
;
MandibleControl:
adin 16, MandibleFSRInput ;Read FSR

IF MandibleFSRInput > cMandibleContact THEN ; IF this is true the mandible are touching the object
IF NOT MandibleFSR_Activated THEN ; If in contact with FSR for the first time
RPOTSensePos = RPOTCtrlPos ; Save the potmeter position
ENDIF
;code for adjusting mandibles to the correct Torque level:
IF (MandibleFSRInput > TorqueLevel+100) THEN;If to much torque:
MandRightPWM = MandRightPWM - 1 ; open mandible a little bit
ELSEIF (MandibleFSRInput < TorqueLevel-100) ;If to little torque:
MandRightPWM = MandRightPWM + 1 ; close mandible a little bit
ENDIF

IF RPOTCtrlPos > (RPOTSensePos + 10) THEN ;open mandibles
  MandRightPWM = cMandRightClosedPWM - RPOTCtrlPos*5/2
  
ENDIF
MandibleFSR_Activated = 1

ELSE ;Open/close mandibles
MandibleFSR_Activated = 0
MandRightPWM = cMandRightClosedPWM - RPOTCtrlPos*5/2

ENDIF
MandLeftPWM = cMandLeftClosedPWM - (MandRightPWM - cMandRightClosedPWM)

return
Apod.txt (23.1 KB)

etwx · December 23, 2012, 8:22pm