Start support for Turret

Starting support for turret (Pan/Tilt).  Currently using right joystick
in special mode (use R2 button to choose walk mode).

Also fix Commander Input such that right joystick is actually right
joystick, dito left.

Also added code to AX12 driver, that if the robot is not walking it
plays with the LEDs on the servos such that hopefully I will not forget
to turn power off.  Also I set Digital servo pin 7 high, that I use to
enable an LED display.

Author: KurtE

Hex_Cfg.h

@@ -132,6 +132,8 @@
 #define cLFFemurPin     3   //Front Left leg Hip Vertical
 #define cLFTibiaPin     5   //Front Left leg Knee
 
+#define cTurretRotPin  20   // Turret Rotate Pin
+#define cTurretTiltPin 21  // Turret Tilt pin
 
 //--------------------------------------------------------------------
 //[MIN/MAX ANGLES] - Start off assume same as Phoenix...
@@ -177,6 +179,11 @@
 #define cLFTibiaMin1    -1020
 #define cLFTibiaMax1     670
 
+#define cTurretRotMin1  -1500
+#define cTurretRotMax1  1500
+#define cTurretTiltMin1  -125
+#define cTurretTiltMax1  1100
+
 //--------------------------------------------------------------------
 //[Joint offsets]
 // This allows us to calibrate servos to some fixed position, and then adjust them by moving theim
@@ -293,6 +300,12 @@ extern const byte g_abHexMaxBodyY[] PROGMEM;
 #define cLFInitPosX     CHexInitXZCos60      //Start positions of the Left Front leg
 #define cLFInitPosY     CHexInitY
 #define cLFInitPosZ     -CHexInitXZSin60
+
+// Turret initial position
+#define cTurretRotInit  0
+#define cTurretTiltInit 0
+
+
 //--------------------------------------------------------------------
 //[Tars factors used in formula to calc Tarsus angle relative to the ground]
 #define cTarsst	720	//4DOF ONLY
@@ -302,3 +315,4 @@ extern const byte g_abHexMaxBodyY[] PROGMEM;
 #define cTarsFactorC	50	//4DOF ONLY
 
 #endif // HEX_CFG_H
+

_Phoenix.h

@@ -4,13 +4,13 @@
 //
 //Programmer: Jeroen Janssen [aka Xan]
 //         Kurt Eckhardt(KurtE) converted to C and Arduino
-//   Kåre Halvorsen aka Zenta - Makes everything work correctly!     
+//   K�re Halvorsen aka Zenta - Makes everything work correctly!     
 //
 // This version of the Phoenix code was ported over to the Arduino Environement
 //
 //
 // Phoenix.h - This is the first header file that is needed to build
-//			a Phoenix program for a specific Hex Robot.
+//          a Phoenix program for a specific Hex Robot.
 //
 //
 // This file assumes that the main source file either directly or through include
@@ -29,17 +29,17 @@
 #include <stdarg.h>
 //#include <EEPROM.h>
 #if defined(__SAM3X8E__)
-    #define PROGMEM
-    #define pgm_read_byte(x)        (*((char *)x))
+#define PROGMEM
+#define pgm_read_byte(x)        (*((char *)x))
 //  #define pgm_read_word(x)        (*((short *)(x & 0xfffffffe)))
-    #define pgm_read_word(x)        ( ((*((unsigned char *)x + 1)) << 8) + (*((unsigned char *)x)))
-    #define pgm_read_byte_near(x)   (*((char *)x))
-    #define pgm_read_byte_far(x)    (*((char *)x))
+#define pgm_read_word(x)        ( ((*((unsigned char *)x + 1)) << 8) + (*((unsigned char *)x)))
+#define pgm_read_byte_near(x)   (*((char *)x))
+#define pgm_read_byte_far(x)    (*((char *)x))
 //  #define pgm_read_word_near(x)   (*((short *)(x & 0xfffffffe))
 //  #define pgm_read_word_far(x)    (*((short *)(x & 0xfffffffe)))
-    #define pgm_read_word_near(x)   ( ((*((unsigned char *)x + 1)) << 8) + (*((unsigned char *)x)))
-    #define pgm_read_word_far(x)    ( ((*((unsigned char *)x + 1)) << 8) + (*((unsigned char *)x))))
-    #define PSTR(x)  x
+#define pgm_read_word_near(x)   ( ((*((unsigned char *)x + 1)) << 8) + (*((unsigned char *)x)))
+#define pgm_read_word_far(x)    ( ((*((unsigned char *)x + 1)) << 8) + (*((unsigned char *)x))))
+#define PSTR(x)  x
 #endif
 
 #ifdef USEXBEE
@@ -50,27 +50,29 @@
 //[CONSTANTS]
 //=============================================================================
 #define BUTTON_DOWN 0
-#define BUTTON_UP 	1
+#define BUTTON_UP   1
 
-#define	c1DEC		10
-#define	c2DEC		100
-#define	c4DEC		10000
-#define	c6DEC		1000000
+#define c1DEC       10
+#define c2DEC       100
+#define c4DEC       10000
+#define c6DEC       1000000
 
 #ifdef QUADMODE
-enum {cRR=0, cRF, cLR, cLF, CNT_LEGS};
+enum {
+  cRR=0, cRF, cLR, cLF, CNT_LEGS};
 #else
-enum {cRR=0, cRM, cRF, cLR, cLM, cLF, CNT_LEGS};
+enum {
+  cRR=0, cRM, cRF, cLR, cLM, cLF, CNT_LEGS};
 #endif
 
-#define	WTIMERTICSPERMSMUL  	64	// BAP28 is 16mhz need a multiplyer and divider to make the conversion with /8192
-#define WTIMERTICSPERMSDIV  	125 // 
+#define WTIMERTICSPERMSMUL      64  // BAP28 is 16mhz need a multiplyer and divider to make the conversion with /8192
+#define WTIMERTICSPERMSDIV      125 // 
 #define USEINT_TIMERAV
 
 
 // BUGBUG: to make Dynamic first pass simpl make it a variable.
-extern  byte 	NUM_GAITS;
-#define SmDiv    	 4  //"Smooth division" factor for the smooth control function, a value of 3 to 5 is most suitable
+extern  byte    NUM_GAITS;
+#define SmDiv        4  //"Smooth division" factor for the smooth control function, a value of 3 to 5 is most suitable
 extern void GaitSelect(void);
 extern short SmoothControl (short CtrlMoveInp, short CtrlMoveOut, byte CtrlDivider);
 
@@ -124,10 +126,10 @@ extern SoftwareSerial SSCSerial;
 #endif
 #endif
 #if defined(__PIC32MX__)
-  #if defined F
-    #undef F
-  #endif
-  #define F(X) (X)
+#if defined F
+#undef F
+#endif
+#define F(X) (X)
 #endif
 
 
@@ -139,12 +141,13 @@ extern SoftwareSerial SSCSerial;
 //=============================================================================
 class InputController {
 public:
-    virtual void     Init(void);
-    virtual void     ControlInput(void);
-    virtual void     AllowControllerInterrupts(boolean fAllow);
+  virtual void     Init(void);
+  virtual void     ControlInput(void);
+  virtual void     AllowControllerInterrupts(boolean fAllow);
 
 private:
-} ;   
+} 
+;   
 
 // Define a function that allows us to define which controllers are to be used.
 extern void  RegisterInputController(InputController *pic);
@@ -155,33 +158,35 @@ typedef struct _Coord3D {
   long      x;
   long      y;
   long      z;
-} COORD3D;
+} 
+COORD3D;
 
 //==============================================================================
 // Define Gait structure/class - Hopefully allow specific robots to define their
 // own gaits and/or define which of the standard ones they want.
 //==============================================================================
 typedef struct _PhoenixGait {
-	short	 		NomGaitSpeed;		//Nominal speed of the gait
-	byte            StepsInGait;         //Number of steps in gait
-	byte            NrLiftedPos;         //Number of positions that a single leg is lifted [1-3]
-	byte            FrontDownPos;        //Where the leg should be put down to ground
-	byte            LiftDivFactor;       //Normaly: 2, when NrLiftedPos=5: 4
-	byte            TLDivFactor;         //Number of steps that a leg is on the floor while walking
-	byte	        HalfLiftHeight;      // How high to lift at halfway up.
+  short           NomGaitSpeed;       //Nominal speed of the gait
+  byte            StepsInGait;         //Number of steps in gait
+  byte            NrLiftedPos;         //Number of positions that a single leg is lifted [1-3]
+  byte            FrontDownPos;        //Where the leg should be put down to ground
+  byte            LiftDivFactor;       //Normaly: 2, when NrLiftedPos=5: 4
+  byte            TLDivFactor;         //Number of steps that a leg is on the floor while walking
+  byte            HalfLiftHeight;      // How high to lift at halfway up.
 
 #ifdef QUADMODE
-    // Extra information used in the Quad balance mode
-    word			COGAngleStart1;		 // COG shifting starting angle
-    word			COGAngleStep1;		 // COG Angle Steps in degrees
-    byte			COGRadius;			 // COG Radius; the amount the body shifts
-    boolean 		COGCCW;				 // COG Gait sequence runs counter clock wise
+  // Extra information used in the Quad balance mode
+  word            COGAngleStart1;      // COG shifting starting angle
+  word            COGAngleStep1;       // COG Angle Steps in degrees
+  byte            COGRadius;           // COG Radius; the amount the body shifts
+  boolean         COGCCW;              // COG Gait sequence runs counter clock wise
 #endif    
-	byte            GaitLegNr[CNT_LEGS]; //Init position of the leg
+  byte            GaitLegNr[CNT_LEGS]; //Init position of the leg
 #ifdef DISPLAY_GAIT_NAMES
-    PGM_P           pszName;             // The gait name
+  PGM_P           pszName;             // The gait name
 #endif
-} PHOENIXGAIT;
+} 
+PHOENIXGAIT;
 
 #ifdef DISPLAY_GAIT_NAMES
 #define GATENAME(name)  ,name
@@ -195,8 +200,8 @@ typedef struct _PhoenixGait {
 //      requested.
 //==============================================================================
 typedef struct _InControlState {
-  boolean		fRobotOn;			 //Switch to turn on Phoenix
-  boolean		fPrev_RobotOn;		 //Previous loop state 
+  boolean       fRobotOn;            //Switch to turn on Phoenix
+  boolean       fPrev_RobotOn;       //Previous loop state 
   //Body position
   COORD3D       BodyPos;
   COORD3D       BodyRotOffset;      // Body rotation offset;
@@ -205,34 +210,41 @@ typedef struct _InControlState {
   COORD3D       BodyRot1;            // X -Pitch, Y-Rotation, Z-Roll
 
   //[gait]
-  byte			GaitType;			 //Gait type
+  byte          GaitType;            //Gait type
   byte          GaitStep;            //Actual current step in gait
   PHOENIXGAIT   gaitCur;             // Definition of the current gait
 
-    short       LegLiftHeight;		 //Current Travel height
+  short       LegLiftHeight;       //Current Travel height
   COORD3D       TravelLength;        // X-Z or Length, Y is rotation.
 
+#ifdef cTurretRotPin
+  // Turret information
+  int           TurretRotAngle1;      // Rotation of turrent in 10ths of degree
+  int           TurretTiltAngle1;    // the tile for the turret
+#endif
+
   //[Single Leg Control]
-  byte			SelectedLeg;
+  byte          SelectedLeg;
   COORD3D       SLLeg;               // 
-  boolean		fSLHold;		 	 //Single leg control mode
+  boolean       fSLHold;             //Single leg control mode
 
 
   //[Balance]
   boolean       BalanceMode;
 
   //[TIMING]
-  byte			InputTimeDelay;	//Delay that depends on the input to get the "sneaking" effect
-  word			SpeedControl;	//Adjustible Delay
+  byte          InputTimeDelay; //Delay that depends on the input to get the "sneaking" effect
+  word          SpeedControl;   //Adjustible Delay
   byte          ForceGaitStepCnt;          // new to allow us to force a step even when not moving
-  
+
 #ifdef ADJUSTABLE_LEG_ANGLES
   short         aCoxaInitAngle1[CNT_LEGS]; 
 #endif
 
   // 
-  
-} INCONTROLSTATE;
+
+} 
+INCONTROLSTATE;
 
 //==============================================================================
 //==============================================================================
@@ -243,32 +255,41 @@ class ServoDriver {
 public:
   void Init(void);
 
-    word GetBatteryVoltage(void);
+  word GetBatteryVoltage(void);
 
 #ifdef OPT_GPPLAYER    
-  inline boolean  FIsGPEnabled(void) {return _fGPEnabled;};
+  inline boolean  FIsGPEnabled(void) {
+    return _fGPEnabled;
+  };
   boolean         FIsGPSeqDefined(uint8_t iSeq);
-  inline boolean  FIsGPSeqActive(void) {return _fGPActive;};
+  inline boolean  FIsGPSeqActive(void) {
+    return _fGPActive;
+  };
   void            GPStartSeq(uint8_t iSeq);  // 0xff - says to abort...
   void            GPPlayer(void);
   uint8_t         GPNumSteps(void);          // How many steps does the current sequence have
   uint8_t         GPCurStep(void);           // Return which step currently on... 
   void            GPSetSpeedMultiplyer(short sm) ;      // Set the Speed multiplier (100 is default)
 #endif
-  void 			  BeginServoUpdate(void);    // Start the update 
+  void            BeginServoUpdate(void);    // Start the update 
 #ifdef c4DOF
   void            OutputServoInfoForLeg(byte LegIndex, short sCoxaAngle1, short sFemurAngle1, short sTibiaAngle1, short sTarsAngle1);
 #else
   void            OutputServoInfoForLeg(byte LegIndex, short sCoxaAngle1, short sFemurAngle1, short sTibiaAngle1);
 #endif    
+#ifdef cTurretRotPin
+  void            OutputServoInfoForTurret(short sRotateAngle1, short sTiltAngle1);
+#endif
   void            CommitServoDriver(word wMoveTime);
   void            FreeServos(void);
+  
+  void            IdleTime(void);        // called when the main loop when the robot is not on
 
-    // Allow for background process to happen...
+  // Allow for background process to happen...
 #ifdef OPT_BACKGROUND_PROCESS
   void            BackgroundProcess(void);
 #endif    
-    
+
 #ifdef OPT_TERMINAL_MONITOR  
   void            ShowTerminalCommandList(void);
   boolean         ProcessTerminalCommand(byte *psz, byte bLen);
@@ -280,10 +301,11 @@ class ServoDriver {
   boolean _fGPEnabled;     // IS GP defined for this servo driver?
   boolean _fGPActive;      // Is a sequence currently active - May change later when we integrate in sequence timing adjustment code
   uint8_t    _iSeq;        // current sequence we are running
-    short    _sGPSM;        // Speed multiplier +-200 
+  short    _sGPSM;        // Speed multiplier +-200 
 #endif
 
-} ;   
+} 
+;   
 
 //==============================================================================
 //==============================================================================
@@ -292,8 +314,10 @@ class ServoDriver {
 //==============================================================================
 extern ServoDriver      g_ServoDriver;           // our global servo driver class
 extern InputController  g_InputController;       // Our Input controller 
-extern INCONTROLSTATE   g_InControlState;		 // State information that controller changes
+extern INCONTROLSTATE   g_InControlState;        // State information that controller changes
 
 
 #endif
 
+
+