Merge branch 'master' into Quad-Support

Phoenix/PhantomX_V2_Commander_AX12_Stock/Hex_Cfg.h

@@ -238,7 +238,6 @@
 #define cLFTibiaLength    cXXTibiaLength
 
 
-
 //--------------------------------------------------------------------
 //[BODY DIMENSIONS]
 #define cRRCoxaAngle1   -450   //Default Coxa setup angle, decimals = 1

Phoenix/Phoenix.h

@@ -195,9 +195,9 @@ typedef struct _PhoenixGait {
 PHOENIXGAIT;
 
 #ifdef DISPLAY_GAIT_NAMES
-#define GATENAME(name)  ,name
+#define GAITNAME(name)  ,name
 #else
-#define GATENAME(name)
+#define GAITNAME(name)
 #endif
 
 //==============================================================================
@@ -262,7 +262,7 @@ class ServoDriver {
 public:
   void Init(void);
 
-    word GetBatteryVoltage(void);
+    uint16_t GetBatteryVoltage(void);
 
 #ifdef OPT_GPPLAYER    
   inline boolean  FIsGPEnabled(void) {

Phoenix/Phoenix_Code.h

@@ -482,7 +482,7 @@ INCONTROLSTATE   g_InControlState;      // This is our global Input control stat
 ServoDriver  g_ServoDriver;      // our global servo driver class
 
 boolean         g_fLowVoltageShutdown;    // If set the bot shuts down because the input voltage is to low
-word            Voltage;
+uint16_t      Voltage;
 
 
 //--boolean         g_InControlState.fRobotOn;            //Switch to turn on Phoenix
@@ -545,12 +545,12 @@ extern "C" {
 #endif
 
 PHOENIXGAIT APG[] = { 
-    {DEFAULT_SLOW_GAIT, 12, 3, 2, 2, 8, 3, {7, 11, 3, 1, 5, 9} GATENAME(s_szGN1)},        // Ripple 12
-    {DEFAULT_SLOW_GAIT, 8, 3, 2, 2, 4, 3, {1, 5, 1, 5, 1, 5} GATENAME(s_szGN2)},           //Tripod 8 steps
-    {DEFAULT_GAIT_SPEED, 12, 3, 2, 2, 8, 3, {5, 10, 3, 11, 4, 9} GATENAME(s_szGN3) },      //Triple Tripod 12 step
-    {DEFAULT_GAIT_SPEED, 16, 5, 3, 4, 10, 1, {6, 13, 4, 14, 5, 12} GATENAME(s_szGN4)},    // Triple Tripod 16 steps, use 5 lifted positions
-    {DEFAULT_SLOW_GAIT, 24, 3, 2, 2, 20, 3, {13, 17, 21, 1, 5, 9} GATENAME(s_szGN5)},     //Wave 24 steps
-    {DEFAULT_GAIT_SPEED, 6, 2, 1, 2, 4, 1, {1, 4, 1, 4, 1, 4} GATENAME(s_szGN6)}          //Tripod 6 steps
+    {DEFAULT_SLOW_GAIT, 12, 3, 2, 2, 8, 3, {7, 11, 3, 1, 5, 9} GAITNAME(s_szGN1)},        // Ripple 12
+    {DEFAULT_SLOW_GAIT, 8, 3, 2, 2, 4, 3, {1, 5, 1, 5, 1, 5} GAITNAME(s_szGN2)},           //Tripod 8 steps
+    {DEFAULT_GAIT_SPEED, 12, 3, 2, 2, 8, 3, {5, 10, 3, 11, 4, 9} GAITNAME(s_szGN3) },      //Triple Tripod 12 step
+    {DEFAULT_GAIT_SPEED, 16, 5, 3, 4, 10, 1, {6, 13, 4, 14, 5, 12} GAITNAME(s_szGN4)},    // Triple Tripod 16 steps, use 5 lifted positions
+    {DEFAULT_SLOW_GAIT, 24, 3, 2, 2, 20, 3, {13, 17, 21, 1, 5, 9} GAITNAME(s_szGN5)},     //Wave 24 steps
+    {DEFAULT_GAIT_SPEED, 6, 2, 1, 2, 4, 1, {1, 4, 1, 4, 1, 4} GAITNAME(s_szGN6)}          //Tripod 6 steps
 };    
 
 #else
@@ -562,8 +562,8 @@ extern "C" {
 }
 #endif
 PHOENIXGAIT APG[] = { 
-    {DEFAULT_GAIT_SPEED, 16, 3, 2, 2, 12, 3, 2250, 3600/16, 30, true, {5, 9, 1, 13} GATENAME(s_szGN1)},            // Wave 16
-    {1, 28, 3, 2, 2, 24, 3, 2250, 3600/28, 30, true, {8, 15, 1, 22} GATENAME(s_szGN2)}                             // Wave 28?
+    {DEFAULT_GAIT_SPEED, 16, 3, 2, 2, 12, 3, 2250, 3600/16, 30, true, {5, 9, 1, 13} GAITNAME(s_szGN1)},            // Wave 16
+    {1, 28, 3, 2, 2, 24, 3, 2250, 3600/28, 30, true, {8, 15, 1, 22} GAITNAME(s_szGN2)}                             // Wave 28?
 };    
 
 #endif
@@ -1845,7 +1845,7 @@ short CheckServoAngleBounds(short sID,  short sVal, const short *sMin PROGMEM, c
     // Note ID is bogus, but something to let me know which one.
     short s = (short)pgm_read_word(sMin);
     if (sVal < s) {
-#ifdef DEBUG
+#ifdef DEBUG_BOUNDS
       if (g_fDebugOutput) {
         DBGSerial.print(sID, DEC);
         DBGSerial.print(" ");
@@ -1859,7 +1859,7 @@ short CheckServoAngleBounds(short sID,  short sVal, const short *sMin PROGMEM, c
 
     s = (short)pgm_read_word(sMax);
     if (sVal > s) {
-#ifdef DEBUG
+#ifdef DEBUG_BOUNDS
       if (g_fDebugOutput) {
         DBGSerial.print(sID, DEC);
         DBGSerial.print(" ");

Phoenix_Driver_AX12/Phoenix_Driver_AX12.h

@@ -28,10 +28,9 @@
 #define VOLTAGE_MAX_TIME_BETWEEN_CALLS 1000    // call at least once per second...
 #define VOLTAGE_TIME_TO_ERROR          3000    // Error out if no valid item is returned in 3 seconds...
 
-//#include <ax12.h>
 
 #define USE_BIOLOIDEX            // Use the Bioloid code to control the AX12 servos...
-#define USE_AX12_SPEED_CONTROL   // Experiment to see if the speed control works well enough...
+//#define USE_AX12_SPEED_CONTROL   // Experiment to see if the speed control works well enough...
 boolean g_fAXSpeedControl;      // flag to know which way we are doing output...
 #include "BioloidEx.h"
 
@@ -179,12 +178,12 @@ void ServoDriver::Init(void) {
 //--------------------------------------------------------------------
 
 #ifdef cVoltagePin  
-word  g_awVoltages[8]={
+uint16_t  g_awVoltages[8]={
   0,0,0,0,0,0,0,0};
-word  g_wVoltageSum = 0;
+uint16_t  g_wVoltageSum = 0;
 byte  g_iVoltages = 0;
 
-word ServoDriver::GetBatteryVoltage(void) {
+uint16_t ServoDriver::GetBatteryVoltage(void) {
   g_iVoltages = (++g_iVoltages)&0x7;  // setup index to our array...
   g_wVoltageSum -= g_awVoltages[g_iVoltages];
   g_awVoltages[g_iVoltages] = analogRead(cVoltagePin);
@@ -198,11 +197,11 @@ word ServoDriver::GetBatteryVoltage(void) {
 }
 
 #else
-word g_wLastVoltage = 0xffff;    // save the last voltage we retrieved...
+uint16_t g_wLastVoltage = 0xffff;    // save the last voltage we retrieved...
 byte g_bLegVoltage = 0;		// what leg did we last check?
 unsigned long g_ulTimeLastBatteryVoltage;
 
-word ServoDriver::GetBatteryVoltage(void) {
+uint16_t ServoDriver::GetBatteryVoltage(void) {
   // In this case, we have to ask a servo for it's current voltage level, which is a lot more overhead than simply doing
   // one AtoD operation.  So we will limit when we actually do this to maybe a few times per second.  
   // Also if interpolating, the code will try to only call us when it thinks it won't interfer with timing of interpolation.
@@ -214,7 +213,7 @@ word ServoDriver::GetBatteryVoltage(void) {
   }
 
   // Lets cycle through the Tibia servos asking for voltages as they may be the ones doing the most work...
-  register word wVoltage = ax12GetRegister (pgm_read_byte(&cPinTable[FIRSTTIBIAPIN+g_bLegVoltage]), AX_PRESENT_VOLTAGE, 1);
+  register uint16_t wVoltage = ax12GetRegister (pgm_read_byte(&cPinTable[FIRSTTIBIAPIN+g_bLegVoltage]), AX_PRESENT_VOLTAGE, 1);
   if (++g_bLegVoltage >= CNT_LEGS)
 	g_bLegVoltage = 0;
   if (wVoltage != 0xffff) {

Phoenix_Driver_SSC32/Phoenix_Driver_SSC32.h

@@ -105,18 +105,22 @@ void ServoDriver::Init(void) {
 #define CVADR2      10  // VD Resistor 2
 #endif
 
-word  g_awVoltages[8]={
+uint16_t  g_awVoltages[8]={
   0,0,0,0,0,0,0,0};
-word  g_wVoltageSum = 0;
+uint16_t  g_wVoltageSum = 0;
 byte  g_iVoltages = 0;
 
-word ServoDriver::GetBatteryVoltage(void) {
+uint16_t ServoDriver::GetBatteryVoltage(void) {
   g_iVoltages = (++g_iVoltages)&0x7;  // setup index to our array...
   g_wVoltageSum -= g_awVoltages[g_iVoltages];
   g_awVoltages[g_iVoltages] = analogRead(cVoltagePin);
   g_wVoltageSum += g_awVoltages[g_iVoltages];
 
+#ifdef CVREF
+  return ((long)((long)g_wVoltageSum*CVREF*(CVADR1+CVADR2))/(long)(8192*(long)CVADR2));  
+#else
   return ((long)((long)g_wVoltageSum*125*(CVADR1+CVADR2))/(long)(2048*(long)CVADR2));  
+#endif
 
 }
 #endif

Phoenix_Driver_ServoEx/phoenix_driver_ServoEx.h

@@ -122,18 +122,22 @@ void ServoDriver::Init(void) {
 #define CVADR2      10  // VD Resistor 2
 #endif
 
-word  g_awVoltages[8]={
+uint16_t  g_awVoltages[8]={
   0,0,0,0,0,0,0,0};
-word  g_wVoltageSum = 0;
+uint16_t  g_wVoltageSum = 0;
 byte  g_iVoltages = 0;
 
-word ServoDriver::GetBatteryVoltage(void) {
+uint16_t ServoDriver::GetBatteryVoltage(void) {
   g_iVoltages = (++g_iVoltages)&0x7;  // setup index to our array...
   g_wVoltageSum -= g_awVoltages[g_iVoltages];
   g_awVoltages[g_iVoltages] = analogRead(cVoltagePin);
   g_wVoltageSum += g_awVoltages[g_iVoltages];
 
+#ifdef CVREF
+  return ((long)((long)g_wVoltageSum*CVREF*(CVADR1+CVADR2))/(long)(8192*(long)CVADR2));  
+#else
   return ((long)((long)g_wVoltageSum*125*(CVADR1+CVADR2))/(long)(2048*(long)CVADR2));  
+#endif
 
 }
 #endif

Phoenix_Input_DIYXbee/Phoenix_Input_DIYXbee.h

@@ -1,4 +1,4 @@
-#define DEBUGTOGGLE(pin) digitalWrite(pin, !digitalRead(pin))
+
 //=============================================================================
 //Project Lynxmotion Phoenix
 //Description: Phoenix, control file.
@@ -157,7 +157,7 @@ void DIYXBeeController::Init(void)
   delay(20);
   ClearXBeeInputBuffer();
 
-#if 0 //def DBGSerial    
+#ifdef DBGSerial    
   uint16_t wMy = GetXBeeMY();
   DBGSerial.print("XBee My: ");
   DBGSerial.println(wMy, HEX);
@@ -202,7 +202,7 @@ void DIYXBeeController::ControlInput(void)
     while (g_diystate.fNewPacket) {
         if (!ReceiveXBeePacket(&g_diyp)) {
            for (int i=0; i<10; i++) {
-                DEBUGTOGGLE(2);
+                DEBUGTOGGLE(DEBUG_PINS_FIRST);
                 delayMicroseconds(5);
             }
             MSound(1, 50, 2500);
@@ -225,8 +225,9 @@ void DIYXBeeController::ControlInput(void)
         g_InControlState.TravelLength.y = 0;
         g_BodyYOffset = 0;
         g_BodyYSift = 0;
+#ifdef OPT_SINGLELEG      
         g_InControlState.SelectedLeg = 255;
-
+#endif
         g_InControlState.fRobotOn = 0;
       } 
       else  {
@@ -256,6 +257,7 @@ void DIYXBeeController::ControlInput(void)
         XBeeOutputStringF(F("Body Rotate"));
       }
 
+#ifdef OPT_SINGLELEG      
       if ((g_diyp.s.wButtons & (1<<0xD)) && ((wButtonsPrev & (1<<0xd)) == 0)) { // D button test - Single Leg
         MSound(1, 50, 2000);
         if (g_InControlState.SelectedLeg==255) // none
@@ -265,6 +267,7 @@ void DIYXBeeController::ControlInput(void)
         bXBeeControlMode=SINGLELEGMODE;        
         XBeeOutputStringF (F("Single Leg"));
       }
+#endif
       if ((g_diyp.s.wButtons & (1<<0xe)) && ((wButtonsPrev & (1<<0xe)) == 0)) { // E button test - Balance mode
         if (!g_InControlState.BalanceMode) {
           g_InControlState.BalanceMode = 1;
@@ -294,9 +297,10 @@ void DIYXBeeController::ControlInput(void)
           g_InControlState.TravelLength.x = 0;
           g_InControlState.TravelLength.z = 0;
           g_InControlState.TravelLength.y = 0;
-
+#ifdef OPT_SINGLELEG      
           g_InControlState.SelectedLeg=255;  //none
           g_InControlState.fSLHold=0;
+#endif
 
           bXBeeControlMode = GPPLAYERMODE;
         } 
@@ -330,11 +334,16 @@ void DIYXBeeController::ControlInput(void)
             MSound( 1, 50, 2000);   //Sound P9, [50\4000]
             g_InControlState.GaitType = iNumButton-1;
 #ifdef DEBUG
-            DBGPrintf("New Gate: %d\n\r", g_InControlState.GaitType);
+#ifdef DBGSerial            
+            DBGSerial.print("New Gate: ");
+            DBGSerial.println(g_InControlState.GaitType, DEC);
+#endif
 #endif
             GaitSelect();
 #ifdef DEBUG
-            DBGPrintf("Output Gate Named\n\r");
+#ifdef DBGSerial
+            DBGSerial.println("Output Gate Named");
+#endif
 #endif
 
 #ifdef DISPLAY_GAIT_NAMES
@@ -349,6 +358,7 @@ void DIYXBeeController::ControlInput(void)
       }
 
       //Switch single leg
+#ifdef OPT_SINGLELEG      
       if (bXBeeControlMode==SINGLELEGMODE) {
         if (iNumButton>=1 && iNumButton<=CNT_LEGS) {
           MSound( 1, 50, 2000);   //Sound P9, [50\4000]
@@ -366,6 +376,7 @@ void DIYXBeeController::ControlInput(void)
         g_InControlState.SelectedLeg=255; // none
         g_InControlState.fSLHold=0;
       }
+#endif
 
       //Body Height - Control depends on how big our packet was (ie which controller we are using) 
       if (g_diystate.cbPacketSize > PKT_MSLIDER)
@@ -491,6 +502,7 @@ void DIYXBeeController::ControlInput(void)
 
       }
 
+#ifdef OPT_SINGLELEG      
       //---------------------------------------------------------------------------------------------------
       //Single Leg Mode
       //---------------------------------------------------------------------------------------------------
@@ -501,6 +513,7 @@ void DIYXBeeController::ControlInput(void)
         g_InControlState.InputTimeDelay = 128 -  max( max( abs(g_diyp.s.bRJoyLR-128),  abs(g_diyp.s.bRJoyUD-128)),  abs(g_diyp.s.bLJoyLR-128)) + (128 -(g_diyp.s.bLSlider)/2);
       }
 
+#endif
       //---------------------------------------------------------------------------------------------------
       // Sequence General Player Mode
       //---------------------------------------------------------------------------------------------------
@@ -580,7 +593,9 @@ void DIYXBeeController::ControlInput(void)
       g_InControlState.TravelLength.y = 0;
       g_BodyYOffset = 0;
       g_BodyYSift = 0;
+#ifdef OPT_SINGLELEG      
       g_InControlState.SelectedLeg = 255;
+#endif
       g_InControlState.fRobotOn = 0;
     }
   }

ReadMe.md

@@ -6,24 +6,17 @@ This is a Work In Progress!  There are no warrantees or Guarantees of any type t
 But I hope it is.
 
 This set of directories are my latest attempt of breaking up the Phoenix code base for the Arduino, 
-into libraries that you can use to build different builds for different Quads (and hopefully Hexapods as well)
+into libraries that you can use to build different builds for many different hexapods as well as a few quads
 
+Quad SUPPORT
+============
 
-Quad SUPPORT WIP
-================
-
-WARNING: This branch is a real WIP to try to add some Quad support.  I also try to make sure that I don't break Hexapod
-support, but again no guarantees.
-
-So at times things that are checked in here may not work. Likewise I am in the process of trying to support other 
-platforms like the Arduino Due, so some changes could cause things not to compile.
-
+WARNING: This branch now has some and maybe even some support for Octopods.  
 
-Note: This branch includes functionality of the changes made by Jeroen (Xan) for the code up on the Lynxmotion 
+Note: This includes functionality of the changes made by Jeroen (Xan) for the code up on the Lynxmotion 
 gethub Quadcode... However I changed the implementation such that you can choose which servos need to be reversed in the 
 hex config file, also not specific to quads. This change required updates to all of the servo Driver files
 
-
 General
 =======