diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino index 3348af5a..ac864e42 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino @@ -45,29 +45,36 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -//#define USE_BASE // Enable the base controller code -#undef USE_BASE // Disable the base controller code +#define USE_BASE // Enable the base controller code +//#undef USE_BASE // Disable the base controller code /* Define the motor controller and encoder library you are using */ #ifdef USE_BASE /* The Pololu VNH5019 dual motor driver shield */ - #define POLOLU_VNH5019 + //#define POLOLU_VNH5019 /* The Pololu MC33926 dual motor driver shield */ //#define POLOLU_MC33926 /* The RoboGaia encoder shield */ - #define ROBOGAIA + //#define ROBOGAIA + /* The Pololu ZUMO 32U4 integrated motor driver */ + #define ZUMO32U4_MOTOR_DRIVER + /* Encoders directly attached to Arduino board */ //#define ARDUINO_ENC_COUNTER /* L298 Motor driver*/ //#define L298_MOTOR_DRIVER + + /* ZUMO 32U4 integrated encoder */ + #define ZUMO32U4 + #endif -#define USE_SERVOS // Enable use of PWM servos as defined in servos.h -//#undef USE_SERVOS // Disable use of PWM servos +//#define USE_SERVOS // Enable use of PWM servos as defined in servos.h +#undef USE_SERVOS // Disable use of PWM servos /* Serial port baud rate */ #define BAUDRATE 57600 diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino index ef2b8b11..16746490 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino @@ -68,6 +68,27 @@ return; } } +#elif defined(ZUMO32U4) +#include + + /* Create the encoders object */ + Zumo32U4Encoders encoders; + + /* Wrap the encoder reading function */ + long readEncoder(int i) { + if (i == LEFT) + return encoders.getCountsLeft(); + else + return encoders.getCountsRight(); + } + + /* Wrap the encoder reset function */ + void resetEncoder(int i) { + if (i == LEFT) + encoders.getCountsAndResetLeft(); + else + encoders.getCountsAndResetRight(); + } #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 facb8a8a..6f01bd58 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino @@ -82,6 +82,29 @@ } } + void setMotorSpeeds(int leftSpeed, int rightSpeed) { + setMotorSpeed(LEFT, leftSpeed); + setMotorSpeed(RIGHT, rightSpeed); + } +#elif defined ZUMO32U4_MOTOR_DRIVER + /* Include the Pololu library */ + #include + Zumo32U4Motors motors; + + /* Wrap the motor driver initialization */ + void initMotorController() { +// motors.init(); + } + + /* Wrap the drive motor set speed function */ + void setMotorSpeed(int i, int spd) { + if (i == LEFT) + motors.setLeftSpeed(spd); + else + motors.setRightSpeed(spd); + } + + // A convenience function for setting both motor speeds void setMotorSpeeds(int leftSpeed, int rightSpeed) { setMotorSpeed(LEFT, leftSpeed); setMotorSpeed(RIGHT, rightSpeed); diff --git a/ros_arduino_python/config/zumo32u4_arduino_params.yaml b/ros_arduino_python/config/zumo32u4_arduino_params.yaml new file mode 100644 index 00000000..f0948db1 --- /dev/null +++ b/ros_arduino_python/config/zumo32u4_arduino_params.yaml @@ -0,0 +1,54 @@ +# For a direct USB cable connection, the port name is typically +# /dev/ttyACM# where is # is a number such as 0, 1, 2, etc +# For a wireless connection like XBee, the port is typically +# /dev/ttyUSB# where # is a number such as 0, 1, 2, etc. + +port: /dev/ttyACM0 +baud: 57600 +timeout: 0.1 + +rate: 50 +sensorstate_rate: 10 + +use_base_controller: True +base_controller_rate: 10 + +# For a robot that uses base_footprint, change base_frame to base_footprint +base_frame: base_link + +# === Robot drivetrain parameters +#Based on Pololu 22T Track that comes with Zumo32U4: +wheel_diameter: 0.035 +wheel_track: 0.084 +encoder_resolution: 600 # from Pololu for 50:1 motors +gear_reduction: 1.0 +motors_reversed: False + +# === PID parameters +Kp: 10 +Kd: 12 +Ki: 0 +Ko: 50 +accel_limit: 1.0 + +# === Sensor definitions. Examples only - edit for your robot. +# Sensor type can be one of the follow (case sensitive!): +# * Ping +# * GP2D12 +# * Analog +# * Digital +# * PololuMotorCurrent +# * PhidgetsVoltage +# * PhidgetsCurrent (20 Amp, DC) + + + +sensors: { + #motor_current_left: {pin: 0, type: PololuMotorCurrent, rate: 5}, + #motor_current_right: {pin: 1, type: PololuMotorCurrent, rate: 5}, + #ir_front_center: {pin: 2, type: GP2D12, rate: 10}, + #sonar_front_center: {pin: 5, type: Ping, rate: 10}, + arduino_red_led: {pin: 17, type: Digital, rate: 5, direction: output}, + #arduino_green_led: {pin: PD5, type: Digital, rate: 5, direction: output}, + arduino_led: {pin: 13, type: Digital, rate: 5, direction: output} +} diff --git a/ros_arduino_python/launch/arduino.launch b/ros_arduino_python/launch/arduino.launch index 0dd803a9..9e4d5d96 100755 --- a/ros_arduino_python/launch/arduino.launch +++ b/ros_arduino_python/launch/arduino.launch @@ -1,5 +1,5 @@ - +