[examples] Add mecanum drive snippets (#8931)

shows both polar, and cartesian

wpilibcExamples/example_projects.bzl

@@ -72,6 +72,7 @@ SNIPPET_FOLDERS = [
     "I2CCommunication",
     "IntermediateVision",
     "LimitSwitch",
+    "MecanumDrive",
     "MotorControl",
     "OnboardIMU",
     "ProfiledPIDFeedforward",

wpilibcExamples/src/main/cpp/snippets/MecanumDrive/cpp/Robot.cpp

@@ -0,0 +1,78 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "wpi/drive/MecanumDrive.hpp"
+#include "wpi/driverstation/Joystick.hpp"
+#include "wpi/framework/TimedRobot.hpp"
+#include "wpi/hardware/imu/OnboardIMU.hpp"
+#include "wpi/hardware/motor/PWMSparkMax.hpp"
+
+/**
+ * This is a sample program that uses mecanum drive with a gyro sensor to
+ * maintain rotation vectors in relation to the starting orientation of the
+ * robot (field-oriented controls).
+ *
+ * Finally, short code snippets show how to use cartesian and polar drive
+ * methods for wpilib-docs.
+ * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/motors/wpi-drive-classes.html
+ */
+class Robot : public wpi::TimedRobot {
+ public:
+  Robot() {
+    wpi::util::SendableRegistry::AddChild(&robotDrive, &frontLeft);
+    wpi::util::SendableRegistry::AddChild(&robotDrive, &rearLeft);
+    wpi::util::SendableRegistry::AddChild(&robotDrive, &frontRight);
+    wpi::util::SendableRegistry::AddChild(&robotDrive, &rearRight);
+
+    // Invert the right side motors. You may need to change or remove this to
+    // match your robot.
+    frontRight.SetInverted(true);
+    rearRight.SetInverted(true);
+  }
+
+  /**
+   * Mecanum drive is used with the gyro angle as an input.
+   */
+  void TeleopPeriodic() override {
+    /* Use the joystick Y axis for forward movement, X axis for lateral
+     * movement, and Z axis for rotation, field oriented.
+     */
+    robotDrive.DriveCartesian(-joystick.GetY(), -joystick.GetX(),
+                              -joystick.GetZ(), imu.GetRotation2d());
+    // Drive using the X, Y, and Z axes of the joystick.
+    robotDrive.DriveCartesian(-joystick.GetY(), -joystick.GetX(),
+                              -joystick.GetZ());
+    // Drive at 45 degrees relative to the robot, at the speed given by the Y
+    // axis of the joystick, with no rotation.
+    robotDrive.DrivePolar(-joystick.GetY(), 45_deg, 0);
+  }
+
+ private:
+  static constexpr int kFrontLeftMotorPort = 0;
+  static constexpr int kRearLeftMotorPort = 1;
+  static constexpr int kFrontRightMotorPort = 2;
+  static constexpr int kRearRightMotorPort = 3;
+  static constexpr wpi::OnboardIMU::MountOrientation kIMUMountOrientation =
+      wpi::OnboardIMU::FLAT;
+  static constexpr int kJoystickPort = 0;
+
+  wpi::PWMSparkMax frontLeft{kFrontLeftMotorPort};
+  wpi::PWMSparkMax rearLeft{kRearLeftMotorPort};
+  wpi::PWMSparkMax frontRight{kFrontRightMotorPort};
+  wpi::PWMSparkMax rearRight{kRearRightMotorPort};
+  wpi::MecanumDrive robotDrive{
+      [&](double output) { frontLeft.SetThrottle(output); },
+      [&](double output) { rearLeft.SetThrottle(output); },
+      [&](double output) { frontRight.SetThrottle(output); },
+      [&](double output) { rearRight.SetThrottle(output); }};
+
+  wpi::OnboardIMU imu{kIMUMountOrientation};
+  wpi::Joystick joystick{kJoystickPort};
+};
+
+#ifndef RUNNING_WPILIB_TESTS
+int main() {
+  return wpi::StartRobot<Robot>();
+}
+#endif

wpilibcExamples/src/main/cpp/snippets/snippets.json

@@ -304,6 +304,17 @@
     "foldername": "Solenoid",
     "gradlebase": "cpp"
   },
+  {
+    "name": "Mecanum Drive",
+    "description": "Show mecanum drive control with cartesian and polar drive in teleop.",
+    "tags": [
+      "Mecanum Drive",
+      "Gyro",
+      "Joystick"
+    ],
+    "foldername": "MecanumDrive",
+    "gradlebase": "cpp"
+  },
   {
     "name": "Differential Drive",
     "description": "Show differential drive control with tank, arcade, and curvature drive in teleop",

wpilibjExamples/example_projects.bzl

@@ -73,6 +73,7 @@ SNIPPET_FOLDERS = [
     "i2ccommunication",
     "intermediatevision",
     "limitswitch",
+    "mecanumdrive",
     "motorcontrol",
     "onboardimu",
     "profiledpidfeedforward",

wpilibjExamples/src/main/java/org/wpilib/snippets/mecanumdrive/Robot.java

@@ -0,0 +1,75 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.wpilib.snippets.mecanumdrive;
+
+import org.wpilib.drive.MecanumDrive;
+import org.wpilib.driverstation.Joystick;
+import org.wpilib.framework.TimedRobot;
+import org.wpilib.hardware.imu.OnboardIMU;
+import org.wpilib.hardware.motor.PWMSparkMax;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.util.sendable.SendableRegistry;
+
+/**
+ * This is a sample program that uses mecanum drive with a gyro sensor to maintain rotation vectors
+ * in relation to the starting orientation of the robot (field-oriented controls).
+ *
+ * <p>Finally, short code snippets show how to use cartesian and polar drive methods for
+ * wpilib-docs.
+ * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/motors/wpi-drive-classes.html
+ */
+public class Robot extends TimedRobot {
+  private static final int kFrontLeftChannel = 0;
+  private static final int kRearLeftChannel = 1;
+  private static final int kFrontRightChannel = 2;
+  private static final int kRearRightChannel = 3;
+  private static final OnboardIMU.MountOrientation kIMUMountOrientation =
+      OnboardIMU.MountOrientation.FLAT;
+  private static final int kJoystickPort = 0;
+
+  private final MecanumDrive robotDrive;
+  private final OnboardIMU imu = new OnboardIMU(kIMUMountOrientation);
+  private final Joystick joystick = new Joystick(kJoystickPort);
+
+  /** Called once at the beginning of the robot program. */
+  public Robot() {
+    PWMSparkMax frontLeft = new PWMSparkMax(kFrontLeftChannel);
+    PWMSparkMax rearLeft = new PWMSparkMax(kRearLeftChannel);
+    PWMSparkMax frontRight = new PWMSparkMax(kFrontRightChannel);
+    PWMSparkMax rearRight = new PWMSparkMax(kRearRightChannel);
+
+    // Invert the right side motors.
+    // You may need to change or remove this to match your robot.
+    frontRight.setInverted(true);
+    rearRight.setInverted(true);
+
+    robotDrive =
+        new MecanumDrive(
+            frontLeft::setThrottle,
+            rearLeft::setThrottle,
+            frontRight::setThrottle,
+            rearRight::setThrottle);
+
+    SendableRegistry.addChild(robotDrive, frontLeft);
+    SendableRegistry.addChild(robotDrive, rearLeft);
+    SendableRegistry.addChild(robotDrive, frontRight);
+    SendableRegistry.addChild(robotDrive, rearRight);
+  }
+
+  /** Mecanum drive is used with the gyro angle as an input. */
+  @Override
+  public void teleopPeriodic() {
+    // Use the joystick Y axis for forward movement, X axis for lateral
+    // movement, and Z axis for rotation, and the gyro angle for field-oriented controls.
+    robotDrive.driveCartesian(
+        -joystick.getY(), -joystick.getX(), -joystick.getZ(), imu.getRotation2d());
+    // Use the joystick Y axis for forward movement, X axis for lateral
+    // movement, and Z axis for rotation.
+    robotDrive.driveCartesian(-joystick.getY(), -joystick.getX(), -joystick.getZ());
+    // Drive at 45 degrees relative to the robot, at the speed given by the Y axis of the joystick,
+    // with no rotation.
+    robotDrive.drivePolar(-joystick.getY(), Rotation2d.fromDegrees(45), 0);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/snippets/snippets.json

@@ -343,5 +343,17 @@
     "foldername": "differentialdrive",
     "gradlebase": "java",
     "robotclass": "Robot"
+  },
+  {
+    "name": "Mecanum Drive",
+    "description": "Show mecanum drive control with cartesian and polar drive in teleop.",
+    "tags": [
+      "Mecanum Drive",
+      "Gyro",
+      "Joystick"
+    ],
+    "foldername": "mecanumdrive",
+    "gradlebase": "java",
+    "robotclass": "Robot"
   }
 ]