;   - R1 & L. Stick   Moves Head X/Z
 
     
------------------------------------------------------------------     
    Phoenix_Config_3DOFPhoenix_ARC32 in ;[ARC32 PIN NUMBERS]
     
      HeadPan       con P19
      HeadTilt      con P3
-----------------------------------------------------------------      
      
    Phoenix_Config_3DOFPhoenix_ARC32 in ;[MIN/MAX ANGLES]  
      
    HeadPan_MAX con 20
    HeadPan_MIN con -20
  
    HeadTilt_MAX con 20
    HeadTilt_MIN con -30
-----------------------------------------------------------------   
  
    Phoenix_Config_3DOFPhoenix_ARC32 in ;[BODY DIMENSIONS]
  
   
    PanOffsetX    con 0     ;Distance X from center of the body to the head
    PanOffsetZ    con 110   ;Distance Z from center of the body to the head
      
    TiltOffsetX   con 130   ;Distance X from center of the body to the head
    TiltffsetZ    con 0     ;Distance Z from center of the body to the head

------------------------------------------------------------------------

    Phoenix_Core  in   ;[ANGLES]

    HeadPanAngle   var sword   ;Actual Angle of the Head pan
    HeadTiltAngle   var sword   ;Actual Angle of the Head tilt
    
 ---------------------------------------------------------------------   
       Phoenix_Core  in    ;[MAIN]	 just before   GOSUB CheckAngles
     
     GOSUB HeadTracking
     
     
      ;[MAIN]	 just before ;Drive Servos
     
     ;[Head Tracking]
      HeadTracking:

      ;Return to the middle position
    ;  HeadPanAngle=0
    ;IF (ABS(TravelLengthX)>TravelDeadZone | ABS(TravelLengthZ)>TravelDeadZone | ABS(TravelRotationY*2)>TravelDeadZone) THEN
        ;Calculate walking direction X and Z
    ;    TravelLengthXZ = SQR((TravelLengthX * TravelLengthX) + TravelLengthZ * TravelLengthZ)
    ;    HeadPanAngle = TOINT(FACOS(TOFLOAT(TravelLengthZ) / TOFLOAT(TravelLengthXZ)) * 180.0 / 3.141592)-180
        ;Add sign depending on the direction of X
    ;    HeadPanAngle = HeadPanAngle * (TravelLengthX/ABS(TravelLengthX))
    ;  ENDIF
      ;Calculate body angle depending on rotation
    ;  IF ABS(TravelRotationY*2)>TravelDeadZone & ABS(TravelRotationY*3) > ABS(HeadPanAngle) THEN
    ;    HeadPanAngle = -TravelRotationY*3 ; Rotation max = 16*6 to get max range of 90 deg.   
    ;  ENDIF
     
      ;Return to the middle position
         HeadPanAngle=0
    IF (ABS(BodyRotY)>TravelDeadZone | ABS(BodyRotY)>TravelDeadZone | ABS(TravelRotationY*2)>TravelDeadZone) THEN
        ;Calculate direction Z and X
        HeadPanAngle = SQR((BodyRotY * BodyRotY) + BodyYShift * BodyYShift)
         
        ;Add sign depending on the direction of X
        HeadPanAngle = HeadPanAngle * (BodyRotY/ABS(BodyRotY))
      ENDIF
      ;Calculate body angle depending on rotation
      IF ABS(TravelRotationY*2)>TravelDeadZone & ABS(TravelRotationY*3) > ABS(HeadPanAngle) THEN
        HeadPanAngle = -TravelRotationY*3 ; Rotation max = 16*6 to get max range of 90 deg.   
      ENDIF
    ;----------------
       ;Return to the middle position
    ;   HeadTiltAngle=0
    ;IF (ABS(TravelLengthZ)>TravelDeadZone | ABS(TravelLengthX)>TravelDeadZone | ABS(TravelRotationY*2)>TravelDeadZone) THEN
        ;Calculate walking direction Z and X
    ;    TravelLengthXZ = SQR((TravelLengthZ * TravelLengthZ) + TravelLengthX * TravelLengthX)
    ;    HeadTiltAngle = TOINT(FACOS(TOFLOAT(TravelLengthX) / TOFLOAT(TravelLengthXZ)) * 180.0 / 3.141592)-180
        ;Add sign depending on the direction of Z
    ;    HeadTiltAngle = HeadTiltAngle * (TravelLengthZ/ABS(TravelLengthZ))
    ;  ENDIF
      ;Calculate body angle depending on rotation
    ;  IF ABS(TravelRotationY*2)>TravelDeadZone & ABS(TravelRotationY*3) > ABS(HeadTiltAngle) THEN
    ;    HeadTiltAngle = -TravelRotationY*3 ; Rotation max = 16*6 to get max range of 90 deg.   
    ;  ENDIF

    ;Return to the middle position
       HeadTiltAngle=0
    IF (ABS(BodyYShift)>TravelDeadZone | ABS(BodyRotY)>TravelDeadZone | ABS(TravelRotationY*2)>TravelDeadZone) THEN
        ;Calculate direction Z and X
        HeadTiltAngle = SQR((BodyRotY * BodyRotY) + BodyYShift * BodyYShift)
         
        ;Add sign depending on the direction of Z
        HeadTiltAngle = HeadTiltAngle * (BodyYShift/ABS(BodyYShift))
      ENDIF
      ;Calculate body angle depending on rotation
      IF ABS(TravelRotationY*2)>TravelDeadZone & ABS(TravelRotationY*3) > ABS(HeadTiltAngle) THEN
        HeadTiltAngle = -TravelRotationY*3 ; Rotation max = 16*6 to get max range of 90 deg.   
      ENDIF

    return
    
------------------------------------------------------------------    
       Phoenix_Core  in    ;[CHECK ANGLES] 
           
           
       
         HeadPanAngle = (HeadPanAngle min HeadPan_MIN) max HeadPan_MAX
      HeadTiltAngle = (HeadTiltAngle min HeadTilt_MIN) max HeadTilt_MAX  
      
    
    
-------------------------------------------------------------    
 Phoenix_Control_ps2    after  ;Start Sequence
       

        if (DualShock(1).bit2 = 0) then  ;R3 Button test   
         HeadPanAngle = (Dualshock(3) - 128)/2  ;X-axis Right stick horz
         HeadTiltAngle = (Dualshock(4) - 128)/2 ;Z-axis Right stick vert

        ENDIF
--------------------------------------------------    
       
       
  
      
       this did not seem to apply
      
       Serout SSC_OUT,SSC_BAUTE,["#",dec HeadPan,"P",dec (-HeadPanAngle+90)*10000/1059+650]
      Serout SSC_OUT,SSC_BAUTE,["#",dec HeadTilt,"P",dec (-HeadTiltAngle+90)*10000/1059+650]