zumo32u4 initial mods

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

ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino

@@ -68,6 +68,27 @@
       return;
     }
   }
+#elif defined(ZUMO32U4)
+#include <Zumo32U4Encoders.h>
+
+  /* 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

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.h>
+  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);

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}
+}

ros_arduino_python/launch/arduino.launch

@@ -1,5 +1,5 @@
 <launch>
    <node name="arduino" pkg="ros_arduino_python" type="arduino_node.py" output="screen">
-      <rosparam file="$(find ros_arduino_python)/config/my_arduino_params.yaml" command="load" />
+      <rosparam file="$(find ros_arduino_python)/config/zumo32u4_arduino_params.yaml" command="load" />
    </node>
 </launch>