From a3c18d24a7953fbc454c5c8fb93d2d59e72eda55 Mon Sep 17 00:00:00 2001 From: sciencewhiz Date: Fri, 29 May 2026 23:10:42 -0700 Subject: [PATCH] [examples] Add mecanum drive snippets (#8931) shows both polar, and cartesian --- wpilibcExamples/example_projects.bzl | 1 + .../cpp/snippets/MecanumDrive/cpp/Robot.cpp | 78 +++++++++++++++++++ .../src/main/cpp/snippets/snippets.json | 11 +++ wpilibjExamples/example_projects.bzl | 1 + .../wpilib/snippets/mecanumdrive/Robot.java | 75 ++++++++++++++++++ .../java/org/wpilib/snippets/snippets.json | 12 +++ 6 files changed, 178 insertions(+) create mode 100644 wpilibcExamples/src/main/cpp/snippets/MecanumDrive/cpp/Robot.cpp create mode 100644 wpilibjExamples/src/main/java/org/wpilib/snippets/mecanumdrive/Robot.java diff --git a/wpilibcExamples/example_projects.bzl b/wpilibcExamples/example_projects.bzl index 545b093c4a..af0bbb3520 100644 --- a/wpilibcExamples/example_projects.bzl +++ b/wpilibcExamples/example_projects.bzl @@ -72,6 +72,7 @@ SNIPPET_FOLDERS = [ "I2CCommunication", "IntermediateVision", "LimitSwitch", + "MecanumDrive", "MotorControl", "OnboardIMU", "ProfiledPIDFeedforward", diff --git a/wpilibcExamples/src/main/cpp/snippets/MecanumDrive/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/snippets/MecanumDrive/cpp/Robot.cpp new file mode 100644 index 0000000000..27e8c86124 --- /dev/null +++ b/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(); +} +#endif diff --git a/wpilibcExamples/src/main/cpp/snippets/snippets.json b/wpilibcExamples/src/main/cpp/snippets/snippets.json index 1caba12b3b..292d9b591a 100644 --- a/wpilibcExamples/src/main/cpp/snippets/snippets.json +++ b/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", diff --git a/wpilibjExamples/example_projects.bzl b/wpilibjExamples/example_projects.bzl index 68915de68e..d3b78e18b7 100644 --- a/wpilibjExamples/example_projects.bzl +++ b/wpilibjExamples/example_projects.bzl @@ -73,6 +73,7 @@ SNIPPET_FOLDERS = [ "i2ccommunication", "intermediatevision", "limitswitch", + "mecanumdrive", "motorcontrol", "onboardimu", "profiledpidfeedforward", diff --git a/wpilibjExamples/src/main/java/org/wpilib/snippets/mecanumdrive/Robot.java b/wpilibjExamples/src/main/java/org/wpilib/snippets/mecanumdrive/Robot.java new file mode 100644 index 0000000000..09842c84e1 --- /dev/null +++ b/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). + * + *

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); + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/snippets/snippets.json b/wpilibjExamples/src/main/java/org/wpilib/snippets/snippets.json index 8a3edbf56a..10c81a8e9a 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/snippets/snippets.json +++ b/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" } ]