diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java new file mode 100644 index 0000000000..af50dc8589 --- /dev/null +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java @@ -0,0 +1,353 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import java.util.function.Consumer; +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.controller.ProfiledPIDController; +import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics; +import edu.wpi.first.wpilibj.kinematics.MecanumDriveMotorVoltages; +import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds; +import edu.wpi.first.wpilibj.trajectory.Trajectory; + +import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; + +/** + * A command that uses two PID controllers ({@link PIDController}) and a + * ProfiledPIDController ({@link ProfiledPIDController}) to follow a trajectory + * {@link Trajectory} with a mecanum drive. + * + *

The command handles trajectory-following, + * Velocity PID calculations, and feedforwards internally. This + * is intended to be a more-or-less "complete solution" that can be used by teams without a great + * deal of controls expertise. + * + *

Advanced teams seeking more flexibility (for example, those who wish to use the onboard + * PID functionality of a "smart" motor controller) may use the secondary constructor that omits + * the PID and feedforward functionality, returning only the raw wheel speeds from the PID + * controllers. + * + *

The robot angle controller does not follow the angle given by + * the trajectory but rather goes to the angle given in the final state of the trajectory. + */ + +@SuppressWarnings({"PMD.TooManyFields", "MemberName"}) +public class MecanumControllerCommand extends CommandBase { + private final Timer m_timer = new Timer(); + private MecanumDriveWheelSpeeds m_prevSpeeds; + private double m_prevTime; + private Pose2d m_finalPose; + private final boolean m_usePID; + + private final Trajectory m_trajectory; + private final Supplier m_pose; + private final SimpleMotorFeedforward m_feedforward; + private final MecanumDriveKinematics m_kinematics; + private final PIDController m_xController; + private final PIDController m_yController; + private final ProfiledPIDController m_thetaController; + private final double m_maxWheelVelocityMetersPerSecond; + private final PIDController m_frontLeftController; + private final PIDController m_rearLeftController; + private final PIDController m_frontRightController; + private final PIDController m_rearRightController; + private final Supplier m_currentWheelSpeeds; + private final Consumer m_outputDriveVoltages; + private final Consumer m_outputWheelSpeeds; + + /** + * Constructs a new MecanumControllerCommand that when executed will follow the provided + * trajectory. PID control and feedforward are handled internally. Outputs are scaled from -12 to + * 12 as a voltage output to the motor. + * + *

Note: The controllers will *not* set the outputVolts to zero upon completion of the path + * this is left to the user, since it is not appropriate for paths with nonstationary endstates. + * + *

Note 2: The rotation controller will calculate the rotation based on the final pose in the + * trajectory, not the poses at each time step. + * + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose - use one of + * the odometry classes to provide this. + * @param feedforward The feedforward to use for the drivetrain. + * @param kinematics The kinematics for the robot drivetrain. + * @param xController The Trajectory Tracker PID controller + * for the robot's x position. + * @param yController The Trajectory Tracker PID controller + * for the robot's y position. + * @param thetaController The Trajectory Tracker PID controller + * for angle for the robot. + * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel. + * @param frontLeftController The front left wheel velocity PID. + * @param rearLeftController The rear left wheel velocity PID. + * @param frontRightController The front right wheel velocity PID. + * @param rearRightController The rear right wheel velocity PID. + * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing + * the current wheel speeds. + * @param outputDriveVoltages A MecanumDriveMotorVoltages object containing + * the output motor voltages. + * @param requirements The subsystems to require. + */ + + @SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"}) + public MecanumControllerCommand(Trajectory trajectory, + Supplier pose, + SimpleMotorFeedforward feedforward, + MecanumDriveKinematics kinematics, + + PIDController xController, + PIDController yController, + ProfiledPIDController thetaController, + + double maxWheelVelocityMetersPerSecond, + + PIDController frontLeftController, + PIDController rearLeftController, + PIDController frontRightController, + PIDController rearRightController, + + Supplier currentWheelSpeeds, + + Consumer outputDriveVoltages, + Subsystem... requirements) { + m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand"); + m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand"); + m_feedforward = requireNonNullParam(feedforward, "feedforward", "MecanumControllerCommand"); + m_kinematics = requireNonNullParam(kinematics, "kinematics", "MecanumControllerCommand"); + + m_xController = requireNonNullParam(xController, "xController", + "MecanumControllerCommand"); + m_yController = requireNonNullParam(yController, "yController", + "MecanumControllerCommand"); + m_thetaController = requireNonNullParam(thetaController, "thetaController", + "MecanumControllerCommand"); + + m_maxWheelVelocityMetersPerSecond = requireNonNullParam(maxWheelVelocityMetersPerSecond, + "maxWheelVelocityMetersPerSecond", "MecanumControllerCommand"); + + m_frontLeftController = requireNonNullParam(frontLeftController, + "frontLeftController", "MecanumControllerCommand"); + m_rearLeftController = requireNonNullParam(rearLeftController, + "rearLeftController", "MecanumControllerCommand"); + m_frontRightController = requireNonNullParam(frontRightController, + "frontRightController", "MecanumControllerCommand"); + m_rearRightController = requireNonNullParam(rearRightController, + "rearRightController", "MecanumControllerCommand"); + + m_currentWheelSpeeds = requireNonNullParam(currentWheelSpeeds, + "currentWheelSpeeds", "MecanumControllerCommand"); + + m_outputDriveVoltages = requireNonNullParam(outputDriveVoltages, + "outputDriveVoltages", "MecanumControllerCommand"); + + m_outputWheelSpeeds = null; + + m_usePID = true; + + addRequirements(requirements); + } + + /** + * Constructs a new MecanumControllerCommand that when executed will follow the provided + * trajectory. The user should implement a velocity PID on the desired output wheel velocities. + * + *

Note: The controllers will *not* set the outputVolts to zero upon completion of the path - + * this is left to the user, since it is not appropriate for paths with non-stationary end-states. + * + *

Note2: The rotation controller will calculate the rotation based on the final pose + * in the trajectory, not the poses at each time step. + * + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose - use one of + * the odometry classes to provide this. + * @param kinematics The kinematics for the robot drivetrain. + * @param xController The Trajectory Tracker PID controller + * for the robot's x position. + * @param yController The Trajectory Tracker PID controller + * for the robot's y position. + * @param thetaController The Trajectory Tracker PID controller + * for angle for the robot. + * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel. + * @param outputWheelSpeeds A MecanumDriveWheelSpeeds object containing + * the output wheel speeds. + * @param requirements The subsystems to require. + */ + + @SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"}) + public MecanumControllerCommand(Trajectory trajectory, + Supplier pose, + MecanumDriveKinematics kinematics, + PIDController xController, + PIDController yController, + ProfiledPIDController thetaController, + + double maxWheelVelocityMetersPerSecond, + + Consumer outputWheelSpeeds, + Subsystem... requirements) { + m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand"); + m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand"); + m_feedforward = new SimpleMotorFeedforward(0, 0, 0); + m_kinematics = requireNonNullParam(kinematics, + "kinematics", "MecanumControllerCommand"); + + m_xController = requireNonNullParam(xController, + "xController", "MecanumControllerCommand"); + m_yController = requireNonNullParam(yController, + "xController", "MecanumControllerCommand"); + m_thetaController = requireNonNullParam(thetaController, + "thetaController", "MecanumControllerCommand"); + + m_maxWheelVelocityMetersPerSecond = requireNonNullParam(maxWheelVelocityMetersPerSecond, + "maxWheelVelocityMetersPerSecond", "MecanumControllerCommand"); + + m_frontLeftController = null; + m_rearLeftController = null; + m_frontRightController = null; + m_rearRightController = null; + + m_currentWheelSpeeds = null; + + m_outputWheelSpeeds = requireNonNullParam(outputWheelSpeeds, + "outputWheelSpeeds", "MecanumControllerCommand"); + + m_outputDriveVoltages = null; + + m_usePID = false; + + addRequirements(requirements); + } + + @Override + public void initialize() { + var initialState = m_trajectory.sample(0); + + // Sample final pose to get robot rotation + m_finalPose = m_trajectory.sample(m_trajectory.getTotalTimeSeconds()).poseMeters; + + var initialXVelocity = initialState.velocityMetersPerSecond + * initialState.poseMeters.getRotation().getCos(); + var initialYVelocity = initialState.velocityMetersPerSecond + * initialState.poseMeters.getRotation().getSin(); + + m_prevSpeeds = m_kinematics.toWheelSpeeds( + new ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0)); + + m_timer.reset(); + m_timer.start(); + } + + @Override + @SuppressWarnings("LocalVariableName") + public void execute() { + double curTime = m_timer.get(); + double dt = curTime - m_prevTime; + + var desiredState = m_trajectory.sample(curTime); + var desiredPose = desiredState.poseMeters; + + var poseError = desiredPose.relativeTo(m_pose.get()); + + double targetXVel = m_xController.calculate( + m_pose.get().getTranslation().getX(), + desiredPose.getTranslation().getX()); + + double targetYVel = m_yController.calculate( + m_pose.get().getTranslation().getY(), + desiredPose.getTranslation().getY()); + + // The robot will go to the desired rotation of the final pose in the trajectory, + // not following the poses at individual states. + double targetAngularVel = m_thetaController.calculate( + m_pose.get().getRotation().getRadians(), + m_finalPose.getRotation().getRadians()); + + double vRef = desiredState.velocityMetersPerSecond; + + targetXVel += vRef * poseError.getRotation().getCos(); + targetYVel += vRef * poseError.getRotation().getSin(); + + var targetChassisSpeeds = new ChassisSpeeds(targetXVel, targetYVel, targetAngularVel); + + var targetWheelSpeeds = m_kinematics.toWheelSpeeds(targetChassisSpeeds); + + targetWheelSpeeds.normalize(m_maxWheelVelocityMetersPerSecond); + + var frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeftMetersPerSecond; + var rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeftMetersPerSecond; + var frontRightSpeedSetpoint = targetWheelSpeeds.frontRightMetersPerSecond; + var rearRightSpeedSetpoint = targetWheelSpeeds.rearRightMetersPerSecond; + + double frontLeftOutput; + double rearLeftOutput; + double frontRightOutput; + double rearRightOutput; + + if (m_usePID) { + final double frontLeftFeedforward = m_feedforward.calculate(frontLeftSpeedSetpoint, + (frontLeftSpeedSetpoint - m_prevSpeeds.frontLeftMetersPerSecond) / dt); + + final double rearLeftFeedforward = m_feedforward.calculate(rearLeftSpeedSetpoint, + (rearLeftSpeedSetpoint - m_prevSpeeds.rearLeftMetersPerSecond) / dt); + + final double frontRightFeedforward = m_feedforward.calculate(frontRightSpeedSetpoint, + (frontRightSpeedSetpoint - m_prevSpeeds.frontRightMetersPerSecond) / dt); + + final double rearRightFeedforward = m_feedforward.calculate(rearRightSpeedSetpoint, + (rearRightSpeedSetpoint - m_prevSpeeds.rearRightMetersPerSecond) / dt); + + frontLeftOutput = frontLeftFeedforward + m_frontLeftController.calculate( + m_currentWheelSpeeds.get().frontLeftMetersPerSecond, + frontLeftSpeedSetpoint); + + rearLeftOutput = rearLeftFeedforward + m_rearLeftController.calculate( + m_currentWheelSpeeds.get().rearLeftMetersPerSecond, + rearLeftSpeedSetpoint); + + frontRightOutput = frontRightFeedforward + m_frontRightController.calculate( + m_currentWheelSpeeds.get().frontRightMetersPerSecond, + frontRightSpeedSetpoint); + + rearRightOutput = rearRightFeedforward + m_rearRightController.calculate( + m_currentWheelSpeeds.get().rearRightMetersPerSecond, + rearRightSpeedSetpoint); + + m_outputDriveVoltages.accept(new MecanumDriveMotorVoltages( + frontLeftOutput, + frontRightOutput, + rearLeftOutput, + rearRightOutput)); + + } else { + m_outputWheelSpeeds.accept(new MecanumDriveWheelSpeeds( + frontLeftSpeedSetpoint, + frontRightSpeedSetpoint, + rearLeftSpeedSetpoint, + rearRightSpeedSetpoint)); + } + + m_prevTime = curTime; + m_prevSpeeds = targetWheelSpeeds; + } + + @Override + public void end(boolean interrupted) { + m_timer.stop(); + } + + @Override + public boolean isFinished() { + return m_timer.hasPeriodPassed(m_trajectory.getTotalTimeSeconds()); + } +} diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java new file mode 100644 index 0000000000..b05dc201f0 --- /dev/null +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java @@ -0,0 +1,158 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + + +import java.util.function.Consumer; +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.controller.ProfiledPIDController; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics; +import edu.wpi.first.wpilibj.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.trajectory.Trajectory; + +import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; + +/** + * A command that uses two PID controllers ({@link PIDController}) and a + * ProfiledPIDController ({@link ProfiledPIDController}) to follow a trajectory + * {@link Trajectory} with a swerve drive. + * + *

This command outputs the raw desired Swerve Module States ({@link SwerveModuleState}) + * in an array. The desired wheel and module rotation velocities should be taken + * from those and used in velocity PIDs. + * + *

The robot angle controller does not follow the angle given by + * the trajectory but rather goes to the angle given in the final state of the trajectory. + */ + +@SuppressWarnings("MemberName") +public class SwerveControllerCommand extends CommandBase { + private final Timer m_timer = new Timer(); + private Pose2d m_finalPose; + + private final Trajectory m_trajectory; + private final Supplier m_pose; + private final SwerveDriveKinematics m_kinematics; + private final PIDController m_xController; + private final PIDController m_yController; + private final ProfiledPIDController m_thetaController; + private final Consumer m_outputModuleStates; + + /** + * Constructs a new SwerveControllerCommand that when executed will follow the provided + * trajectory. This command will not return output voltages but rather raw module states from the + * position controllers which need to be put into a velocity PID. + * + *

Note: The controllers will *not* set the outputVolts to zero upon completion of the path- + * this is left to the user, since it is not appropriate for paths with nonstationary endstates. + * + *

Note 2: The rotation controller will calculate the rotation based on the final pose + * in the trajectory, not the poses at each time step. + * + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose - use one of + * the odometry classes to provide this. + * @param kinematics The kinematics for the robot drivetrain. + * @param xController The Trajectory Tracker PID controller + * for the robot's x position. + * @param yController The Trajectory Tracker PID controller + * for the robot's y position. + * @param thetaController The Trajectory Tracker PID controller + * for angle for the robot. + * @param outputModuleStates The raw output module states from the + * position controllers. + * @param requirements The subsystems to require. + */ + + @SuppressWarnings("ParameterName") + public SwerveControllerCommand(Trajectory trajectory, + Supplier pose, + SwerveDriveKinematics kinematics, + PIDController xController, + PIDController yController, + ProfiledPIDController thetaController, + + Consumer outputModuleStates, + Subsystem... requirements) { + m_trajectory = requireNonNullParam(trajectory, "trajectory", "SwerveControllerCommand"); + m_pose = requireNonNullParam(pose, "pose", "SwerveControllerCommand"); + m_kinematics = requireNonNullParam(kinematics, "kinematics", "SwerveControllerCommand"); + + m_xController = requireNonNullParam(xController, + "xController", "SwerveControllerCommand"); + m_yController = requireNonNullParam(yController, + "xController", "SwerveControllerCommand"); + m_thetaController = requireNonNullParam(thetaController, + "thetaController", "SwerveControllerCommand"); + + m_outputModuleStates = requireNonNullParam(outputModuleStates, + "frontLeftOutput", "SwerveControllerCommand"); + addRequirements(requirements); + } + + @Override + public void initialize() { + // Sample final pose to get robot rotation + m_finalPose = m_trajectory.sample(m_trajectory.getTotalTimeSeconds()).poseMeters; + + m_timer.reset(); + m_timer.start(); + } + + @Override + @SuppressWarnings("LocalVariableName") + public void execute() { + double curTime = m_timer.get(); + + var desiredState = m_trajectory.sample(curTime); + var desiredPose = desiredState.poseMeters; + + var poseError = desiredPose.relativeTo(m_pose.get()); + + double targetXVel = m_xController.calculate( + m_pose.get().getTranslation().getX(), + desiredPose.getTranslation().getX()); + + double targetYVel = m_yController.calculate( + m_pose.get().getTranslation().getY(), + desiredPose.getTranslation().getY()); + + // The robot will go to the desired rotation of the final pose in the trajectory, + // not following the poses at individual states. + double targetAngularVel = m_thetaController.calculate( + m_pose.get().getRotation().getRadians(), + m_finalPose.getRotation().getRadians()); + + double vRef = desiredState.velocityMetersPerSecond; + + targetXVel += vRef * poseError.getRotation().getCos(); + targetYVel += vRef * poseError.getRotation().getSin(); + + var targetChassisSpeeds = new ChassisSpeeds(targetXVel, targetYVel, targetAngularVel); + + var targetModuleStates = m_kinematics.toSwerveModuleStates(targetChassisSpeeds); + + m_outputModuleStates.accept(targetModuleStates); + + } + + @Override + public void end(boolean interrupted) { + m_timer.stop(); + } + + @Override + public boolean isFinished() { + return m_timer.hasPeriodPassed(m_trajectory.getTotalTimeSeconds()); + } +} diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp new file mode 100644 index 0000000000..15fb254fbe --- /dev/null +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp @@ -0,0 +1,187 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/MecanumControllerCommand.h" + +using namespace frc2; +using namespace units; + +MecanumControllerCommand::MecanumControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::SimpleMotorFeedforward feedforward, + frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, + frc2::PIDController yController, + frc::ProfiledPIDController thetaController, + units::meters_per_second_t maxWheelVelocity, + std::function currentWheelSpeeds, + frc2::PIDController frontLeftController, + frc2::PIDController rearLeftController, + frc2::PIDController frontRightController, + frc2::PIDController rearRightController, + std::function + output, + std::initializer_list requirements) + : m_trajectory(trajectory), + m_pose(pose), + m_feedforward(feedforward), + m_kinematics(kinematics), + m_xController(std::make_unique(xController)), + m_yController(std::make_unique(yController)), + m_thetaController( + std::make_unique>( + thetaController)), + m_maxWheelVelocity(maxWheelVelocity), + m_frontLeftController( + std::make_unique(frontLeftController)), + m_rearLeftController( + std::make_unique(rearLeftController)), + m_frontRightController( + std::make_unique(frontRightController)), + m_rearRightController( + std::make_unique(rearRightController)), + m_currentWheelSpeeds(currentWheelSpeeds), + m_outputVolts(output), + m_usePID(true) { + AddRequirements(requirements); +} + +MecanumControllerCommand::MecanumControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, + frc2::PIDController yController, + frc::ProfiledPIDController thetaController, + units::meters_per_second_t maxWheelVelocity, + std::function + output, + std::initializer_list requirements) + : m_trajectory(trajectory), + m_pose(pose), + m_kinematics(kinematics), + m_xController(std::make_unique(xController)), + m_yController(std::make_unique(yController)), + m_thetaController( + std::make_unique>( + thetaController)), + m_maxWheelVelocity(maxWheelVelocity), + m_outputVel(output), + m_usePID(false) { + AddRequirements(requirements); +} + +void MecanumControllerCommand::Initialize() { + m_prevTime = 0_s; + auto initialState = m_trajectory.Sample(0_s); + + auto initialXVelocity = + initialState.velocity * initialState.pose.Rotation().Cos(); + auto initialYVelocity = + initialState.velocity * initialState.pose.Rotation().Sin(); + + m_prevSpeeds = m_kinematics.ToWheelSpeeds( + frc::ChassisSpeeds{initialXVelocity, initialYVelocity, 0_rad_per_s}); + + m_timer.Reset(); + m_timer.Start(); + if (m_usePID) { + m_frontLeftController->Reset(); + m_rearLeftController->Reset(); + m_frontRightController->Reset(); + m_rearRightController->Reset(); + } +} + +void MecanumControllerCommand::Execute() { + auto curTime = second_t(m_timer.Get()); + auto dt = curTime - m_prevTime; + + auto m_desiredState = m_trajectory.Sample(curTime); + auto m_desiredPose = m_desiredState.pose; + + auto m_poseError = m_desiredPose.RelativeTo(m_pose()); + + auto targetXVel = meters_per_second_t( + m_xController->Calculate((m_pose().Translation().X().to()), + (m_desiredPose.Translation().X().to()))); + auto targetYVel = meters_per_second_t( + m_yController->Calculate((m_pose().Translation().Y().to()), + (m_desiredPose.Translation().Y().to()))); + + // Profiled PID Controller only takes meters as setpoint and measurement + // The robot will go to the desired rotation of the final pose in the + // trajectory, not following the poses at individual states. + auto targetAngularVel = radians_per_second_t(m_thetaController->Calculate( + m_pose().Rotation().Radians(), m_finalPose.Rotation().Radians())); + + auto vRef = m_desiredState.velocity; + + targetXVel += vRef * m_poseError.Rotation().Cos(); + targetYVel += vRef * m_poseError.Rotation().Sin(); + + auto targetChassisSpeeds = + frc::ChassisSpeeds{targetXVel, targetYVel, targetAngularVel}; + + auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(targetChassisSpeeds); + + targetWheelSpeeds.Normalize(m_maxWheelVelocity); + + auto frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeft; + auto rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeft; + auto frontRightSpeedSetpoint = targetWheelSpeeds.frontRight; + auto rearRightSpeedSetpoint = targetWheelSpeeds.rearRight; + + if (m_usePID) { + auto frontLeftFeedforward = m_feedforward.Calculate( + frontLeftSpeedSetpoint, + (frontLeftSpeedSetpoint - m_prevSpeeds.frontLeft) / dt); + + auto rearLeftFeedforward = m_feedforward.Calculate( + rearLeftSpeedSetpoint, + (rearLeftSpeedSetpoint - m_prevSpeeds.rearLeft) / dt); + + auto frontRightFeedforward = m_feedforward.Calculate( + frontRightSpeedSetpoint, + (frontRightSpeedSetpoint - m_prevSpeeds.frontRight) / dt); + + auto rearRightFeedforward = m_feedforward.Calculate( + rearRightSpeedSetpoint, + (rearRightSpeedSetpoint - m_prevSpeeds.rearRight) / dt); + + auto frontLeftOutput = volt_t(m_frontLeftController->Calculate( + m_currentWheelSpeeds().frontLeft.to(), + frontLeftSpeedSetpoint.to())) + + frontLeftFeedforward; + auto rearLeftOutput = volt_t(m_rearLeftController->Calculate( + m_currentWheelSpeeds().rearLeft.to(), + rearLeftSpeedSetpoint.to())) + + rearLeftFeedforward; + auto frontRightOutput = volt_t(m_frontRightController->Calculate( + m_currentWheelSpeeds().frontRight.to(), + frontRightSpeedSetpoint.to())) + + frontRightFeedforward; + auto rearRightOutput = volt_t(m_rearRightController->Calculate( + m_currentWheelSpeeds().rearRight.to(), + rearRightSpeedSetpoint.to())) + + rearRightFeedforward; + + m_outputVolts(frontLeftOutput, rearLeftOutput, frontRightOutput, + rearRightOutput); + } else { + m_outputVel(frontLeftSpeedSetpoint, rearLeftSpeedSetpoint, + frontRightSpeedSetpoint, rearRightSpeedSetpoint); + + m_prevTime = curTime; + m_prevSpeeds = targetWheelSpeeds; + } +} + +void MecanumControllerCommand::End(bool interrupted) { m_timer.Stop(); } + +bool MecanumControllerCommand::IsFinished() { + return m_timer.HasPeriodPassed(m_trajectory.TotalTime()); +} diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h new file mode 100644 index 0000000000..e5a22a5c3f --- /dev/null +++ b/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h @@ -0,0 +1,175 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "CommandBase.h" +#include "CommandHelper.h" +#include "frc2/Timer.h" + +#pragma once + +namespace frc2 { +/** + * A command that uses two PID controllers ({@link PIDController}) and a + * ProfiledPIDController ({@link ProfiledPIDController}) to follow a trajectory + * {@link Trajectory} with a mecanum drive. + * + *

The command handles trajectory-following, + * Velocity PID calculations, and feedforwards internally. This + * is intended to be a more-or-less "complete solution" that can be used by + * teams without a great deal of controls expertise. + * + *

Advanced teams seeking more flexibility (for example, those who wish to + * use the onboard PID functionality of a "smart" motor controller) may use the + * secondary constructor that omits the PID and feedforward functionality, + * returning only the raw wheel speeds from the PID controllers. + * + *

The robot angle controller does not follow the angle given by + * the trajectory but rather goes to the angle given in the final state of the + * trajectory. + */ +class MecanumControllerCommand + : public CommandHelper { + public: + /** + * Constructs a new MecanumControllerCommand that when executed will follow + * the provided trajectory. PID control and feedforward are handled + * internally. Outputs are scaled from -12 to 12 as a voltage output to the + * motor. + * + *

Note: The controllers will *not* set the outputVolts to zero upon + * completion of the path this is left to the user, since it is not + * appropriate for paths with nonstationary endstates. + * + *

Note 2: The rotation controller will calculate the rotation based on the + * final pose in the trajectory, not the poses at each time step. + * + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose, + * provided by the odometry class. + * @param feedforward The feedforward to use for the drivetrain. + * @param kinematics The kinematics for the robot drivetrain. + * @param xController The Trajectory Tracker PID controller + * for the robot's x position. + * @param yController The Trajectory Tracker PID controller + * for the robot's y position. + * @param thetaController The Trajectory Tracker PID controller + * for angle for the robot. + * @param maxWheelVelocity The maximum velocity of a drivetrain wheel. + * @param frontLeftController The front left wheel velocity PID. + * @param rearLeftController The rear left wheel velocity PID. + * @param frontRightController The front right wheel velocity PID. + * @param rearRightController The rear right wheel velocity PID. + * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing + * the current wheel speeds. + * @param output The output of the velocity PIDs. + * @param requirements The subsystems to require. + */ + MecanumControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::SimpleMotorFeedforward feedforward, + frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, + frc2::PIDController yController, + frc::ProfiledPIDController thetaController, + units::meters_per_second_t maxWheelVelocity, + std::function currentWheelSpeeds, + frc2::PIDController frontLeftController, + frc2::PIDController rearLeftController, + frc2::PIDController frontRightController, + frc2::PIDController rearRightController, + std::function + output, + std::initializer_list requirements); + + /** + * Constructs a new MecanumControllerCommand that when executed will follow + * the provided trajectory. The user should implement a velocity PID on the + * desired output wheel velocities. + * + *

Note: The controllers will *not* set the outputVolts to zero upon + * completion of the path - this is left to the user, since it is not + * appropriate for paths with non-stationary end-states. + * + *

Note2: The rotation controller will calculate the rotation based on the + * final pose in the trajectory, not the poses at each time step. + * + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose - use one + * of the odometry classes to provide this. + * @param kinematics The kinematics for the robot drivetrain. + * @param xController The Trajectory Tracker PID controller + * for the robot's x position. + * @param yController The Trajectory Tracker PID controller + * for the robot's y position. + * @param thetaController The Trajectory Tracker PID controller + * for angle for the robot. + * @param maxWheelVelocity The maximum velocity of a drivetrain wheel. + * @param output The output of the position PIDs. + * @param requirements The subsystems to require. + */ + MecanumControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, + frc2::PIDController yController, + frc::ProfiledPIDController thetaController, + units::meters_per_second_t maxWheelVelocity, + std::function + output, + std::initializer_list requirements); + + void Initialize() override; + + void Execute() override; + + void End(bool interrupted) override; + + bool IsFinished() override; + + private: + frc::Trajectory m_trajectory; + std::function m_pose; + frc::SimpleMotorFeedforward m_feedforward; + frc::MecanumDriveKinematics m_kinematics; + std::unique_ptr m_xController; + std::unique_ptr m_yController; + std::unique_ptr> m_thetaController; + const units::meters_per_second_t m_maxWheelVelocity; + std::unique_ptr m_frontLeftController; + std::unique_ptr m_rearLeftController; + std::unique_ptr m_frontRightController; + std::unique_ptr m_rearRightController; + std::function m_currentWheelSpeeds; + std::function + m_outputVel; + std::function + m_outputVolts; + + bool m_usePID; + frc2::Timer m_timer; + frc::MecanumDriveWheelSpeeds m_prevSpeeds; + units::second_t m_prevTime; + frc::Pose2d m_finalPose; +}; +} // namespace frc2 diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h new file mode 100644 index 0000000000..ab3fe10ae9 --- /dev/null +++ b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h @@ -0,0 +1,119 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "CommandBase.h" +#include "CommandHelper.h" +#include "frc2/Timer.h" + +#pragma once + +namespace frc2 { + +/** + * A command that uses two PID controllers ({@link PIDController}) and a + * ProfiledPIDController ({@link ProfiledPIDController}) to follow a trajectory + * {@link Trajectory} with a swerve drive. + * + *

The command handles trajectory-following, Velocity PID calculations, and + * feedforwards internally. This is intended to be a more-or-less "complete + * solution" that can be used by teams without a great deal of controls + * expertise. + * + *

Advanced teams seeking more flexibility (for example, those who wish to + * use the onboard PID functionality of a "smart" motor controller) may use the + * secondary constructor that omits the PID and feedforward functionality, + * returning only the raw module states from the position PID controllers. + * + *

The robot angle controller does not follow the angle given by + * the trajectory but rather goes to the angle given in the final state of the + * trajectory. + */ +template +class SwerveControllerCommand + : public CommandHelper> { + using voltsecondspermeter = + units::compound_unit>; + using voltsecondssquaredpermeter = + units::compound_unit, + units::inverse>; + + public: + /** + * Constructs a new SwerveControllerCommand that when executed will follow the + * provided trajectory. This command will not return output voltages but + * rather raw module states from the position controllers which need to be put + * into a velocity PID. + * + *

Note: The controllers will *not* set the outputVolts to zero upon + * completion of the path- this is left to the user, since it is not + * appropriate for paths with nonstationary endstates. + * + *

Note 2: The rotation controller will calculate the rotation based on the + * final pose in the trajectory, not the poses at each time step. + * + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose, + * provided by the odometry class. + * @param kinematics The kinematics for the robot drivetrain. + * @param xController The Trajectory Tracker PID controller + * for the robot's x position. + * @param yController The Trajectory Tracker PID controller + * for the robot's y position. + * @param thetaController The Trajectory Tracker PID controller + * for angle for the robot. + * @param output The raw output module states from the + * position controllers. + * @param requirements The subsystems to require. + */ + SwerveControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::SwerveDriveKinematics kinematics, + frc2::PIDController xController, frc2::PIDController yController, + frc::ProfiledPIDController thetaController, + std::function)> + output, + std::initializer_list requirements); + + void Initialize() override; + + void Execute() override; + + void End(bool interrupted) override; + + bool IsFinished() override; + + private: + frc::Trajectory m_trajectory; + std::function m_pose; + frc::SwerveDriveKinematics m_kinematics; + std::unique_ptr m_xController; + std::unique_ptr m_yController; + std::unique_ptr> m_thetaController; + std::function)> + m_outputStates; + + frc2::Timer m_timer; + units::second_t m_prevTime; + frc::Pose2d m_finalPose; +}; +} // namespace frc2 + +#include "SwerveControllerCommand.inc" diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc new file mode 100644 index 0000000000..7beafefdfc --- /dev/null +++ b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc @@ -0,0 +1,89 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +namespace frc2 { + +template +SwerveControllerCommand::SwerveControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::SwerveDriveKinematics kinematics, + frc2::PIDController xController, frc2::PIDController yController, + frc::ProfiledPIDController thetaController, + std::function)> output, + std::initializer_list requirements) + : m_trajectory(trajectory), + m_pose(pose), + m_kinematics(kinematics), + m_xController(std::make_unique(xController)), + m_yController(std::make_unique(yController)), + m_thetaController( + std::make_unique>( + thetaController)), + m_outputStates(output) { + this->AddRequirements(requirements); +} + +template +void SwerveControllerCommand::Initialize() { + m_finalPose = m_trajectory.Sample(m_trajectory.TotalTime()).pose; + + m_timer.Reset(); + m_timer.Start(); +} + +template +void SwerveControllerCommand::Execute() { + auto curTime = units::second_t(m_timer.Get()); + + auto m_desiredState = m_trajectory.Sample(curTime); + auto m_desiredPose = m_desiredState.pose; + + auto m_poseError = m_desiredPose.RelativeTo(m_pose()); + + auto targetXVel = units::meters_per_second_t(m_xController->Calculate( + (m_pose().Translation().X().template to()), + (m_desiredPose.Translation().X().template to()))); + auto targetYVel = units::meters_per_second_t(m_yController->Calculate( + (m_pose().Translation().Y().template to()), + (m_desiredPose.Translation().Y().template to()))); + + // Profiled PID Controller only takes meters as setpoint and measurement + // The robot will go to the desired rotation of the final pose in the + // trajectory, not following the poses at individual states. + auto targetAngularVel = + units::radians_per_second_t(m_thetaController->Calculate( + m_pose().Rotation().Radians(), m_finalPose.Rotation().Radians())); + + auto vRef = m_desiredState.velocity; + + targetXVel += vRef * m_poseError.Rotation().Cos(); + targetYVel += vRef * m_poseError.Rotation().Sin(); + + auto targetChassisSpeeds = + frc::ChassisSpeeds{targetXVel, targetYVel, targetAngularVel}; + + auto targetModuleStates = + m_kinematics.ToSwerveModuleStates(targetChassisSpeeds); + + m_outputStates(targetModuleStates); +} + +template +void SwerveControllerCommand::End(bool interrupted) { + m_timer.Stop(); +} + +template +bool SwerveControllerCommand::IsFinished() { + return m_timer.HasPeriodPassed(m_trajectory.TotalTime()); +} + +} // namespace frc2 diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java new file mode 100644 index 0000000000..2ac2129b68 --- /dev/null +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java @@ -0,0 +1,118 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import java.util.ArrayList; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.controller.ProfiledPIDController; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics; +import edu.wpi.first.wpilibj.kinematics.MecanumDriveOdometry; +import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds; +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; +import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; + +class MecanumControllerCommandTest { + private final Timer m_timer = new Timer(); + private Rotation2d m_angle = new Rotation2d(0); + + private double m_frontLeftSpeed; + private double m_rearLeftSpeed; + private double m_frontRightSpeed; + private double m_rearRightSpeed; + + private final ProfiledPIDController m_rotController = new ProfiledPIDController(1, 0, 0, + new TrapezoidProfile.Constraints(3 * Math.PI, Math.PI)); + + private static final double kxTolerance = 1 / 12.0; + private static final double kyTolerance = 1 / 12.0; + private static final double kAngularTolerance = 1 / 12.0; + + private static final double kWheelBase = 0.5; + private static final double kTrackWidth = 0.5; + + private final MecanumDriveKinematics m_kinematics = new MecanumDriveKinematics( + new Translation2d(kWheelBase / 2, kTrackWidth / 2), + new Translation2d(kWheelBase / 2, -kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); + + private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics, + new Rotation2d(0), new Pose2d(0, 0, new Rotation2d(0))); + + public void setWheelSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) { + this.m_frontLeftSpeed = wheelSpeeds.frontLeftMetersPerSecond; + this.m_rearLeftSpeed = wheelSpeeds.rearLeftMetersPerSecond; + this.m_frontRightSpeed = wheelSpeeds.frontRightMetersPerSecond; + this.m_rearRightSpeed = wheelSpeeds.rearRightMetersPerSecond; + } + + public MecanumDriveWheelSpeeds getCurrentWheelSpeeds() { + return new MecanumDriveWheelSpeeds(m_frontLeftSpeed, + m_frontRightSpeed, m_rearLeftSpeed, m_rearRightSpeed); + } + + public Pose2d getRobotPose() { + m_odometry.updateWithTime(m_timer.get(), m_angle, getCurrentWheelSpeeds()); + return m_odometry.getPoseMeters(); + } + + @Test + @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops") + void testReachesReference() { + final var subsystem = new Subsystem() {}; + + final var waypoints = new ArrayList(); + waypoints.add(new Pose2d(0, 0, new Rotation2d(0))); + waypoints.add(new Pose2d(1, 5, new Rotation2d(3))); + var config = new TrajectoryConfig(8.8, 0.1); + final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config); + + final var endState = trajectory.sample(trajectory.getTotalTimeSeconds()); + + final var command = new MecanumControllerCommand(trajectory, + this::getRobotPose, + m_kinematics, + new PIDController(0.6, 0, 0), + new PIDController(0.6, 0, 0), + m_rotController, + 8.8, + this::setWheelSpeeds, + subsystem); + + m_timer.reset(); + m_timer.start(); + + command.initialize(); + while (!command.isFinished()) { + command.execute(); + m_angle = trajectory.sample(m_timer.get()).poseMeters.getRotation(); + } + m_timer.stop(); + command.end(true); + + assertAll( + () -> assertEquals(endState.poseMeters.getTranslation().getX(), + getRobotPose().getTranslation().getX(), kxTolerance), + () -> assertEquals(endState.poseMeters.getTranslation().getY(), + getRobotPose().getTranslation().getY(), kyTolerance), + () -> assertEquals(endState.poseMeters.getRotation().getRadians(), + getRobotPose().getRotation().getRadians(), kAngularTolerance) + ); + } +} diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java new file mode 100644 index 0000000000..5209199cf9 --- /dev/null +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java @@ -0,0 +1,111 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import java.util.ArrayList; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.controller.ProfiledPIDController; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics; +import edu.wpi.first.wpilibj.kinematics.SwerveDriveOdometry; +import edu.wpi.first.wpilibj.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; +import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; + +class SwerveControllerCommandTest { + private final Timer m_timer = new Timer(); + private Rotation2d m_angle = new Rotation2d(0); + + private SwerveModuleState[] m_moduleStates = new SwerveModuleState[]{ + new SwerveModuleState(0, new Rotation2d(0)), + new SwerveModuleState(0, new Rotation2d(0)), + new SwerveModuleState(0, new Rotation2d(0)), + new SwerveModuleState(0, new Rotation2d(0))}; + + private final ProfiledPIDController m_rotController = new ProfiledPIDController(1, 0, 0, + new TrapezoidProfile.Constraints(3 * Math.PI, Math.PI)); + + private static final double kxTolerance = 1 / 12.0; + private static final double kyTolerance = 1 / 12.0; + private static final double kAngularTolerance = 1 / 12.0; + + private static final double kWheelBase = 0.5; + private static final double kTrackWidth = 0.5; + + private final SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics( + new Translation2d(kWheelBase / 2, kTrackWidth / 2), + new Translation2d(kWheelBase / 2, -kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); + + private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics, + new Rotation2d(0), new Pose2d(0, 0, new Rotation2d(0))); + + @SuppressWarnings("PMD.ArrayIsStoredDirectly") + public void setModuleStates(SwerveModuleState[] moduleStates) { + this.m_moduleStates = moduleStates; + } + + public Pose2d getRobotPose() { + m_odometry.updateWithTime(m_timer.get(), m_angle, m_moduleStates); + return m_odometry.getPoseMeters(); + } + + @Test + @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops") + void testReachesReference() { + final var subsystem = new Subsystem() {}; + + final var waypoints = new ArrayList(); + waypoints.add(new Pose2d(0, 0, new Rotation2d(0))); + waypoints.add(new Pose2d(1, 5, new Rotation2d(3))); + var config = new TrajectoryConfig(8.8, 0.1); + final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config); + + final var endState = trajectory.sample(trajectory.getTotalTimeSeconds()); + + final var command = new SwerveControllerCommand(trajectory, + this::getRobotPose, + m_kinematics, + new PIDController(0.6, 0, 0), + new PIDController(0.6, 0, 0), + m_rotController, + this::setModuleStates, + subsystem); + + m_timer.reset(); + m_timer.start(); + + command.initialize(); + while (!command.isFinished()) { + command.execute(); + m_angle = trajectory.sample(m_timer.get()).poseMeters.getRotation(); + } + m_timer.stop(); + command.end(true); + + assertAll( + () -> assertEquals(endState.poseMeters.getTranslation().getX(), + getRobotPose().getTranslation().getX(), kxTolerance), + () -> assertEquals(endState.poseMeters.getTranslation().getY(), + getRobotPose().getTranslation().getY(), kyTolerance), + () -> assertEquals(endState.poseMeters.getRotation().getRadians(), + getRobotPose().getRotation().getRadians(), kAngularTolerance) + ); + } +} diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp new file mode 100644 index 0000000000..4eae88017b --- /dev/null +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp @@ -0,0 +1,116 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" + +#define EXPECT_NEAR_UNITS(val1, val2, eps) \ + EXPECT_LE(units::math::abs(val1 - val2), eps) + +class MecanumControllerCommandTest : public ::testing::Test { + using radians_per_second_squared_t = + units::compound_unit>>; + + protected: + frc2::Timer m_timer; + frc::Rotation2d m_angle{0_rad}; + + units::meters_per_second_t m_frontLeftSpeed = 0.0_mps; + units::meters_per_second_t m_rearLeftSpeed = 0.0_mps; + units::meters_per_second_t m_frontRightSpeed = 0.0_mps; + units::meters_per_second_t m_rearRightSpeed = 0.0_mps; + + frc::ProfiledPIDController m_rotController{ + 1, 0, 0, + frc::TrapezoidProfile::Constraints{ + 9_rad_per_s, units::unit_t(3)}}; + + static constexpr units::meter_t kxTolerance{1 / 12.0}; + static constexpr units::meter_t kyTolerance{1 / 12.0}; + static constexpr units::radian_t kAngularTolerance{1 / 12.0}; + + static constexpr units::meter_t kWheelBase{0.5}; + static constexpr units::meter_t kTrackWidth{0.5}; + + frc::MecanumDriveKinematics m_kinematics{ + frc::Translation2d{kWheelBase / 2, kTrackWidth / 2}, + frc::Translation2d{kWheelBase / 2, -kTrackWidth / 2}, + frc::Translation2d{-kWheelBase / 2, kTrackWidth / 2}, + frc::Translation2d{-kWheelBase / 2, -kTrackWidth / 2}}; + + frc::MecanumDriveOdometry m_odometry{m_kinematics, 0_rad, + frc::Pose2d{0_m, 0_m, 0_rad}}; + + frc::MecanumDriveWheelSpeeds getCurrentWheelSpeeds() { + return frc::MecanumDriveWheelSpeeds{m_frontLeftSpeed, m_frontRightSpeed, + m_rearLeftSpeed, m_rearRightSpeed}; + } + + frc::Pose2d getRobotPose() { + m_odometry.UpdateWithTime(m_timer.Get(), m_angle, getCurrentWheelSpeeds()); + return m_odometry.GetPose(); + } +}; + +TEST_F(MecanumControllerCommandTest, ReachesReference) { + frc2::Subsystem subsystem{}; + + auto waypoints = + std::vector{frc::Pose2d{0_m, 0_m, 0_rad}, frc::Pose2d{1_m, 5_m, 3_rad}}; + auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory( + waypoints, {8.8_mps, 0.1_mps_sq}); + + auto endState = trajectory.Sample(trajectory.TotalTime()); + + auto command = frc2::MecanumControllerCommand( + trajectory, [&]() { return getRobotPose(); }, m_kinematics, + + frc2::PIDController(0.6, 0, 0), frc2::PIDController(0.6, 0, 0), + m_rotController, units::meters_per_second_t(8.8), + [&](units::meters_per_second_t frontLeft, + units::meters_per_second_t rearLeft, + units::meters_per_second_t frontRight, + units::meters_per_second_t rearRight) { + m_frontLeftSpeed = frontLeft; + m_rearLeftSpeed = rearLeft; + m_frontRightSpeed = frontRight; + m_rearRightSpeed = rearRight; + }, + {&subsystem}); + + m_timer.Reset(); + m_timer.Start(); + command.Initialize(); + while (!command.IsFinished()) { + command.Execute(); + m_angle = trajectory.Sample(m_timer.Get()).pose.Rotation(); + } + m_timer.Stop(); + command.End(false); + + EXPECT_NEAR_UNITS(endState.pose.Translation().X(), + getRobotPose().Translation().X(), kxTolerance); + EXPECT_NEAR_UNITS(endState.pose.Translation().Y(), + getRobotPose().Translation().Y(), kyTolerance); + EXPECT_NEAR_UNITS(endState.pose.Rotation().Radians(), + getRobotPose().Rotation().Radians(), kAngularTolerance); +} diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp new file mode 100644 index 0000000000..fa0838d99e --- /dev/null +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp @@ -0,0 +1,106 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" + +#define EXPECT_NEAR_UNITS(val1, val2, eps) \ + EXPECT_LE(units::math::abs(val1 - val2), eps) + +class SwerveControllerCommandTest : public ::testing::Test { + using radians_per_second_squared_t = + units::compound_unit>>; + + protected: + frc2::Timer m_timer; + frc::Rotation2d m_angle{0_rad}; + + std::array m_moduleStates{ + frc::SwerveModuleState{}, frc::SwerveModuleState{}, + frc::SwerveModuleState{}, frc::SwerveModuleState{}}; + + frc::ProfiledPIDController m_rotController{ + 1, 0, 0, + frc::TrapezoidProfile::Constraints{ + 9_rad_per_s, units::unit_t(3)}}; + + static constexpr units::meter_t kxTolerance{1 / 12.0}; + static constexpr units::meter_t kyTolerance{1 / 12.0}; + static constexpr units::radian_t kAngularTolerance{1 / 12.0}; + + static constexpr units::meter_t kWheelBase{0.5}; + static constexpr units::meter_t kTrackWidth{0.5}; + + frc::SwerveDriveKinematics<4> m_kinematics{ + frc::Translation2d{kWheelBase / 2, kTrackWidth / 2}, + frc::Translation2d{kWheelBase / 2, -kTrackWidth / 2}, + frc::Translation2d{-kWheelBase / 2, kTrackWidth / 2}, + frc::Translation2d{-kWheelBase / 2, -kTrackWidth / 2}}; + + frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad, + frc::Pose2d{0_m, 0_m, 0_rad}}; + + std::array getCurrentWheelSpeeds() { + return m_moduleStates; + } + + frc::Pose2d getRobotPose() { + m_odometry.UpdateWithTime(m_timer.Get(), m_angle, getCurrentWheelSpeeds()); + return m_odometry.GetPose(); + } +}; + +TEST_F(SwerveControllerCommandTest, ReachesReference) { + frc2::Subsystem subsystem{}; + + auto waypoints = + std::vector{frc::Pose2d{0_m, 0_m, 0_rad}, frc::Pose2d{1_m, 5_m, 3_rad}}; + auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory( + waypoints, {8.8_mps, 0.1_mps_sq}); + + auto endState = trajectory.Sample(trajectory.TotalTime()); + + auto command = frc2::SwerveControllerCommand<4>( + trajectory, [&]() { return getRobotPose(); }, m_kinematics, + + frc2::PIDController(0.6, 0, 0), frc2::PIDController(0.6, 0, 0), + m_rotController, + [&](auto moduleStates) { m_moduleStates = moduleStates; }, {&subsystem}); + + m_timer.Reset(); + m_timer.Start(); + command.Initialize(); + while (!command.IsFinished()) { + command.Execute(); + m_angle = trajectory.Sample(m_timer.Get()).pose.Rotation(); + } + m_timer.Stop(); + command.End(false); + + EXPECT_NEAR_UNITS(endState.pose.Translation().X(), + getRobotPose().Translation().X(), kxTolerance); + EXPECT_NEAR_UNITS(endState.pose.Translation().Y(), + getRobotPose().Translation().Y(), kyTolerance); + EXPECT_NEAR_UNITS(endState.pose.Rotation().Radians(), + getRobotPose().Rotation().Radians(), kAngularTolerance); +} diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp new file mode 100644 index 0000000000..2fd8151a84 --- /dev/null +++ b/wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h" + +using namespace frc; + +MecanumDriveKinematicsConstraint::MecanumDriveKinematicsConstraint( + MecanumDriveKinematics kinematics, units::meters_per_second_t maxSpeed) + : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {} + +units::meters_per_second_t MecanumDriveKinematicsConstraint::MaxVelocity( + const Pose2d& pose, curvature_t curvature, + units::meters_per_second_t velocity) { + auto xVelocity = velocity * pose.Rotation().Cos(); + auto yVelocity = velocity * pose.Rotation().Sin(); + auto wheelSpeeds = + m_kinematics.ToWheelSpeeds({xVelocity, yVelocity, velocity * curvature}); + wheelSpeeds.Normalize(m_maxSpeed); + + auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds); + + return units::math::hypot(normSpeeds.vx, normSpeeds.vy); +} + +TrajectoryConstraint::MinMax +MecanumDriveKinematicsConstraint::MinMaxAcceleration( + const Pose2d& pose, curvature_t curvature, + units::meters_per_second_t speed) { + return {}; +} diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h index fcd8087b65..1bdbcea5ec 100644 --- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h +++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h @@ -121,6 +121,22 @@ class SwerveDriveKinematics { template ChassisSpeeds ToChassisSpeeds(ModuleStates&&... wheelStates); + /** + * Performs forward kinematics to return the resulting chassis state from the + * given module states. This method is often used for odometry -- determining + * the robot's position on the field using data from the real-world speed and + * angle of each module on the robot. + * + * @param moduleStates The state of the modules as an std::array of type + * SwerveModuleState, NumModules long as measured from respective encoders + * and gyros. The order of the swerve module states should be same as passed + * into the constructor of this class. + * + * @return The resulting chassis speed. + */ + ChassisSpeeds ToChassisSpeeds( + std::array moduleStates); + /** * Normalizes the wheel speeds using some max attainable speed. Sometimes, * after inverse kinematics, the requested speed from a/several modules may be diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc index 138954d2ac..b362aa0916 100644 --- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc +++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc @@ -63,6 +63,13 @@ ChassisSpeeds SwerveDriveKinematics::ToChassisSpeeds( "locations provided in constructor."); std::array moduleStates{wheelStates...}; + + return this->ToChassisSpeeds(moduleStates); +} + +template +ChassisSpeeds SwerveDriveKinematics::ToChassisSpeeds( + std::array moduleStates) { Eigen::Matrix moduleStatesMatrix; for (size_t i = 0; i < NumModules; i++) { diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h index a739070aca..36118a0dbb 100644 --- a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h +++ b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h @@ -14,7 +14,11 @@ #include #include "frc/kinematics/DifferentialDriveKinematics.h" +#include "frc/kinematics/MecanumDriveKinematics.h" +#include "frc/kinematics/SwerveDriveKinematics.h" #include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h" +#include "frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h" +#include "frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h" #include "frc/trajectory/constraint/TrajectoryConstraint.h" namespace frc { @@ -88,6 +92,27 @@ class TrajectoryConfig { DifferentialDriveKinematicsConstraint(kinematics, m_maxVelocity)); } + /** + * Adds a mecanum drive kinematics constraint to ensure that + * no wheel velocity of a mecanum drive goes above the max velocity. + * + * @param kinematics The mecanum drive kinematics. + */ + void SetKinematics(MecanumDriveKinematics kinematics) { + AddConstraint(MecanumDriveKinematicsConstraint(kinematics, m_maxVelocity)); + } + + /** + * Adds a swerve drive kinematics constraint to ensure that + * no wheel velocity of a swerve drive goes above the max velocity. + * + * @param kinematics The swerve drive kinematics. + */ + template + void SetKinematics(SwerveDriveKinematics& kinematics) { + AddConstraint(SwerveDriveKinematicsConstraint(kinematics, m_maxVelocity)); + } + /** * Returns the starting velocity of the trajectory. * @return The starting velocity of the trajectory. diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h new file mode 100644 index 0000000000..53e3953d15 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h @@ -0,0 +1,40 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include + +#include "frc/kinematics/MecanumDriveKinematics.h" +#include "frc/trajectory/constraint/TrajectoryConstraint.h" + +/** + * A class that enforces constraints on the differential drive kinematics. + * This can be used to ensure that the trajectory is constructed so that the + * commanded velocities for both sides of the drivetrain stay below a certain + * limit. + */ +namespace frc { +class MecanumDriveKinematicsConstraint : public TrajectoryConstraint { + public: + MecanumDriveKinematicsConstraint(MecanumDriveKinematics kinematics, + units::meters_per_second_t maxSpeed); + + units::meters_per_second_t MaxVelocity( + const Pose2d& pose, curvature_t curvature, + units::meters_per_second_t velocity) override; + + MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature, + units::meters_per_second_t speed) override; + + private: + MecanumDriveKinematics m_kinematics; + units::meters_per_second_t m_maxSpeed; +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h new file mode 100644 index 0000000000..a8ad870e95 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h @@ -0,0 +1,45 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include + +#include "frc/kinematics/SwerveDriveKinematics.h" +#include "frc/trajectory/constraint/TrajectoryConstraint.h" + +/** + * A class that enforces constraints on the differential drive kinematics. + * This can be used to ensure that the trajectory is constructed so that the + * commanded velocities for both sides of the drivetrain stay below a certain + * limit. + */ +namespace frc { + +template +class SwerveDriveKinematicsConstraint : public TrajectoryConstraint { + public: + SwerveDriveKinematicsConstraint( + frc::SwerveDriveKinematics kinematics, + units::meters_per_second_t maxSpeed); + + units::meters_per_second_t MaxVelocity( + const Pose2d& pose, curvature_t curvature, + units::meters_per_second_t velocity) override; + + MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature, + units::meters_per_second_t speed) override; + + private: + frc::SwerveDriveKinematics m_kinematics; + units::meters_per_second_t m_maxSpeed; +}; +} // namespace frc + +#include "SwerveDriveKinematicsConstraint.inc" diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc b/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc new file mode 100644 index 0000000000..c3437d5e36 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc @@ -0,0 +1,49 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +/** + * A class that enforces constraints on the differential drive kinematics. + * This can be used to ensure that the trajectory is constructed so that the + * commanded velocities for both sides of the drivetrain stay below a certain + * limit. + */ + +namespace frc { + +template +SwerveDriveKinematicsConstraint::SwerveDriveKinematicsConstraint( + frc::SwerveDriveKinematics kinematics, + units::meters_per_second_t maxSpeed) + : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {} + +template +units::meters_per_second_t +SwerveDriveKinematicsConstraint::MaxVelocity( + const Pose2d& pose, curvature_t curvature, + units::meters_per_second_t velocity) { + auto xVelocity = velocity * pose.Rotation().Cos(); + auto yVelocity = velocity * pose.Rotation().Sin(); + auto wheelSpeeds = m_kinematics.ToSwerveModuleStates( + {xVelocity, yVelocity, velocity * curvature}); + m_kinematics.NormalizeWheelSpeeds(&wheelSpeeds, m_maxSpeed); + + auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds); + + return units::math::hypot(normSpeeds.vx, normSpeeds.vy); +} + +template +TrajectoryConstraint::MinMax +SwerveDriveKinematicsConstraint::MinMaxAcceleration( + const Pose2d& pose, curvature_t curvature, + units::meters_per_second_t speed) { + return {}; +} + +} // namespace frc diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp new file mode 100644 index 0000000000..cd19aeb7d8 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp @@ -0,0 +1,71 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "Robot.h" + +#include +#include + +void Robot::RobotInit() {} + +/** + * This function is called every robot packet, no matter the mode. Use + * this for items like diagnostics that you want to run during disabled, + * autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ +void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); } + +/** + * This function is called once each time the robot enters Disabled mode. You + * can use it to reset any subsystem information you want to clear when the + * robot is disabled. + */ +void Robot::DisabledInit() {} + +void Robot::DisabledPeriodic() {} + +/** + * This autonomous runs the autonomous command selected by your {@link + * RobotContainer} class. + */ +void Robot::AutonomousInit() { + m_autonomousCommand = m_container.GetAutonomousCommand(); + + if (m_autonomousCommand != nullptr) { + m_autonomousCommand->Schedule(); + } +} + +void Robot::AutonomousPeriodic() {} + +void Robot::TeleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (m_autonomousCommand != nullptr) { + m_autonomousCommand->Cancel(); + m_autonomousCommand = nullptr; + } +} + +/** + * This function is called periodically during operator control. + */ +void Robot::TeleopPeriodic() {} + +/** + * This function is called periodically during test mode. + */ +void Robot::TestPeriodic() {} + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp new file mode 100644 index 0000000000..c7379cbacd --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp @@ -0,0 +1,116 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "RobotContainer.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Constants.h" + +using namespace DriveConstants; + +const frc::MecanumDriveKinematics DriveConstants::kDriveKinematics{ + frc::Translation2d(kWheelBase / 2, kTrackWidth / 2), + frc::Translation2d(kWheelBase / 2, -kTrackWidth / 2), + frc::Translation2d(-kWheelBase / 2, kTrackWidth / 2), + frc::Translation2d(-kWheelBase / 2, -kTrackWidth / 2)}; + +RobotContainer::RobotContainer() { + // Initialize all of your commands and subsystems here + + // Configure the button bindings + ConfigureButtonBindings(); + + // Set up default drive command + m_drive.SetDefaultCommand(frc2::RunCommand( + [this] { + m_drive.Drive(m_driverController.GetY(frc::GenericHID::kLeftHand), + m_driverController.GetX(frc::GenericHID::kRightHand), + m_driverController.GetX(frc::GenericHID::kLeftHand), + false); + }, + {&m_drive})); +} + +void RobotContainer::ConfigureButtonBindings() { + // Configure your button bindings here + + // While holding the shoulder button, drive at half speed + frc2::JoystickButton(&m_driverController, 6) + .WhenPressed(&m_driveHalfSpeed) + .WhenReleased(&m_driveFullSpeed); +} + +frc2::Command* RobotContainer::GetAutonomousCommand() { + // Set up config for trajectory + frc::TrajectoryConfig config(AutoConstants::kMaxSpeed, + AutoConstants::kMaxAcceleration); + // Add kinematics to ensure max speed is actually obeyed + config.SetKinematics(DriveConstants::kDriveKinematics); + + // An example trajectory to follow. All units in meters. + auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory( + // Start at the origin facing the +X direction + frc::Pose2d(0_m, 0_m, frc::Rotation2d(0_deg)), + // Pass through these two interior waypoints, making an 's' curve path + {frc::Translation2d(1_m, 1_m), frc::Translation2d(2_m, -1_m)}, + // End 3 meters straight ahead of where we started, facing forward + frc::Pose2d(3_m, 0_m, frc::Rotation2d(0_deg)), + // Pass the config + config); + + frc2::MecanumControllerCommand mecanumControllerCommand( + exampleTrajectory, [this]() { return m_drive.GetPose(); }, + + frc::SimpleMotorFeedforward(ks, kv, ka), + DriveConstants::kDriveKinematics, + + frc2::PIDController(AutoConstants::kPXController, 0, 0), + frc2::PIDController(AutoConstants::kPYController, 0, 0), + frc::ProfiledPIDController( + AutoConstants::kPThetaController, 0, 0, + AutoConstants::kThetaControllerConstraints), + + AutoConstants::kMaxSpeed, + + [this]() { + return frc::MecanumDriveWheelSpeeds{ + units::meters_per_second_t(m_drive.GetFrontLeftEncoder().GetRate()), + units::meters_per_second_t( + m_drive.GetFrontRightEncoder().GetRate()), + units::meters_per_second_t(m_drive.GetRearLeftEncoder().GetRate()), + units::meters_per_second_t( + m_drive.GetRearRightEncoder().GetRate())}; + }, + + frc2::PIDController(DriveConstants::kPFrontLeftVel, 0, 0), + frc2::PIDController(DriveConstants::kPRearLeftVel, 0, 0), + frc2::PIDController(DriveConstants::kPFrontRightVel, 0, 0), + frc2::PIDController(DriveConstants::kPRearRightVel, 0, 0), + + [this](units::volt_t frontLeft, units::volt_t rearLeft, + units::volt_t frontRight, units::volt_t rearRight) { + m_drive.SetSpeedControllersVolts(frontLeft, rearLeft, frontRight, + rearRight); + }, + + {&m_drive}); + + // no auto + return new frc2::SequentialCommandGroup( + std::move(mecanumControllerCommand), + frc2::InstantCommand([this]() { m_drive.Drive(0, 0, 0, false); }, {})); +} diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp new file mode 100644 index 0000000000..b758b0755e --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp @@ -0,0 +1,121 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "subsystems/DriveSubsystem.h" + +#include +#include + +#include "Constants.h" + +using namespace DriveConstants; + +DriveSubsystem::DriveSubsystem() + : m_frontLeft{kFrontLeftMotorPort}, + m_rearLeft{kRearLeftMotorPort}, + m_frontRight{kFrontRightMotorPort}, + m_rearRight{kRearRightMotorPort}, + + m_frontLeftEncoder{kFrontLeftEncoderPorts[0], kFrontLeftEncoderPorts[1], + kFrontLeftEncoderReversed}, + m_rearLeftEncoder{kRearLeftEncoderPorts[0], kRearLeftEncoderPorts[1], + kRearLeftEncoderReversed}, + m_frontRightEncoder{kFrontRightEncoderPorts[0], + kFrontRightEncoderPorts[1], + kFrontRightEncoderReversed}, + m_rearRightEncoder{kRearRightEncoderPorts[0], kRearRightEncoderPorts[1], + kRearRightEncoderReversed}, + + m_odometry{kDriveKinematics, + frc::Rotation2d(units::degree_t(GetHeading())), + frc::Pose2d()} { + // Set the distance per pulse for the encoders + m_frontLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); + m_rearLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); + m_frontRightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); + m_rearRightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); +} + +void DriveSubsystem::Periodic() { + // Implementation of subsystem periodic method goes here. + m_odometry.Update( + frc::Rotation2d(units::degree_t(GetHeading())), + frc::MecanumDriveWheelSpeeds{ + units::meters_per_second_t(m_frontLeftEncoder.GetRate()), + units::meters_per_second_t(m_rearLeftEncoder.GetRate()), + units::meters_per_second_t(m_frontRightEncoder.GetRate()), + units::meters_per_second_t(m_rearRightEncoder.GetRate())}); +} + +void DriveSubsystem::Drive(double xSpeed, double ySpeed, double rot, + bool feildRelative) { + if (feildRelative) { + m_drive.DriveCartesian(ySpeed, xSpeed, rot, -m_gyro.GetAngle()); + } else { + m_drive.DriveCartesian(ySpeed, xSpeed, rot); + } +} + +void DriveSubsystem::SetSpeedControllersVolts(units::volt_t frontLeftPower, + units::volt_t rearLeftPower, + units::volt_t frontRightPower, + units::volt_t rearRightPower) { + m_frontLeft.SetVoltage(frontLeftPower); + m_rearLeft.SetVoltage(rearLeftPower); + m_frontRight.SetVoltage(frontRightPower); + m_rearRight.SetVoltage(rearRightPower); +} + +void DriveSubsystem::ResetEncoders() { + m_frontLeftEncoder.Reset(); + m_rearLeftEncoder.Reset(); + m_frontRightEncoder.Reset(); + m_rearRightEncoder.Reset(); +} + +frc::Encoder& DriveSubsystem::GetFrontLeftEncoder() { + return m_frontLeftEncoder; +} + +frc::Encoder& DriveSubsystem::GetRearLeftEncoder() { return m_rearLeftEncoder; } + +frc::Encoder& DriveSubsystem::GetFrontRightEncoder() { + return m_frontRightEncoder; +} + +frc::Encoder& DriveSubsystem::GetRearRightEncoder() { + return m_rearRightEncoder; +} + +frc::MecanumDriveWheelSpeeds DriveSubsystem::getCurrentWheelSpeeds() { + return (frc::MecanumDriveWheelSpeeds{ + units::meters_per_second_t(m_frontLeftEncoder.GetRate()), + units::meters_per_second_t(m_rearLeftEncoder.GetRate()), + units::meters_per_second_t(m_frontRightEncoder.GetRate()), + units::meters_per_second_t(m_rearRightEncoder.GetRate())}); +} + +void DriveSubsystem::SetMaxOutput(double maxOutput) { + m_drive.SetMaxOutput(maxOutput); +} + +double DriveSubsystem::GetHeading() { + return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1. : 1.); +} + +void DriveSubsystem::ZeroHeading() { m_gyro.Reset(); } + +double DriveSubsystem::GetTurnRate() { + return m_gyro.GetRate() * (kGyroReversed ? -1. : 1.); +} + +frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); } + +void DriveSubsystem::ResetOdometry(frc::Pose2d pose) { + m_odometry.ResetPosition(pose, + frc::Rotation2d(units::degree_t(GetHeading()))); +} diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h new file mode 100644 index 0000000000..5f5f05f063 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h @@ -0,0 +1,93 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include +#include +#include +#include +#include + +#pragma once + +/** + * The Constants header provides a convenient place for teams to hold robot-wide + * numerical or bool constants. This should not be used for any other purpose. + * + * It is generally a good idea to place constants into subsystem- or + * command-specific namespaces within this header, which can then be used where + * they are needed. + */ + +namespace DriveConstants { +constexpr int kFrontLeftMotorPort = 0; +constexpr int kRearLeftMotorPort = 1; +constexpr int kFrontRightMotorPort = 2; +constexpr int kRearRightMotorPort = 3; + +constexpr int kFrontLeftEncoderPorts[]{0, 1}; +constexpr int kRearLeftEncoderPorts[]{2, 3}; +constexpr int kFrontRightEncoderPorts[]{4, 5}; +constexpr int kRearRightEncoderPorts[]{5, 6}; + +constexpr bool kFrontLeftEncoderReversed = false; +constexpr bool kRearLeftEncoderReversed = true; +constexpr bool kFrontRightEncoderReversed = false; +constexpr bool kRearRightEncoderReversed = true; + +constexpr auto kTrackWidth = + 0.5_m; // Distance between centers of right and left wheels on robot +constexpr auto kWheelBase = + 0.7_m; // Distance between centers of front and back wheels on robot +extern const frc::MecanumDriveKinematics kDriveKinematics; + +constexpr int kEncoderCPR = 1024; +constexpr double kWheelDiameterMeters = .15; +constexpr double kEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + (kWheelDiameterMeters * wpi::math::pi) / static_cast(kEncoderCPR); + +constexpr bool kGyroReversed = false; + +// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! +// These characterization values MUST be determined either experimentally or +// theoretically for *your* robot's drive. The RobotPy Characterization +// Toolsuite provides a convenient tool for obtaining these values for your +// robot. +constexpr auto ks = 1_V; +constexpr auto kv = 0.8 * 1_V * 1_s / 1_m; +constexpr auto ka = 0.15 * 1_V * 1_s * 1_s / 1_m; + +// Example value only - as above, this must be tuned for your drive! +constexpr double kPFrontLeftVel = 0.5; +constexpr double kPRearLeftVel = 0.5; +constexpr double kPFrontRightVel = 0.5; +constexpr double kPRearRightVel = 0.5; +} // namespace DriveConstants + +namespace AutoConstants { +using radians_per_second_squared_t = + units::compound_unit>>; + +constexpr auto kMaxSpeed = units::meters_per_second_t(3); +constexpr auto kMaxAcceleration = units::meters_per_second_squared_t(3); +constexpr auto kMaxAngularSpeed = units::radians_per_second_t(3); +constexpr auto kMaxAngularAcceleration = + units::unit_t(3); + +constexpr double kPXController = 0.5; +constexpr double kPYController = 0.5; +constexpr double kPThetaController = 0.5; + +constexpr frc::TrapezoidProfile::Constraints + kThetaControllerConstraints{kMaxAngularSpeed, kMaxAngularAcceleration}; + +} // namespace AutoConstants + +namespace OIConstants { +constexpr int kDriverControllerPort = 1; +} // namespace OIConstants diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h new file mode 100644 index 0000000000..fa173d39e1 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "RobotContainer.h" + +class Robot : public frc::TimedRobot { + public: + void RobotInit() override; + void RobotPeriodic() override; + void DisabledInit() override; + void DisabledPeriodic() override; + void AutonomousInit() override; + void AutonomousPeriodic() override; + void TeleopInit() override; + void TeleopPeriodic() override; + void TestPeriodic() override; + + private: + // Have it null by default so that if testing teleop it + // doesn't have undefined behavior and potentially crash. + frc2::Command* m_autonomousCommand = nullptr; + + RobotContainer m_container; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h new file mode 100644 index 0000000000..3b4f6cc93f --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h @@ -0,0 +1,54 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Constants.h" +#include "subsystems/DriveSubsystem.h" + +/** + * This class is where the bulk of the robot should be declared. Since + * Command-based is a "declarative" paradigm, very little robot logic should + * actually be handled in the {@link Robot} periodic methods (other than the + * scheduler calls). Instead, the structure of the robot (including subsystems, + * commands, and button mappings) should be declared here. + */ +class RobotContainer { + public: + RobotContainer(); + + frc2::Command* GetAutonomousCommand(); + + private: + // The driver's controller + frc::XboxController m_driverController{OIConstants::kDriverControllerPort}; + + // The robot's subsystems and commands are defined here... + + // The robot's subsystems + DriveSubsystem m_drive; + + frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(.5); }, + {}}; + frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); }, + {}}; + + // The chooser for the autonomous routines + frc::SendableChooser m_chooser; + + void ConfigureButtonBindings(); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h new file mode 100644 index 0000000000..fb3c19dd7d --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h @@ -0,0 +1,166 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Constants.h" + +class DriveSubsystem : public frc2::SubsystemBase { + public: + DriveSubsystem(); + + /** + * Will be called periodically whenever the CommandScheduler runs. + */ + void Periodic() override; + + // Subsystem methods go here. + + /** + * Drives the robot at given x, y and theta speeds. Speeds range from [-1, 1] + * and the linear speeds have no effect on the angular speed. + * + * @param xSpeed Speed of the robot in the x direction + * (forward/backwards). + * @param ySpeed Speed of the robot in the y direction (sideways). + * @param rot Angular rate of the robot. + * @param fieldRelative Whether the provided x and y speeds are relative to + * the field. + */ + void Drive(double xSpeed, double ySpeed, double rot, bool feildRelative); + + /** + * Resets the drive encoders to currently read a position of 0. + */ + void ResetEncoders(); + + /** + * Gets the front left drive encoder. + * + * @return the front left drive encoder + */ + frc::Encoder& GetFrontLeftEncoder(); + + /** + * Gets the rear left drive encoder. + * + * @return the rear left drive encoder + */ + frc::Encoder& GetRearLeftEncoder(); + + /** + * Gets the front right drive encoder. + * + * @return the front right drive encoder + */ + frc::Encoder& GetFrontRightEncoder(); + + /** + * Gets the rear right drive encoder. + * + * @return the rear right drive encoder + */ + frc::Encoder& GetRearRightEncoder(); + + /** + * Gets the wheel speeds. + * + * @return the current wheel speeds. + */ + frc::MecanumDriveWheelSpeeds getCurrentWheelSpeeds(); + + /** + * Sets the drive SpeedControllers to a desired voltage. + */ + void SetSpeedControllersVolts(units::volt_t frontLeftPower, + units::volt_t rearLeftPower, + units::volt_t frontRightPower, + units::volt_t rearRightPower); + + /** + * Sets the max output of the drive. Useful for scaling the drive to drive + * more slowly. + * + * @param maxOutput the maximum output to which the drive will be constrained + */ + void SetMaxOutput(double maxOutput); + + /** + * Returns the heading of the robot. + * + * @return the robot's heading in degrees, from 180 to 180 + */ + double GetHeading(); + + /** + * Zeroes the heading of the robot. + */ + void ZeroHeading(); + + /** + * Returns the turn rate of the robot. + * + * @return The turn rate of the robot, in degrees per second + */ + double GetTurnRate(); + + /** + * Returns the currently-estimated pose of the robot. + * + * @return The pose. + */ + frc::Pose2d GetPose(); + + /** + * Resets the odometry to the specified pose. + * + * @param pose The pose to which to set the odometry. + */ + void ResetOdometry(frc::Pose2d pose); + + private: + // Components (e.g. motor controllers and sensors) should generally be + // declared private and exposed only through public methods. + + // The motor controllers + frc::PWMVictorSPX m_frontLeft; + frc::PWMVictorSPX m_rearLeft; + frc::PWMVictorSPX m_frontRight; + frc::PWMVictorSPX m_rearRight; + + // The robot's drive + frc::MecanumDrive m_drive{m_frontLeft, m_rearLeft, m_frontRight, m_rearRight}; + + // The front-left-side drive encoder + frc::Encoder m_frontLeftEncoder; + + // The rear-left-side drive encoder + frc::Encoder m_rearLeftEncoder; + + // The front-right--side drive encoder + frc::Encoder m_frontRightEncoder; + + // The rear-right-side drive encoder + frc::Encoder m_rearRightEncoder; + + // The gyro sensor + frc::ADXRS450_Gyro m_gyro; + + // Odometry class for tracking robot pose + frc::MecanumDriveOdometry m_odometry; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp new file mode 100644 index 0000000000..cd19aeb7d8 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp @@ -0,0 +1,71 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "Robot.h" + +#include +#include + +void Robot::RobotInit() {} + +/** + * This function is called every robot packet, no matter the mode. Use + * this for items like diagnostics that you want to run during disabled, + * autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ +void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); } + +/** + * This function is called once each time the robot enters Disabled mode. You + * can use it to reset any subsystem information you want to clear when the + * robot is disabled. + */ +void Robot::DisabledInit() {} + +void Robot::DisabledPeriodic() {} + +/** + * This autonomous runs the autonomous command selected by your {@link + * RobotContainer} class. + */ +void Robot::AutonomousInit() { + m_autonomousCommand = m_container.GetAutonomousCommand(); + + if (m_autonomousCommand != nullptr) { + m_autonomousCommand->Schedule(); + } +} + +void Robot::AutonomousPeriodic() {} + +void Robot::TeleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (m_autonomousCommand != nullptr) { + m_autonomousCommand->Cancel(); + m_autonomousCommand = nullptr; + } +} + +/** + * This function is called periodically during operator control. + */ +void Robot::TeleopPeriodic() {} + +/** + * This function is called periodically during test mode. + */ +void Robot::TestPeriodic() {} + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp new file mode 100644 index 0000000000..44efba52b5 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp @@ -0,0 +1,91 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "RobotContainer.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Constants.h" +#include "subsystems/DriveSubsystem.h" + +using namespace DriveConstants; + +RobotContainer::RobotContainer() { + // Initialize all of your commands and subsystems here + + // Configure the button bindings + ConfigureButtonBindings(); + + // Set up default drive command + m_drive.SetDefaultCommand(frc2::RunCommand( + [this] { + m_drive.Drive(units::meters_per_second_t( + m_driverController.GetY(frc::GenericHID::kLeftHand)), + units::meters_per_second_t( + m_driverController.GetY(frc::GenericHID::kRightHand)), + units::radians_per_second_t( + m_driverController.GetX(frc::GenericHID::kLeftHand)), + false); + }, + {&m_drive})); +} + +void RobotContainer::ConfigureButtonBindings() {} + +frc2::Command* RobotContainer::GetAutonomousCommand() { + // Set up config for trajectory + frc::TrajectoryConfig config(AutoConstants::kMaxSpeed, + AutoConstants::kMaxAcceleration); + // Add kinematics to ensure max speed is actually obeyed + config.SetKinematics(m_drive.kDriveKinematics); + + // An example trajectory to follow. All units in meters. + auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory( + // Start at the origin facing the +X direction + frc::Pose2d(0_m, 0_m, frc::Rotation2d(0_deg)), + // Pass through these two interior waypoints, making an 's' curve path + {frc::Translation2d(1_m, 1_m), frc::Translation2d(2_m, -1_m)}, + // End 3 meters straight ahead of where we started, facing forward + frc::Pose2d(3_m, 0_m, frc::Rotation2d(0_deg)), + // Pass the config + config); + + frc2::SwerveControllerCommand<4> swerveControllerCommand( + exampleTrajectory, [this]() { return m_drive.GetPose(); }, + + m_drive.kDriveKinematics, + + frc2::PIDController(AutoConstants::kPXController, 0, 0), + frc2::PIDController(AutoConstants::kPYController, 0, 0), + frc::ProfiledPIDController( + AutoConstants::kPThetaController, 0, 0, + AutoConstants::kThetaControllerConstraints), + + [this](auto moduleStates) { m_drive.SetModuleStates(moduleStates); }, + + {&m_drive}); + + // no auto + return new frc2::SequentialCommandGroup( + std::move(swerveControllerCommand), std::move(swerveControllerCommand), + frc2::InstantCommand( + [this]() { + m_drive.Drive(units::meters_per_second_t(0), + units::meters_per_second_t(0), + units::radians_per_second_t(0), false); + }, + {})); +} diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp new file mode 100644 index 0000000000..f5d05ceb1d --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp @@ -0,0 +1,103 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "subsystems/DriveSubsystem.h" + +#include +#include + +#include "Constants.h" + +using namespace DriveConstants; + +DriveSubsystem::DriveSubsystem() + : m_frontLeft{kFrontLeftDriveMotorPort, + kFrontLeftTurningMotorPort, + kFrontLeftDriveEncoderPorts, + kFrontLeftTurningEncoderPorts, + kFrontLeftDriveEncoderReversed, + kFrontLeftTurningEncoderReversed}, + + m_rearLeft{ + kRearLeftDriveMotorPort, kRearLeftTurningMotorPort, + kRearLeftDriveEncoderPorts, kRearLeftTurningEncoderPorts, + kRearLeftDriveEncoderReversed, kRearLeftTurningEncoderReversed}, + + m_frontRight{ + kFrontRightDriveMotorPort, kFrontRightTurningMotorPort, + kFrontRightDriveEncoderPorts, kFrontRightTurningEncoderPorts, + kFrontRightDriveEncoderReversed, kFrontRightTurningEncoderReversed}, + + m_rearRight{ + kRearRightDriveMotorPort, kRearRightTurningMotorPort, + kRearRightDriveEncoderPorts, kRearRightTurningEncoderPorts, + kRearRightDriveEncoderReversed, kRearRightTurningEncoderReversed}, + + m_odometry{kDriveKinematics, + frc::Rotation2d(units::degree_t(GetHeading())), + frc::Pose2d()} {} + +void DriveSubsystem::Periodic() { + // Implementation of subsystem periodic method goes here. + m_odometry.Update(frc::Rotation2d(units::degree_t(GetHeading())), + m_frontLeft.GetState(), m_rearLeft.GetState(), + m_frontRight.GetState(), m_rearRight.GetState()); +} + +void DriveSubsystem::Drive(units::meters_per_second_t xSpeed, + units::meters_per_second_t ySpeed, + units::radians_per_second_t rot, + bool fieldRelative) { + auto states = kDriveKinematics.ToSwerveModuleStates( + fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds( + xSpeed, ySpeed, rot, + frc::Rotation2d(units::degree_t(GetHeading()))) + : frc::ChassisSpeeds{xSpeed, ySpeed, rot}); + + kDriveKinematics.NormalizeWheelSpeeds(&states, AutoConstants::kMaxSpeed); + + auto [fl, fr, bl, br] = states; + + m_frontLeft.SetDesiredState(fl); + m_frontRight.SetDesiredState(fr); + m_rearLeft.SetDesiredState(bl); + m_rearRight.SetDesiredState(br); +} + +void DriveSubsystem::SetModuleStates( + std::array desiredStates) { + kDriveKinematics.NormalizeWheelSpeeds(&desiredStates, + AutoConstants::kMaxSpeed); + m_frontLeft.SetDesiredState(desiredStates[0]); + m_rearLeft.SetDesiredState(desiredStates[1]); + m_frontRight.SetDesiredState(desiredStates[2]); + m_rearRight.SetDesiredState(desiredStates[3]); +} + +void DriveSubsystem::ResetEncoders() { + m_frontLeft.ResetEncoders(); + m_rearLeft.ResetEncoders(); + m_frontRight.ResetEncoders(); + m_rearRight.ResetEncoders(); +} + +double DriveSubsystem::GetHeading() { + return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1. : 1.); +} + +void DriveSubsystem::ZeroHeading() { m_gyro.Reset(); } + +double DriveSubsystem::GetTurnRate() { + return m_gyro.GetRate() * (kGyroReversed ? -1. : 1.); +} + +frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); } + +void DriveSubsystem::ResetOdometry(frc::Pose2d pose) { + m_odometry.ResetPosition(pose, + frc::Rotation2d(units::degree_t(GetHeading()))); +} diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp new file mode 100644 index 0000000000..22b3f0d27f --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp @@ -0,0 +1,66 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "subsystems/SwerveModule.h" + +#include +#include + +#include "Constants.h" + +SwerveModule::SwerveModule(int driveMotorChannel, int turningMotorChannel, + const int driveEncoderPorts[], + const int turningEncoderPorts[], + bool driveEncoderReversed, + bool turningEncoderReversed) + : m_driveMotor(driveMotorChannel), + m_turningMotor(turningMotorChannel), + m_driveEncoder(driveEncoderPorts[0], driveEncoderPorts[1]), + m_turningEncoder(turningEncoderPorts[0], turningEncoderPorts[1]), + m_reverseDriveEncoder(driveEncoderReversed), + m_reverseTurningEncoder(turningEncoderReversed) { + // Set the distance per pulse for the drive encoder. We can simply use the + // distance traveled for one rotation of the wheel divided by the encoder + // resolution. + m_driveEncoder.SetDistancePerPulse( + ModuleConstants::kDriveEncoderDistancePerPulse); + + // Set the distance (in this case, angle) per pulse for the turning encoder. + // This is the the angle through an entire rotation (2 * wpi::math::pi) + // divided by the encoder resolution. + m_turningEncoder.SetDistancePerPulse( + ModuleConstants::kTurningEncoderDistancePerPulse); + + // Limit the PID Controller's input range between -pi and pi and set the input + // to be continuous. + m_turningPIDController.EnableContinuousInput(units::radian_t(-wpi::math::pi), + units::radian_t(wpi::math::pi)); +} + +frc::SwerveModuleState SwerveModule::GetState() { + return {units::meters_per_second_t{m_driveEncoder.GetRate()}, + frc::Rotation2d(units::radian_t(m_turningEncoder.Get()))}; +} + +void SwerveModule::SetDesiredState(frc::SwerveModuleState& state) { + // Calculate the drive output from the drive PID controller. + const auto driveOutput = m_drivePIDController.Calculate( + m_driveEncoder.GetRate(), state.speed.to()); + + // Calculate the turning motor output from the turning PID controller. + auto turnOutput = m_turningPIDController.Calculate( + units::radian_t(m_turningEncoder.Get()), state.angle.Radians()); + + // Set the motor outputs. + m_driveMotor.Set(driveOutput); + m_turningMotor.Set(turnOutput); +} + +void SwerveModule::ResetEncoders() { + m_driveEncoder.Reset(); + m_turningEncoder.Reset(); +} diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h new file mode 100644 index 0000000000..ed1f8809bb --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h @@ -0,0 +1,113 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include +#include +#include +#include +#include + +#pragma once + +/** + * The Constants header provides a convenient place for teams to hold robot-wide + * numerical or bool constants. This should not be used for any other purpose. + * + * It is generally a good idea to place constants into subsystem- or + * command-specific namespaces within this header, which can then be used where + * they are needed. + */ + +namespace DriveConstants { +constexpr int kFrontLeftDriveMotorPort = 0; +constexpr int kRearLeftDriveMotorPort = 2; +constexpr int kFrontRightDriveMotorPort = 4; +constexpr int kRearRightDriveMotorPort = 6; + +constexpr int kFrontLeftTurningMotorPort = 1; +constexpr int kRearLeftTurningMotorPort = 3; +constexpr int kFrontRightTurningMotorPort = 5; +constexpr int kRearRightTurningMotorPort = 7; + +constexpr int kFrontLeftTurningEncoderPorts[2]{0, 1}; +constexpr int kRearLeftTurningEncoderPorts[2]{2, 3}; +constexpr int kFrontRightTurningEncoderPorts[2]{4, 5}; +constexpr int kRearRightTurningEncoderPorts[2]{5, 6}; + +constexpr bool kFrontLeftTurningEncoderReversed = false; +constexpr bool kRearLeftTurningEncoderReversed = true; +constexpr bool kFrontRightTurningEncoderReversed = false; +constexpr bool kRearRightTurningEncoderReversed = true; + +constexpr int kFrontLeftDriveEncoderPorts[2]{0, 1}; +constexpr int kRearLeftDriveEncoderPorts[2]{2, 3}; +constexpr int kFrontRightDriveEncoderPorts[2]{4, 5}; +constexpr int kRearRightDriveEncoderPorts[2]{5, 6}; + +constexpr bool kFrontLeftDriveEncoderReversed = false; +constexpr bool kRearLeftDriveEncoderReversed = true; +constexpr bool kFrontRightDriveEncoderReversed = false; +constexpr bool kRearRightDriveEncoderReversed = true; + +constexpr bool kGyroReversed = false; + +// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! +// These characterization values MUST be determined either experimentally or +// theoretically for *your* robot's drive. The RobotPy Characterization +// Toolsuite provides a convenient tool for obtaining these values for your +// robot. +constexpr auto ks = 1_V; +constexpr auto kv = 0.8 * 1_V * 1_s / 1_m; +constexpr auto ka = 0.15 * 1_V * 1_s * 1_s / 1_m; + +// Example value only - as above, this must be tuned for your drive! +constexpr double kPFrontLeftVel = 0.5; +constexpr double kPRearLeftVel = 0.5; +constexpr double kPFrontRightVel = 0.5; +constexpr double kPRearRightVel = 0.5; +} // namespace DriveConstants + +namespace ModuleConstants { +constexpr int kEncoderCPR = 1024; +constexpr double kWheelDiameterMeters = .15; +constexpr double kDriveEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + (kWheelDiameterMeters * wpi::math::pi) / static_cast(kEncoderCPR); + +constexpr double kTurningEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + (wpi::math::pi * 2) / static_cast(kEncoderCPR); + +constexpr double kPModuleTurningController = 1; +constexpr double kPModuleDriveController = 1; +} // namespace ModuleConstants + +namespace AutoConstants { +using radians_per_second_squared_t = + units::compound_unit>>; + +constexpr auto kMaxSpeed = units::meters_per_second_t(3); +constexpr auto kMaxAcceleration = units::meters_per_second_squared_t(3); +constexpr auto kMaxAngularSpeed = units::radians_per_second_t(3.142); +constexpr auto kMaxAngularAcceleration = + units::unit_t(3.142); + +constexpr double kPXController = 0.5; +constexpr double kPYController = 0.5; +constexpr double kPThetaController = 0.5; + +// + +constexpr frc::TrapezoidProfile::Constraints + kThetaControllerConstraints{kMaxAngularSpeed, kMaxAngularAcceleration}; + +} // namespace AutoConstants + +namespace OIConstants { +constexpr int kDriverControllerPort = 1; +} // namespace OIConstants diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h new file mode 100644 index 0000000000..fa173d39e1 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "RobotContainer.h" + +class Robot : public frc::TimedRobot { + public: + void RobotInit() override; + void RobotPeriodic() override; + void DisabledInit() override; + void DisabledPeriodic() override; + void AutonomousInit() override; + void AutonomousPeriodic() override; + void TeleopInit() override; + void TeleopPeriodic() override; + void TestPeriodic() override; + + private: + // Have it null by default so that if testing teleop it + // doesn't have undefined behavior and potentially crash. + frc2::Command* m_autonomousCommand = nullptr; + + RobotContainer m_container; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h new file mode 100644 index 0000000000..8b366179fe --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h @@ -0,0 +1,49 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Constants.h" +#include "subsystems/DriveSubsystem.h" + +/** + * This class is where the bulk of the robot should be declared. Since + * Command-based is a "declarative" paradigm, very little robot logic should + * actually be handled in the {@link Robot} periodic methods (other than the + * scheduler calls). Instead, the structure of the robot (including subsystems, + * commands, and button mappings) should be declared here. + */ +class RobotContainer { + public: + RobotContainer(); + + frc2::Command* GetAutonomousCommand(); + + private: + // The driver's controller + frc::XboxController m_driverController{OIConstants::kDriverControllerPort}; + + // The robot's subsystems and commands are defined here... + + // The robot's subsystems + DriveSubsystem m_drive; + + // The chooser for the autonomous routines + frc::SendableChooser m_chooser; + + void ConfigureButtonBindings(); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h new file mode 100644 index 0000000000..db4623f8aa --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h @@ -0,0 +1,120 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Constants.h" +#include "SwerveModule.h" + +class DriveSubsystem : public frc2::SubsystemBase { + public: + DriveSubsystem(); + + /** + * Will be called periodically whenever the CommandScheduler runs. + */ + void Periodic() override; + + // Subsystem methods go here. + + /** + * Drives the robot at given x, y and theta speeds. Speeds range from [-1, 1] + * and the linear speeds have no effect on the angular speed. + * + * @param xSpeed Speed of the robot in the x direction + * (forward/backwards). + * @param ySpeed Speed of the robot in the y direction (sideways). + * @param rot Angular rate of the robot. + * @param fieldRelative Whether the provided x and y speeds are relative to + * the field. + */ + void Drive(units::meters_per_second_t xSpeed, + units::meters_per_second_t ySpeed, units::radians_per_second_t rot, + bool feildRelative); + + /** + * Resets the drive encoders to currently read a position of 0. + */ + void ResetEncoders(); + + /** + * Sets the drive SpeedControllers to a power from -1 to 1. + */ + void SetModuleStates(std::array desiredStates); + + /** + * Returns the heading of the robot. + * + * @return the robot's heading in degrees, from 180 to 180 + */ + double GetHeading(); + + /** + * Zeroes the heading of the robot. + */ + void ZeroHeading(); + + /** + * Returns the turn rate of the robot. + * + * @return The turn rate of the robot, in degrees per second + */ + double GetTurnRate(); + + /** + * Returns the currently-estimated pose of the robot. + * + * @return The pose. + */ + frc::Pose2d GetPose(); + + /** + * Resets the odometry to the specified pose. + * + * @param pose The pose to which to set the odometry. + */ + void ResetOdometry(frc::Pose2d pose); + + units::meter_t kTrackWidth = + .5_m; // Distance between centers of right and left wheels on robot + units::meter_t kWheelBase = + .7_m; // Distance between centers of front and back wheels on robot + + frc::SwerveDriveKinematics<4> kDriveKinematics{ + frc::Translation2d(kWheelBase / 2, kTrackWidth / 2), + frc::Translation2d(kWheelBase / 2, -kTrackWidth / 2), + frc::Translation2d(-kWheelBase / 2, kTrackWidth / 2), + frc::Translation2d(-kWheelBase / 2, -kTrackWidth / 2)}; + + private: + // Components (e.g. motor controllers and sensors) should generally be + // declared private and exposed only through public methods. + + SwerveModule m_frontLeft; + SwerveModule m_rearLeft; + SwerveModule m_frontRight; + SwerveModule m_rearRight; + + // The gyro sensor + frc::ADXRS450_Gyro m_gyro; + + // Odometry class for tracking robot pose + // 4 defines the number of modules + frc::SwerveDriveOdometry<4> m_odometry; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h new file mode 100644 index 0000000000..f8d816a359 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h @@ -0,0 +1,65 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Constants.h" + +class SwerveModule { + using radians_per_second_squared_t = + units::compound_unit>>; + + public: + SwerveModule(int driveMotorChannel, int turningMotorChannel, + const int driveEncoderPorts[2], const int turningEncoderPorts[2], + bool driveEncoderReversed, bool turningEncoderReversed); + + frc::SwerveModuleState GetState(); + + void SetDesiredState(frc::SwerveModuleState& state); + + void ResetEncoders(); + + private: + // We have to use meters here instead of radians due to the fact that + // ProfiledPIDController's constraints only take in meters per second and + // meters per second squared. + + static constexpr units::radians_per_second_t kModuleMaxAngularVelocity = + units::radians_per_second_t(wpi::math::pi); // radians per second + static constexpr units::unit_t + kModuleMaxAngularAcceleration = + units::unit_t( + wpi::math::pi * 2.0); // radians per second squared + + frc::Spark m_driveMotor; + frc::Spark m_turningMotor; + + frc::Encoder m_driveEncoder; + frc::Encoder m_turningEncoder; + + bool m_reverseDriveEncoder; + bool m_reverseTurningEncoder; + + frc2::PIDController m_drivePIDController{ + ModuleConstants::kPModuleDriveController, 0, 0}; + frc::ProfiledPIDController m_turningPIDController{ + ModuleConstants::kPModuleTurningController, + 0.0, + 0.0, + {kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration}}; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index 8f78436829..60730dbdad 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -468,5 +468,33 @@ "foldername": "DMA", "gradlebase": "cpp", "commandversion": 2 + }, + { + "name": "MecanumControllerCommand", + "description": "An example command-based robot demonstrating the use of a MecanumControllerCommand to follow a pregenerated trajectory.", + "tags": [ + "MecanumControllerCommand", + "Mecanum", + "PID", + "Trajectory", + "Path following" + ], + "foldername": "MecanumControllerCommand", + "gradlebase": "cpp", + "commandversion": 2 + }, + { + "name": "SwerveControllerCommand", + "description": "An example command-based robot demonstrating the use of a SwerveControllerCommand to follow a pregenerated trajectory.", + "tags": [ + "SwerveControllerCommand", + "Swerve", + "PID", + "Trajectory", + "Path following" + ], + "foldername": "SwerveControllerCommand", + "gradlebase": "cpp", + "commandversion": 2 } ] diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java new file mode 100644 index 0000000000..af4a7f1d8d --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java @@ -0,0 +1,58 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.kinematics; + +/** + * Represents the motor voltages for a mecanum drive drivetrain. + */ +@SuppressWarnings("MemberName") +public class MecanumDriveMotorVoltages { + /** + * Voltage of the front left motor. + */ + public double frontLeftVoltage; + + /** + * Voltage of the front right motor. + */ + public double frontRightVoltage; + + /** + * Voltage of the rear left motor. + */ + public double rearLeftVoltage; + + /** + * Voltage of the rear right motor. + */ + public double rearRightVoltage; + + /** + * Constructs a MecanumDriveMotorVoltages with zeros for all member fields. + */ + public MecanumDriveMotorVoltages() { + } + + /** + * Constructs a MecanumDriveMotorVoltages. + * + * @param frontLeftVoltage Voltage of the front left motor. + * @param frontRightVoltage Voltage of the front right motor. + * @param rearLeftVoltage Voltage of the rear left motor. + * @param rearRightVoltage Voltage of the rear right motor. + */ + public MecanumDriveMotorVoltages(double frontLeftVoltage, + double frontRightVoltage, + double rearLeftVoltage, + double rearRightVoltage) { + this.frontLeftVoltage = frontLeftVoltage; + this.frontRightVoltage = frontRightVoltage; + this.rearLeftVoltage = rearLeftVoltage; + this.rearRightVoltage = rearRightVoltage; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java index 7051a0fdde..fbc5ddbee4 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java @@ -11,7 +11,11 @@ import java.util.ArrayList; import java.util.List; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics; +import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics; import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint; +import edu.wpi.first.wpilibj.trajectory.constraint.MecanumDriveKinematicsConstraint; +import edu.wpi.first.wpilibj.trajectory.constraint.SwerveDriveKinematicsConstraint; import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint; /** @@ -77,10 +81,34 @@ public class TrajectoryConfig { } /** - * Returns the starting velocity of the trajectory. - * - * @return The starting velocity of the trajectory. - */ + * Adds a mecanum drive kinematics constraint to ensure that + * no wheel velocity of a mecanum drive goes above the max velocity. + * + * @param kinematics The mecanum drive kinematics. + * @return Instance of the current config object. + */ + public TrajectoryConfig setKinematics(MecanumDriveKinematics kinematics) { + addConstraint(new MecanumDriveKinematicsConstraint(kinematics, m_maxVelocity)); + return this; + } + + /** + * Adds a swerve drive kinematics constraint to ensure that + * no wheel velocity of a swerve drive goes above the max velocity. + * + * @param kinematics The swerve drive kinematics. + * @return Instance of the current config object. + */ + public TrajectoryConfig setKinematics(SwerveDriveKinematics kinematics) { + addConstraint(new SwerveDriveKinematicsConstraint(kinematics, m_maxVelocity)); + return this; + } + + /** + * Returns the starting velocity of the trajectory. + * + * @return The starting velocity of the trajectory. + */ public double getStartVelocity() { return m_startVelocity; } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java new file mode 100644 index 0000000000..dfa2583e46 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java @@ -0,0 +1,84 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.trajectory.constraint; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics; + +/** + * A class that enforces constraints on the mecanum drive kinematics. + * This can be used to ensure that the trajectory is constructed so that the + * commanded velocities for all 4 wheels of the drivetrain stay below a certain + * limit. + */ +public class MecanumDriveKinematicsConstraint implements TrajectoryConstraint { + private final double m_maxSpeedMetersPerSecond; + private final MecanumDriveKinematics m_kinematics; + + /** + * Constructs a mecanum drive dynamics constraint. + * + * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at. + */ + public MecanumDriveKinematicsConstraint(final MecanumDriveKinematics kinematics, + double maxSpeedMetersPerSecond) { + m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond; + m_kinematics = kinematics; + } + + + /** + * Returns the max velocity given the current pose and curvature. + * + * @param poseMeters The pose at the current point in the trajectory. + * @param curvatureRadPerMeter The curvature at the current point in the trajectory. + * @param velocityMetersPerSecond The velocity at the current point in the trajectory before + * constraints are applied. + * @return The absolute maximum velocity. + */ + @Override + public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter, + double velocityMetersPerSecond) { + // Represents the velocity of the chassis in the x direction + var xdVelocity = velocityMetersPerSecond * poseMeters.getRotation().getCos(); + + // Represents the velocity of the chassis in the y direction + var ydVelocity = velocityMetersPerSecond * poseMeters.getRotation().getSin(); + + // Create an object to represent the current chassis speeds. + var chassisSpeeds = new ChassisSpeeds(xdVelocity, + ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter); + + // Get the wheel speeds and normalize them to within the max velocity. + var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds); + wheelSpeeds.normalize(m_maxSpeedMetersPerSecond); + + // Convert normalized wheel speeds back to chassis speeds + var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds); + + // Return the new linear chassis speed. + return Math.hypot(normSpeeds.vxMetersPerSecond, normSpeeds.vyMetersPerSecond); + } + + /** + * Returns the minimum and maximum allowable acceleration for the trajectory + * given pose, curvature, and speed. + * + * @param poseMeters The pose at the current point in the trajectory. + * @param curvatureRadPerMeter The curvature at the current point in the trajectory. + * @param velocityMetersPerSecond The speed at the current point in the trajectory. + * @return The min and max acceleration bounds. + */ + @Override + public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters, + double curvatureRadPerMeter, + double velocityMetersPerSecond) { + return new MinMax(); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java new file mode 100644 index 0000000000..d567009af2 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java @@ -0,0 +1,84 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.trajectory.constraint; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics; + +/** + * A class that enforces constraints on the swerve drive kinematics. + * This can be used to ensure that the trajectory is constructed so that the + * commanded velocities for all 4 wheels of the drivetrain stay below a certain + * limit. + */ +public class SwerveDriveKinematicsConstraint implements TrajectoryConstraint { + private final double m_maxSpeedMetersPerSecond; + private final SwerveDriveKinematics m_kinematics; + + /** + * Constructs a mecanum drive dynamics constraint. + * + * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at. + */ + public SwerveDriveKinematicsConstraint(final SwerveDriveKinematics kinematics, + double maxSpeedMetersPerSecond) { + m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond; + m_kinematics = kinematics; + } + + + /** + * Returns the max velocity given the current pose and curvature. + * + * @param poseMeters The pose at the current point in the trajectory. + * @param curvatureRadPerMeter The curvature at the current point in the trajectory. + * @param velocityMetersPerSecond The velocity at the current point in the trajectory before + * constraints are applied. + * @return The absolute maximum velocity. + */ + @Override + public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter, + double velocityMetersPerSecond) { + // Represents the velocity of the chassis in the x direction + var xdVelocity = velocityMetersPerSecond * poseMeters.getRotation().getCos(); + + // Represents the velocity of the chassis in the y direction + var ydVelocity = velocityMetersPerSecond * poseMeters.getRotation().getSin(); + + // Create an object to represent the current chassis speeds. + var chassisSpeeds = new ChassisSpeeds(xdVelocity, + ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter); + + // Get the wheel speeds and normalize them to within the max velocity. + var wheelSpeeds = m_kinematics.toSwerveModuleStates(chassisSpeeds); + SwerveDriveKinematics.normalizeWheelSpeeds(wheelSpeeds, m_maxSpeedMetersPerSecond); + + // Convert normalized wheel speeds back to chassis speeds + var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds); + + // Return the new linear chassis speed. + return Math.hypot(normSpeeds.vxMetersPerSecond, normSpeeds.vyMetersPerSecond); + } + + /** + * Returns the minimum and maximum allowable acceleration for the trajectory + * given pose, curvature, and speed. + * + * @param poseMeters The pose at the current point in the trajectory. + * @param curvatureRadPerMeter The curvature at the current point in the trajectory. + * @param velocityMetersPerSecond The speed at the current point in the trajectory. + * @return The min and max acceleration bounds. + */ + @Override + public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters, + double curvatureRadPerMeter, + double velocityMetersPerSecond) { + return new MinMax(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json index 212d20bddf..55b2ebe5a2 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json @@ -482,5 +482,35 @@ "gradlebase": "java", "mainclass": "Main", "commandversion": 2 + }, + { + "name": "MecanumControllerCommand", + "description": "An example command-based robot demonstrating the use of a MecanumControllerCommand to follow a pregenerated trajectory.", + "tags": [ + "MecanumControllerCommand", + "Mecanum", + "PID", + "Trajectory", + "Path following" + ], + "foldername": "mecanumcontrollercommand", + "gradlebase": "java", + "mainclass": "Main", + "commandversion": 2 + }, + { + "name": "SwerveControllerCommand", + "description": "An example command-based robot demonstrating the use of a SwerveControllerCommand to follow a pregenerated trajectory.", + "tags": [ + "SwerveControllerCommand", + "Swerve", + "PID", + "Trajectory", + "Path following" + ], + "foldername": "swervecontrollercommand", + "gradlebase": "java", + "mainclass": "Main", + "commandversion": 2 } ] diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java new file mode 100644 index 0000000000..88f904b62e --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java @@ -0,0 +1,97 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.mecanumcontrollercommand; + +import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics; +import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be + * declared globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + public static final class DriveConstants { + public static final int kFrontLeftMotorPort = 0; + public static final int kRearLeftMotorPort = 1; + public static final int kFrontRightMotorPort = 2; + public static final int kRearRightMotorPort = 3; + + public static final int[] kFrontLeftEncoderPorts = new int[]{0, 1}; + public static final int[] kRearLeftEncoderPorts = new int[]{2, 3}; + public static final int[] kFrontRightEncoderPorts = new int[]{4, 5}; + public static final int[] kRearRightEncoderPorts = new int[]{5, 6}; + + public static final boolean kFrontLeftEncoderReversed = false; + public static final boolean kRearLeftEncoderReversed = true; + public static final boolean kFrontRightEncoderReversed = false; + public static final boolean kRearRightEncoderReversed = true; + + public static final double kTrackWidth = 0.5; + // Distance between centers of right and left wheels on robot + public static final double kWheelBase = 0.7; + // Distance between centers of front and back wheels on robot + + public static final MecanumDriveKinematics kDriveKinematics = + new MecanumDriveKinematics( + new Translation2d(kWheelBase / 2, kTrackWidth / 2), + new Translation2d(kWheelBase / 2, -kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); + + public static final int kEncoderCPR = 1024; + public static final double kWheelDiameterMeters = 0.15; + public static final double kEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + (kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR; + + public static final boolean kGyroReversed = false; + + // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! + // These characterization values MUST be determined either experimentally or theoretically + // for *your* robot's drive. + // The RobotPy Characterization Toolsuite provides a convenient tool for obtaining these + // values for your robot. + public static final SimpleMotorFeedforward kFeedforward = + new SimpleMotorFeedforward(1, 0.8, 0.15); + + // Example value only - as above, this must be tuned for your drive! + public static final double kPFrontLeftVel = 0.5; + public static final double kPRearLeftVel = 0.5; + public static final double kPFrontRightVel = 0.5; + public static final double kPRearRightVel = 0.5; + + } + + public static final class OIConstants { + public static final int kDriverControllerPort = 1; + + } + + public static final class AutoConstants { + public static final double kMaxSpeedMetersPerSecond = 3; + public static final double kMaxAccelerationMetersPerSecondSquared = 3; + public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; + public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; + + public static final double kPXController = 0.5; + public static final double kPYController = 0.5; + public static final double kPThetaController = 0.5; + + //Constraint for the motion profilied robot angle controller + public static final TrapezoidProfile.Constraints kThetaControllerConstraints = + new TrapezoidProfile.Constraints(kMaxAngularSpeedRadiansPerSecond, + kMaxAngularSpeedRadiansPerSecondSquared); + + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Main.java new file mode 100644 index 0000000000..9670d3de10 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.mecanumcontrollercommand; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Robot.java new file mode 100644 index 0000000000..6f2ccbfd46 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Robot.java @@ -0,0 +1,121 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.mecanumcontrollercommand; + +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +/** + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the + * project. + */ +public class Robot extends TimedRobot { + private Command m_autonomousCommand; + + private RobotContainer m_robotContainer; + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); + } + + /** + * This function is called every robot packet, no matter the mode. Use this for items like + * diagnostics that you want ran during disabled, autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() { + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled + // commands, running already-scheduled commands, removing finished or interrupted commands, + // and running subsystem periodic() methods. This must be called from the robot's periodic + // block in order for anything in the Command-based framework to work. + CommandScheduler.getInstance().run(); + } + + /** + * This function is called once each time the robot enters Disabled mode. + */ + @Override + public void disabledInit() { + } + + @Override + public void disabledPeriodic() { + } + + /** + * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. + */ + @Override + public void autonomousInit() { + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + /* + * String autoSelected = SmartDashboard.getString("Auto Selector", + * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand + * = new MyAutoCommand(); break; case "Default Auto": default: + * autonomousCommand = new ExampleCommand(); break; } + */ + + // schedule the autonomous command (example) + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } + } + + /** + * This function is called periodically during autonomous. + */ + @Override + public void autonomousPeriodic() { + } + + @Override + public void teleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } + } + + /** + * This function is called periodically during operator control. + */ + @Override + public void teleopPeriodic() { + + } + + @Override + public void testInit() { + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); + } + + /** + * This function is called periodically during test mode. + */ + @Override + public void testPeriodic() { + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java new file mode 100644 index 0000000000..54707e37fc --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java @@ -0,0 +1,148 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.mecanumcontrollercommand; + +import java.util.List; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.XboxController.Button; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.controller.ProfiledPIDController; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.trajectory.Trajectory; +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.MecanumControllerCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + +import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.subsystems.DriveSubsystem; + +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared; +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kMaxSpeedMetersPerSecond; +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kPThetaController; +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kPXController; +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kPYController; +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kThetaControllerConstraints; +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kDriveKinematics; +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFeedforward; +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kPFrontLeftVel; +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kPFrontRightVel; +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kPRearLeftVel; +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kPRearRightVel; +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.OIConstants.kDriverControllerPort; + +/* + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of the robot + * (including subsystems, commands, and button mappings) should be declared here. + */ +@SuppressWarnings("PMD.ExcessiveImports") +public class RobotContainer { + // The robot's subsystems + private final DriveSubsystem m_robotDrive = new DriveSubsystem(); + + // The driver's controller + XboxController m_driverController = new XboxController(kDriverControllerPort); + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + // Configure the button bindings + configureButtonBindings(); + + // Configure default commands + // Set the default drive command to split-stick arcade drive + m_robotDrive.setDefaultCommand( + // A split-stick arcade command, with forward/backward controlled by the left + // hand, and turning controlled by the right. + new RunCommand(() -> m_robotDrive.drive( + m_driverController.getY(GenericHID.Hand.kLeft), + m_driverController.getX(GenericHID.Hand.kRight), + m_driverController.getX(GenericHID.Hand.kLeft), false))); + + } + + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a + * {@link JoystickButton}. + */ + private void configureButtonBindings() { + // Drive at half speed when the right bumper is held + new JoystickButton(m_driverController, Button.kBumperRight.value) + .whenPressed(() -> m_robotDrive.setMaxOutput(0.5)) + .whenReleased(() -> m_robotDrive.setMaxOutput(1)); + + } + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + // Create config for trajectory + TrajectoryConfig config = + new TrajectoryConfig(kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared) + // Add kinematics to ensure max speed is actually obeyed + .setKinematics(kDriveKinematics); + + // An example trajectory to follow. All units in meters. + Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory( + // Start at the origin facing the +X direction + new Pose2d(0, 0, new Rotation2d(0)), + // Pass through these two interior waypoints, making an 's' curve path + List.of( + new Translation2d(1, 1), + new Translation2d(2, - 1) + ), + // End 3 meters straight ahead of where we started, facing forward + new Pose2d(3, 0, new Rotation2d(0)), + config + ); + + MecanumControllerCommand mecanumControllerCommand = new MecanumControllerCommand( + exampleTrajectory, + m_robotDrive::getPose, + + kFeedforward, + kDriveKinematics, + + //Position contollers + new PIDController(kPXController, 0, 0), + new PIDController(kPYController, 0, 0), + new ProfiledPIDController(kPThetaController, 0, 0, kThetaControllerConstraints), + + //Needed for normalizing wheel speeds + kMaxSpeedMetersPerSecond, + + //Velocity PID's + new PIDController(kPFrontLeftVel, 0, 0), + new PIDController(kPRearLeftVel, 0, 0), + new PIDController(kPFrontRightVel, 0, 0), + new PIDController(kPRearRightVel, 0, 0), + + m_robotDrive::getCurrentWheelSpeeds, + + m_robotDrive::setDriveSpeedControllersVolts, //Consumer for the output motor voltages + + m_robotDrive + ); + + // Run path following command, then stop at the end. + return mecanumControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false)); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java new file mode 100644 index 0000000000..b68f770558 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java @@ -0,0 +1,253 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.subsystems; + +import edu.wpi.first.wpilibj.ADXRS450_Gyro; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.drive.MecanumDrive; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.interfaces.Gyro; +import edu.wpi.first.wpilibj.kinematics.MecanumDriveMotorVoltages; +import edu.wpi.first.wpilibj.kinematics.MecanumDriveOdometry; +import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kDriveKinematics; +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kEncoderDistancePerPulse; +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontLeftEncoderPorts; +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontLeftEncoderReversed; +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontLeftMotorPort; +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontRightEncoderPorts; +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontRightEncoderReversed; +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontRightMotorPort; +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kGyroReversed; +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearLeftEncoderPorts; +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearLeftEncoderReversed; +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearLeftMotorPort; +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearRightEncoderPorts; +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearRightEncoderReversed; +import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearRightMotorPort; + +public class DriveSubsystem extends SubsystemBase { + private final PWMVictorSPX m_frontLeft = new PWMVictorSPX(kFrontLeftMotorPort); + private final PWMVictorSPX m_rearLeft = new PWMVictorSPX(kRearLeftMotorPort); + private final PWMVictorSPX m_frontRight = new PWMVictorSPX(kFrontRightMotorPort); + private final PWMVictorSPX m_rearRight = new PWMVictorSPX(kRearRightMotorPort); + + private final MecanumDrive m_drive = new MecanumDrive( + m_frontLeft, + m_rearLeft, + m_frontRight, + m_rearRight); + + // The front-left-side drive encoder + private final Encoder m_frontLeftEncoder = + new Encoder(kFrontLeftEncoderPorts[0], kFrontLeftEncoderPorts[1], + kFrontLeftEncoderReversed); + + // The rear-left-side drive encoder + private final Encoder m_rearLeftEncoder = + new Encoder(kRearLeftEncoderPorts[0], kRearLeftEncoderPorts[1], + kRearLeftEncoderReversed); + + // The front-right--side drive encoder + private final Encoder m_frontRightEncoder = + new Encoder(kFrontRightEncoderPorts[0], kFrontRightEncoderPorts[1], + kFrontRightEncoderReversed); + + // The rear-right-side drive encoder + private final Encoder m_rearRightEncoder = + new Encoder(kRearRightEncoderPorts[0], kRearRightEncoderPorts[1], + kRearRightEncoderReversed); + + // The gyro sensor + private final Gyro m_gyro = new ADXRS450_Gyro(); + + // Odometry class for tracking robot pose + MecanumDriveOdometry m_odometry = + new MecanumDriveOdometry(kDriveKinematics, getAngle()); + + /** + * Creates a new DriveSubsystem. + */ + public DriveSubsystem() { + // Sets the distance per pulse for the encoders + m_frontLeftEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + m_rearLeftEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + m_frontRightEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + m_rearRightEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + } + + /** + * Returns the angle of the robot as a Rotation2d. + * + * @return The angle of the robot. + */ + public Rotation2d getAngle() { + // Negating the angle because WPILib gyros are CW positive. + return Rotation2d.fromDegrees(m_gyro.getAngle() * (kGyroReversed ? 1.0 : -1.0)); + } + + @Override + public void periodic() { + // Update the odometry in the periodic block + m_odometry.update(getAngle(), + new MecanumDriveWheelSpeeds( + m_frontLeftEncoder.getRate(), + m_rearLeftEncoder.getRate(), + m_frontRightEncoder.getRate(), + m_rearRightEncoder.getRate())); + } + + /** + * Returns the currently-estimated pose of the robot. + * + * @return The pose. + */ + public Pose2d getPose() { + return m_odometry.getPoseMeters(); + } + + /** + * Resets the odometry to the specified pose. + * + * @param pose The pose to which to set the odometry. + */ + public void resetOdometry(Pose2d pose) { + m_odometry.resetPosition(pose, getAngle()); + } + + /** + * Drives the robot at given x, y and theta speeds. Speeds range from [-1, 1] and the linear + * speeds have no effect on the angular speed. + * + * @param xSpeed Speed of the robot in the x direction (forward/backwards). + * @param ySpeed Speed of the robot in the y direction (sideways). + * @param rot Angular rate of the robot. + * @param fieldRelative Whether the provided x and y speeds are relative to the field. + */ + @SuppressWarnings("ParameterName") + public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { + if ( fieldRelative ) { + m_drive.driveCartesian(ySpeed, xSpeed, rot, -m_gyro.getAngle()); + } else { + m_drive.driveCartesian(ySpeed, xSpeed, rot); + } + + } + + /** + * Sets the front left drive SpeedController to a voltage. + */ + public void setDriveSpeedControllersVolts(MecanumDriveMotorVoltages volts) { + m_frontLeft.setVoltage(volts.frontLeftVoltage); + m_rearLeft.setVoltage(volts.rearLeftVoltage); + m_frontRight.setVoltage(volts.frontRightVoltage); + m_rearRight.setVoltage(volts.rearRightVoltage); + } + + + /** + * Resets the drive encoders to currently read a position of 0. + */ + public void resetEncoders() { + m_frontLeftEncoder.reset(); + m_rearLeftEncoder.reset(); + m_frontRightEncoder.reset(); + m_rearRightEncoder.reset(); + } + + /** + * Gets the front left drive encoder. + * + * @return the front left drive encoder + */ + + public Encoder getFrontLeftEncoder() { + return m_frontLeftEncoder; + } + + /** + * Gets the rear left drive encoder. + * + * @return the rear left drive encoder + */ + + public Encoder getRearLeftEncoder() { + return m_rearLeftEncoder; + } + + /** + * Gets the front right drive encoder. + * + * @return the front right drive encoder + */ + + public Encoder getFrontRightEncoder() { + return m_frontRightEncoder; + } + /** + * Gets the rear right drive encoder. + * + * @return the rear right encoder + */ + + public Encoder getRearRightEncoder() { + return m_rearRightEncoder; + } + + /** + * Gets the current wheel speeds. + * + * @return the current wheel speeds in a MecanumDriveWheelSpeeds object. + */ + + public MecanumDriveWheelSpeeds getCurrentWheelSpeeds() { + return new MecanumDriveWheelSpeeds(m_frontLeftEncoder.getRate(), + m_rearLeftEncoder.getRate(), + m_frontRightEncoder.getRate(), + m_rearRightEncoder.getRate()); + } + + + /** + * Sets the max output of the drive. Useful for scaling the drive to drive more slowly. + * + * @param maxOutput the maximum output to which the drive will be constrained + */ + public void setMaxOutput(double maxOutput) { + m_drive.setMaxOutput(maxOutput); + } + + /** + * Zeroes the heading of the robot. + */ + public void zeroHeading() { + m_gyro.reset(); + } + + /** + * Returns the heading of the robot. + * + * @return the robot's heading in degrees, from 180 to 180 + */ + public double getHeading() { + return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1.0 : 1.0); + } + + /** + * Returns the turn rate of the robot. + * + * @return The turn rate of the robot, in degrees per second + */ + public double getTurnRate() { + return m_gyro.getRate() * (kGyroReversed ? -1.0 : 1.0); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java index aae8425eda..0d5397e4ad 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java @@ -67,7 +67,7 @@ public final class Constants { new DifferentialDriveKinematicsConstraint(DriveConstants.kDriveKinematics, kMaxSpeedMetersPerSecond); - // Reasonable baseline values for a RAMSETE follower in units of meters and seconds + // Reasonable baseline values for a RAMSETE Controller in units of meters and seconds public static final double kRamseteB = 2; public static final double kRamseteZeta = 0.7; } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java new file mode 100644 index 0000000000..19fc601050 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java @@ -0,0 +1,120 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.swervecontrollercommand; + +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics; +import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be + * declared globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + public static final class DriveConstants { + public static final int kFrontLeftDriveMotorPort = 0; + public static final int kRearLeftDriveMotorPort = 2; + public static final int kFrontRightDriveMotorPort = 4; + public static final int kRearRightDriveMotorPort = 6; + + public static final int kFrontLeftTurningMotorPort = 1; + public static final int kRearLeftTurningMotorPort = 3; + public static final int kFrontRightTurningMotorPort = 5; + public static final int kRearRightTurningMotorPort = 7; + + public static final int[] kFrontLeftTurningEncoderPorts = new int[]{0, 1}; + public static final int[] kRearLeftTurningEncoderPorts = new int[]{2, 3}; + public static final int[] kFrontRightTurningEncoderPorts = new int[]{4, 5}; + public static final int[] kRearRightTurningEncoderPorts = new int[]{5, 6}; + + public static final boolean kFrontLeftTurningEncoderReversed = false; + public static final boolean kRearLeftTurningEncoderReversed = true; + public static final boolean kFrontRightTurningEncoderReversed = false; + public static final boolean kRearRightTurningEncoderReversed = true; + + public static final int[] kFrontLeftDriveEncoderPorts = new int[]{7, 8}; + public static final int[] kRearLeftDriveEncoderPorts = new int[]{9, 10}; + public static final int[] kFrontRightDriveEncoderPorts = new int[]{11, 12}; + public static final int[] kRearRightDriveEncoderPorts = new int[]{13, 14}; + + public static final boolean kFrontLeftDriveEncoderReversed = false; + public static final boolean kRearLeftDriveEncoderReversed = true; + public static final boolean kFrontRightDriveEncoderReversed = false; + public static final boolean kRearRightDriveEncoderReversed = true; + + + public static final double kTrackWidth = 0.5; + //Distance between centers of right and left wheels on robot + public static final double kWheelBase = 0.7; + //Distance between front and back wheels on robot + public static final SwerveDriveKinematics kDriveKinematics = + new SwerveDriveKinematics( + new Translation2d(kWheelBase / 2, kTrackWidth / 2), + new Translation2d(kWheelBase / 2, -kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); + + public static final boolean kGyroReversed = false; + + // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! + // These characterization values MUST be determined either experimentally or theoretically + // for *your* robot's drive. + // The RobotPy Characterization Toolsuite provides a convenient tool for obtaining these + // values for your robot. + public static final double ksVolts = 1; + public static final double kvVoltSecondsPerMeter = 0.8; + public static final double kaVoltSecondsSquaredPerMeter = 0.15; + + } + + public static final class ModuleConstants { + public static final double kMaxModuleAngularSpeedRadiansPerSecond = 2 * Math.PI; + public static final double kMaxModuleAngularAccelerationRadiansPerSecondSquared = 2 * Math.PI; + + public static final int kEncoderCPR = 1024; + public static final double kWheelDiameterMeters = 0.15; + public static final double kDriveEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + (kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR; + + public static final double kTurningEncoderDistancePerPulse = + // Assumes the encoders are on a 1:1 reduction with the module shaft. + (2 * Math.PI) / (double) kEncoderCPR; + + public static final double kPModuleTurningController = 1; + + public static final double kPModuleDriveController = 1; + + } + + public static final class OIConstants { + public static final int kDriverControllerPort = 1; + + } + + public static final class AutoConstants { + public static final double kMaxSpeedMetersPerSecond = 3; + public static final double kMaxAccelerationMetersPerSecondSquared = 3; + public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; + public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; + + public static final double kPXController = 1; + public static final double kPYController = 1; + public static final double kPThetaController = 1; + + //Constraint for the motion profilied robot angle controller + public static final TrapezoidProfile.Constraints kThetaControllerConstraints = + new TrapezoidProfile.Constraints(kMaxAngularSpeedRadiansPerSecond, + kMaxAngularSpeedRadiansPerSecondSquared); + + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Main.java new file mode 100644 index 0000000000..5c8a1dac8d --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.swervecontrollercommand; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Robot.java new file mode 100644 index 0000000000..3d41ba461d --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Robot.java @@ -0,0 +1,121 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.swervecontrollercommand; + +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +/** + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the + * project. + */ +public class Robot extends TimedRobot { + private Command m_autonomousCommand; + + private RobotContainer m_robotContainer; + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); + } + + /** + * This function is called every robot packet, no matter the mode. Use this for items like + * diagnostics that you want ran during disabled, autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() { + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled + // commands, running already-scheduled commands, removing finished or interrupted commands, + // and running subsystem periodic() methods. This must be called from the robot's periodic + // block in order for anything in the Command-based framework to work. + CommandScheduler.getInstance().run(); + } + + /** + * This function is called once each time the robot enters Disabled mode. + */ + @Override + public void disabledInit() { + } + + @Override + public void disabledPeriodic() { + } + + /** + * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. + */ + @Override + public void autonomousInit() { + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + /* + * String autoSelected = SmartDashboard.getString("Auto Selector", + * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand + * = new MyAutoCommand(); break; case "Default Auto": default: + * autonomousCommand = new ExampleCommand(); break; } + */ + + // schedule the autonomous command (example) + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } + } + + /** + * This function is called periodically during autonomous. + */ + @Override + public void autonomousPeriodic() { + } + + @Override + public void teleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } + } + + /** + * This function is called periodically during operator control. + */ + @Override + public void teleopPeriodic() { + + } + + @Override + public void testInit() { + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); + } + + /** + * This function is called periodically during test mode. + */ + @Override + public void testPeriodic() { + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java new file mode 100644 index 0000000000..d0fcc71c32 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java @@ -0,0 +1,125 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.swervecontrollercommand; + +import java.util.List; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.controller.ProfiledPIDController; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.trajectory.Trajectory; +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + +import edu.wpi.first.wpilibj.examples.swervecontrollercommand.subsystems.DriveSubsystem; + +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kMaxSpeedMetersPerSecond; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kPThetaController; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kPXController; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kPYController; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kThetaControllerConstraints; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kDriveKinematics; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.OIConstants.kDriverControllerPort; + +/* + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of the robot + * (including subsystems, commands, and button mappings) should be declared here. + */ +public class RobotContainer { + // The robot's subsystems + private final DriveSubsystem m_robotDrive = new DriveSubsystem(); + + // The driver's controller + XboxController m_driverController = new XboxController(kDriverControllerPort); + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + // Configure the button bindings + configureButtonBindings(); + + // Configure default commands + // Set the default drive command to split-stick arcade drive + m_robotDrive.setDefaultCommand( + // A split-stick arcade command, with forward/backward controlled by the left + // hand, and turning controlled by the right. + new RunCommand(() -> m_robotDrive.drive( + m_driverController.getY(GenericHID.Hand.kLeft), + m_driverController.getX(GenericHID.Hand.kRight), + m_driverController.getX(GenericHID.Hand.kLeft), false))); + + } + + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a + * {@link JoystickButton}. + */ + private void configureButtonBindings() { + } + + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + // Create config for trajectory + TrajectoryConfig config = + new TrajectoryConfig(kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared) + // Add kinematics to ensure max speed is actually obeyed + .setKinematics(kDriveKinematics); + + // An example trajectory to follow. All units in meters. + Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory( + // Start at the origin facing the +X direction + new Pose2d(0, 0, new Rotation2d(0)), + // Pass through these two interior waypoints, making an 's' curve path + List.of( + new Translation2d(1, 1), + new Translation2d(2, - 1) + ), + // End 3 meters straight ahead of where we started, facing forward + new Pose2d(3, 0, new Rotation2d(0)), + config + ); + + SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand( + exampleTrajectory, + m_robotDrive::getPose, //Functional interface to feed supplier + kDriveKinematics, + + //Position controllers + new PIDController(kPXController, 0, 0), + new PIDController(kPYController, 0, 0), + new ProfiledPIDController(kPThetaController, 0, 0, kThetaControllerConstraints), + + m_robotDrive::setModuleStates, + + m_robotDrive + + ); + + // Run path following command, then stop at the end. + return swerveControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false)); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java new file mode 100644 index 0000000000..ba80a15fe6 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java @@ -0,0 +1,200 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.swervecontrollercommand.subsystems; + +import edu.wpi.first.wpilibj.ADXRS450_Gyro; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.interfaces.Gyro; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics; +import edu.wpi.first.wpilibj.kinematics.SwerveDriveOdometry; +import edu.wpi.first.wpilibj.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kMaxSpeedMetersPerSecond; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kDriveKinematics; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftDriveEncoderPorts; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftDriveEncoderReversed; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftDriveMotorPort; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftTurningEncoderPorts; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftTurningEncoderReversed; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftTurningMotorPort; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightDriveEncoderPorts; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightDriveEncoderReversed; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightDriveMotorPort; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightTurningEncoderPorts; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightTurningEncoderReversed; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightTurningMotorPort; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kGyroReversed; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftDriveEncoderPorts; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftDriveEncoderReversed; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftDriveMotorPort; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftTurningEncoderPorts; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftTurningEncoderReversed; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftTurningMotorPort; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightDriveEncoderPorts; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightDriveEncoderReversed; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightDriveMotorPort; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightTurningEncoderPorts; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightTurningEncoderReversed; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightTurningMotorPort; + +@SuppressWarnings("PMD.ExcessiveImports") +public class DriveSubsystem extends SubsystemBase { + //Robot swerve modules + private final SwerveModule m_frontLeft = new SwerveModule(kFrontLeftDriveMotorPort, + kFrontLeftTurningMotorPort, + kFrontLeftDriveEncoderPorts, + kFrontLeftTurningEncoderPorts, + kFrontLeftDriveEncoderReversed, + kFrontLeftTurningEncoderReversed); + + private final SwerveModule m_rearLeft = new SwerveModule(kRearLeftDriveMotorPort, + kRearLeftTurningMotorPort, + kRearLeftDriveEncoderPorts, + kRearLeftTurningEncoderPorts, + kRearLeftDriveEncoderReversed, + kRearLeftTurningEncoderReversed); + + + private final SwerveModule m_frontRight = new SwerveModule(kFrontRightDriveMotorPort, + kFrontRightTurningMotorPort, + kFrontRightDriveEncoderPorts, + kFrontRightTurningEncoderPorts, + kFrontRightDriveEncoderReversed, + kFrontRightTurningEncoderReversed); + + private final SwerveModule m_rearRight = new SwerveModule(kRearRightDriveMotorPort, + kRearRightTurningMotorPort, + kRearRightDriveEncoderPorts, + kRearRightTurningEncoderPorts, + kRearRightDriveEncoderReversed, + kRearRightTurningEncoderReversed); + + // The gyro sensor + private final Gyro m_gyro = new ADXRS450_Gyro(); + + // Odometry class for tracking robot pose + SwerveDriveOdometry m_odometry = + new SwerveDriveOdometry(kDriveKinematics, getAngle()); + + /** + * Creates a new DriveSubsystem. + */ + public DriveSubsystem() {} + + /** + * Returns the angle of the robot as a Rotation2d. + * + * @return The angle of the robot. + */ + public Rotation2d getAngle() { + // Negating the angle because WPILib gyros are CW positive. + return Rotation2d.fromDegrees(m_gyro.getAngle() * (kGyroReversed ? 1.0 : -1.0)); + } + + @Override + public void periodic() { + // Update the odometry in the periodic block + m_odometry.update( + new Rotation2d(getHeading()), + m_frontLeft.getState(), + m_rearLeft.getState(), + m_frontRight.getState(), + m_rearRight.getState()); + } + + /** + * Returns the currently-estimated pose of the robot. + * + * @return The pose. + */ + public Pose2d getPose() { + return m_odometry.getPoseMeters(); + } + + /** + * Resets the odometry to the specified pose. + * + * @param pose The pose to which to set the odometry. + */ + public void resetOdometry(Pose2d pose) { + m_odometry.resetPosition(pose, getAngle()); + } + + /** + * Method to drive the robot using joystick info. + * + * @param xSpeed Speed of the robot in the x direction (forward). + * @param ySpeed Speed of the robot in the y direction (sideways). + * @param rot Angular rate of the robot. + * @param fieldRelative Whether the provided x and y speeds are relative to the field. + */ + @SuppressWarnings("ParameterName") + public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { + var swerveModuleStates = kDriveKinematics.toSwerveModuleStates( + fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, rot, getAngle()) + : new ChassisSpeeds(xSpeed, ySpeed, rot) + ); + SwerveDriveKinematics.normalizeWheelSpeeds(swerveModuleStates, kMaxSpeedMetersPerSecond); + m_frontLeft.setDesiredState(swerveModuleStates[0]); + m_frontRight.setDesiredState(swerveModuleStates[1]); + m_rearLeft.setDesiredState(swerveModuleStates[2]); + m_rearRight.setDesiredState(swerveModuleStates[3]); + } + + /** + * Sets the swerve ModuleStates. + * + * @param desiredStates The desired SwerveModule states. + */ + public void setModuleStates(SwerveModuleState[] desiredStates) { + SwerveDriveKinematics.normalizeWheelSpeeds(desiredStates, kMaxSpeedMetersPerSecond); + m_frontLeft.setDesiredState(desiredStates[0]); + m_frontRight.setDesiredState(desiredStates[1]); + m_rearLeft.setDesiredState(desiredStates[2]); + m_rearRight.setDesiredState(desiredStates[3]); + } + + /** + * Resets the drive encoders to currently read a position of 0. + */ + public void resetEncoders() { + m_frontLeft.resetEncoders(); + m_rearLeft.resetEncoders(); + m_frontRight.resetEncoders(); + m_rearRight.resetEncoders(); + } + + /** + * Zeroes the heading of the robot. + */ + public void zeroHeading() { + m_gyro.reset(); + } + + /** + * Returns the heading of the robot. + * + * @return the robot's heading in degrees, from 180 to 180 + */ + public double getHeading() { + return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1.0 : 1.0); + } + + /** + * Returns the turn rate of the robot. + * + * @return The turn rate of the robot, in degrees per second + */ + public double getTurnRate() { + return m_gyro.getRate() * (kGyroReversed ? -1.0 : 1.0); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java new file mode 100644 index 0000000000..bf33288157 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java @@ -0,0 +1,119 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.swervecontrollercommand.subsystems; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.Spark; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.controller.ProfiledPIDController; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; + +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kDriveEncoderDistancePerPulse; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kMaxModuleAngularAccelerationRadiansPerSecondSquared; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kMaxModuleAngularSpeedRadiansPerSecond; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kPModuleDriveController; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kPModuleTurningController; +import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kTurningEncoderDistancePerPulse; + +public class SwerveModule { + private final Spark m_driveMotor; + private final Spark m_turningMotor; + + private final Encoder m_driveEncoder; + private final Encoder m_turningEncoder; + + private final PIDController m_drivePIDController = + new PIDController(kPModuleDriveController, 0, 0); + + //Using a TrapezoidProfile PIDController to allow for smooth turning + private final ProfiledPIDController m_turningPIDController + = new ProfiledPIDController(kPModuleTurningController, 0, 0, + new TrapezoidProfile.Constraints(kMaxModuleAngularSpeedRadiansPerSecond, + kMaxModuleAngularAccelerationRadiansPerSecondSquared)); + + /** + * Constructs a SwerveModule. + * + * @param driveMotorChannel ID for the drive motor. + * @param turningMotorChannel ID for the turning motor. + */ + public SwerveModule(int driveMotorChannel, + int turningMotorChannel, + int[] driveEncoderPorts, + int[] turningEncoderPorts, + boolean driveEncoderReversed, + boolean turningEncoderReversed) { + + m_driveMotor = new Spark(driveMotorChannel); + m_turningMotor = new Spark(turningMotorChannel); + + this.m_driveEncoder = new Encoder(driveEncoderPorts[0], driveEncoderPorts[1]); + + this.m_turningEncoder = new Encoder(turningEncoderPorts[0], turningEncoderPorts[1]); + + // Set the distance per pulse for the drive encoder. We can simply use the + // distance traveled for one rotation of the wheel divided by the encoder + // resolution. + m_driveEncoder.setDistancePerPulse(kDriveEncoderDistancePerPulse); + + //Set whether drive encoder should be reversed or not + m_driveEncoder.setReverseDirection(driveEncoderReversed); + + // Set the distance (in this case, angle) per pulse for the turning encoder. + // This is the the angle through an entire rotation (2 * wpi::math::pi) + // divided by the encoder resolution. + m_turningEncoder.setDistancePerPulse(kTurningEncoderDistancePerPulse); + + //Set whether turning encoder should be reversed or not + m_turningEncoder.setReverseDirection(turningEncoderReversed); + + // Limit the PID Controller's input range between -pi and pi and set the input + // to be continuous. + m_turningPIDController.enableContinuousInput(-Math.PI, Math.PI); + } + + /** + * Returns the current state of the module. + * + * @return The current state of the module. + */ + public SwerveModuleState getState() { + return new SwerveModuleState(m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.get())); + } + + /** + * Sets the desired state for the module. + * + * @param state Desired state with speed and angle. + */ + public void setDesiredState(SwerveModuleState state) { + // Calculate the drive output from the drive PID controller. + final var driveOutput = m_drivePIDController.calculate( + m_driveEncoder.getRate(), state.speedMetersPerSecond); + + // Calculate the turning motor output from the turning PID controller. + final var turnOutput = m_turningPIDController.calculate( + m_turningEncoder.get(), state.angle.getRadians() + ); + + // Calculate the turning motor output from the turning PID controller. + m_driveMotor.set(driveOutput); + m_turningMotor.set(turnOutput); + } + + /** + * Zeros all the SwerveModule encoders. + */ + + public void resetEncoders() { + m_driveEncoder.reset(); + m_turningEncoder.reset(); + } +}