Added Dagu Motor Controller and Fuel Gauge from DSSCircuits

ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino

@@ -45,6 +45,14 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
 
+//#define USE_FG //Enable use of Fuel Gauge for LiPo batteries: http://dsscircuits.com/lipo-fuel-gauge.html
+#undef USE_FG
+
+#ifdef USE_FG
+  #define FG_Add 0x36 //address
+  #include "Wire.h"
+#endif
+
 #define USE_BASE      // Enable the base controller code
 //#undef USE_BASE     // Disable the base controller code
 
@@ -58,8 +66,42 @@
 
    /* The RoboGaia encoder shield */
    #define ROBOGAIA
+
+   //#define DAGU4MOTOR
 #endif
 
+#ifdef DAGU4MOTOR
+   //#define ENCODER_OPTIMIZE_INTERRUPTS //Doesn't compile Encoder library, works for Arduino Due.
+   #define ENCODER_USE_INTERRUPTS
+
+     //Motors
+     const int pwm_a = 4;  //PWM for CH1 LF
+     const int pwm_b = 7;  //PWM for CH4 RR  
+     const int pwm_c = 9;  //PWM for CH2 LR
+     const int pwm_d = 10;  //PWM for CH3 RF
+     const int dir_a = 45;  //DIR for CH1
+     const int dir_b = 43;  //DIR for CH4
+     const int dir_c = 42;  //DIR for CH2
+     const int dir_d = 44;  //DIR for CH3
+
+     //Current Sensors
+     const int CURRENTA = A0; 
+     const int CURRENTB = A1; 
+     const int CURRENTC = A2; 
+     const int CURRENTD = A3; 
+     const int CURRENT_LIMIT = (1024 / 5) * 2.6;  // amps
+
+     //Encoder Interrupts
+     const int encA1 = 2;
+     const int encA2 = 46;
+     const int encB1 = 3;
+     const int encB2 = 47;
+     const int encC1 = 18;
+     const int encC2 = 48;
+     const int encD1 = 19;
+     const int encD2 = 49;
+#endif   
+
 //#define USE_SERVOS  // Enable use of PWM servos as defined in servos.h
 #undef USE_SERVOS     // Disable use of PWM servos
 
@@ -223,6 +265,11 @@ int runCommand() {
     Ko = pid_args[3];
     Serial.println("OK");
     break;
+#endif
+#ifdef USE_FG
+  case FG_R:
+    FG(arg1);
+    break;
 #endif
   default:
     Serial.println("Invalid Command");
@@ -233,6 +280,9 @@ int runCommand() {
 /* Setup function--runs once at startup. */
 void setup() {
   Serial.begin(BAUDRATE);
+  #ifdef USE_FG
+    Wire.begin();
+  #endif
 
 // Initialize the motor controller if used */
 #ifdef USE_BASE

ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h

@@ -20,6 +20,7 @@
 #define ANALOG_WRITE   'x'
 #define LEFT            0
 #define RIGHT           1
+#define FG_R           'v'
 
 #endif
 

ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino

@@ -27,6 +27,24 @@
     if (i == LEFT) return encoders.YAxisReset();
     else return encoders.XAxisReset();
   }
+#elif defined DAGU4MOTOR
+ /* Dagu4Motor Controller\Encoders */
+  #include <Encoder.h>
+  Encoder motor1E(encA1, encA2); 
+  Encoder motor2E(encB1, encB2); 
+
+  /* Wrap the encoder reading function */
+  long readEncoder(int i) {
+    if (i == LEFT) return motor1E.read();
+    else return motor2E.read();
+    return 0;
+  }
+
+  /* Wrap the encoder reset function */
+  void resetEncoder(int i) {
+    if (i == LEFT) motor1E.write(0);
+    else motor2E.write(0);
+  }
 #else
   #error A encoder driver must be selected!
 #endif

ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino

@@ -55,6 +55,53 @@
     setMotorSpeed(LEFT, leftSpeed);
     setMotorSpeed(RIGHT, rightSpeed);
   }
+#elif defined DAGU4MOTOR
+/* Include the FriedCircuits library */
+  #include "Dagu4Motor.h"
+
+  /* Create the motor driver object */
+  Dagu4Motor motor1(pwm_a, dir_a, CURRENTA, encA1, encA2); 
+  Dagu4Motor motor2(pwm_b, dir_b, CURRENTB, encB1, encB2); 
+  Dagu4Motor motor3(pwm_c, dir_c, CURRENTC, encC1, encC2); 
+  Dagu4Motor motor4(pwm_d, dir_d, CURRENTD, encD1, encD2); 
+
+  /* Wrap the motor driver initialization */
+  void initMotorController() {
+    motor1.begin();
+    motor2.begin();
+    motor3.begin();
+    motor4.begin();
+  }
+
+  /* Wrap the drive motor set speed function */
+  void setMotorSpeed(int i, int spd) {
+    if (i == LEFT){
+      motor1.setMotorDirection(false);
+      motor2.setMotorDirection(true);
+
+      motor1.setSpeed(spd);
+      motor2.setSpeed(spd);      
+
+
+    }
+    else {
+
+      motor3.setMotorDirection(false);
+      motor4.setMotorDirection(true);
+
+      motor3.setSpeed(spd);
+      motor4.setSpeed(spd); 
+
+   }
+  }
+
+  // A convenience function for setting both motor speeds
+  void setMotorSpeeds(int leftSpeed, int rightSpeed) {
+    setMotorSpeed(LEFT, leftSpeed);
+    setMotorSpeed(RIGHT, rightSpeed);
+  }
+
+
 #else
   #error A motor driver must be selected!
 #endif

ros_arduino_firmware/src/libraries/ROSArduinoBridge/sensors.h

@@ -32,3 +32,40 @@ long Ping(int pin) {
   return(range);
 }
 
+
+#ifdef USE_FG
+void FG(char cmd){
+  unsigned int byte1 = 0;
+  unsigned int byte2 = 0;
+  unsigned int comb = 0;
+
+ if (cmd == 1){
+
+  Wire.beginTransmission(FG_Add);
+  Wire.write(0x02);
+  Wire.endTransmission();
+  Wire.requestFrom(FG_Add,2);
+  byte1 = Wire.read();
+  byte2 = Wire.read();
+  comb = ((byte1 << 4));
+  comb |= (byte2 >> 4);
+  Serial.println(map(comb,0x000,0xFFF,0,100000)/10000.0,4);
+
+
+ }
+ else if (cmd == 2){
+  Wire.beginTransmission(FG_Add);
+  Wire.write(0x04);
+  Wire.endTransmission();
+  Wire.requestFrom(FG_Add,2);
+  byte1 = Wire.read();
+  byte2 = Wire.read();
+  comb = ((byte1 << 8));
+  comb |= (byte2);
+  Serial.println(map(comb,0x0000,0x6400,0,10000)/100.0,2);
+
+ }
+ else { Serial.println("Invalid Arg");} 
+
+}
+#endif