diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino index 77322d2e..e3488618 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino +++ b/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 diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h index a1c6672f..1f727a9c 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h +++ b/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 diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino index af9434df..697dac62 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino +++ b/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 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 diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino index c822ef58..cbb1a600 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino +++ b/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 diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/sensors.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/sensors.h index 75caeb77..220c4e2d 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/sensors.h +++ b/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