mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-22 01:11:42 +00:00
Add holonomic follower examples (#2052)
This commit is contained in:
CTT
@@ -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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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<Pose2d> 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<MecanumDriveWheelSpeeds> m_currentWheelSpeeds;
|
||||
private final Consumer<MecanumDriveMotorVoltages> m_outputDriveVoltages;
|
||||
private final Consumer<MecanumDriveWheelSpeeds> 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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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<Pose2d> pose,
|
||||
SimpleMotorFeedforward feedforward,
|
||||
MecanumDriveKinematics kinematics,
|
||||
|
||||
PIDController xController,
|
||||
PIDController yController,
|
||||
ProfiledPIDController thetaController,
|
||||
|
||||
double maxWheelVelocityMetersPerSecond,
|
||||
|
||||
PIDController frontLeftController,
|
||||
PIDController rearLeftController,
|
||||
PIDController frontRightController,
|
||||
PIDController rearRightController,
|
||||
|
||||
Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
|
||||
|
||||
Consumer<MecanumDriveMotorVoltages> 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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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<Pose2d> pose,
|
||||
MecanumDriveKinematics kinematics,
|
||||
PIDController xController,
|
||||
PIDController yController,
|
||||
ProfiledPIDController thetaController,
|
||||
|
||||
double maxWheelVelocityMetersPerSecond,
|
||||
|
||||
Consumer<MecanumDriveWheelSpeeds> 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());
|
||||
}
|
||||
}
|
||||
@@ -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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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<Pose2d> 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<SwerveModuleState[]> 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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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<Pose2d> pose,
|
||||
SwerveDriveKinematics kinematics,
|
||||
PIDController xController,
|
||||
PIDController yController,
|
||||
ProfiledPIDController thetaController,
|
||||
|
||||
Consumer<SwerveModuleState[]> 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());
|
||||
}
|
||||
}
|
||||
@@ -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<frc::Pose2d()> pose,
|
||||
frc::SimpleMotorFeedforward<units::meters> feedforward,
|
||||
frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
|
||||
frc2::PIDController yController,
|
||||
frc::ProfiledPIDController<units::radians> thetaController,
|
||||
units::meters_per_second_t maxWheelVelocity,
|
||||
std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
|
||||
frc2::PIDController frontLeftController,
|
||||
frc2::PIDController rearLeftController,
|
||||
frc2::PIDController frontRightController,
|
||||
frc2::PIDController rearRightController,
|
||||
std::function<void(units::volt_t, units::volt_t, units::volt_t,
|
||||
units::volt_t)>
|
||||
output,
|
||||
std::initializer_list<Subsystem*> requirements)
|
||||
: m_trajectory(trajectory),
|
||||
m_pose(pose),
|
||||
m_feedforward(feedforward),
|
||||
m_kinematics(kinematics),
|
||||
m_xController(std::make_unique<frc2::PIDController>(xController)),
|
||||
m_yController(std::make_unique<frc2::PIDController>(yController)),
|
||||
m_thetaController(
|
||||
std::make_unique<frc::ProfiledPIDController<units::radians>>(
|
||||
thetaController)),
|
||||
m_maxWheelVelocity(maxWheelVelocity),
|
||||
m_frontLeftController(
|
||||
std::make_unique<frc2::PIDController>(frontLeftController)),
|
||||
m_rearLeftController(
|
||||
std::make_unique<frc2::PIDController>(rearLeftController)),
|
||||
m_frontRightController(
|
||||
std::make_unique<frc2::PIDController>(frontRightController)),
|
||||
m_rearRightController(
|
||||
std::make_unique<frc2::PIDController>(rearRightController)),
|
||||
m_currentWheelSpeeds(currentWheelSpeeds),
|
||||
m_outputVolts(output),
|
||||
m_usePID(true) {
|
||||
AddRequirements(requirements);
|
||||
}
|
||||
|
||||
MecanumControllerCommand::MecanumControllerCommand(
|
||||
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
|
||||
frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
|
||||
frc2::PIDController yController,
|
||||
frc::ProfiledPIDController<units::radians> thetaController,
|
||||
units::meters_per_second_t maxWheelVelocity,
|
||||
std::function<void(units::meters_per_second_t, units::meters_per_second_t,
|
||||
units::meters_per_second_t, units::meters_per_second_t)>
|
||||
output,
|
||||
std::initializer_list<Subsystem*> requirements)
|
||||
: m_trajectory(trajectory),
|
||||
m_pose(pose),
|
||||
m_kinematics(kinematics),
|
||||
m_xController(std::make_unique<frc2::PIDController>(xController)),
|
||||
m_yController(std::make_unique<frc2::PIDController>(yController)),
|
||||
m_thetaController(
|
||||
std::make_unique<frc::ProfiledPIDController<units::radians>>(
|
||||
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<double>()),
|
||||
(m_desiredPose.Translation().X().to<double>())));
|
||||
auto targetYVel = meters_per_second_t(
|
||||
m_yController->Calculate((m_pose().Translation().Y().to<double>()),
|
||||
(m_desiredPose.Translation().Y().to<double>())));
|
||||
|
||||
// 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<double>(),
|
||||
frontLeftSpeedSetpoint.to<double>())) +
|
||||
frontLeftFeedforward;
|
||||
auto rearLeftOutput = volt_t(m_rearLeftController->Calculate(
|
||||
m_currentWheelSpeeds().rearLeft.to<double>(),
|
||||
rearLeftSpeedSetpoint.to<double>())) +
|
||||
rearLeftFeedforward;
|
||||
auto frontRightOutput = volt_t(m_frontRightController->Calculate(
|
||||
m_currentWheelSpeeds().frontRight.to<double>(),
|
||||
frontRightSpeedSetpoint.to<double>())) +
|
||||
frontRightFeedforward;
|
||||
auto rearRightOutput = volt_t(m_rearRightController->Calculate(
|
||||
m_currentWheelSpeeds().rearRight.to<double>(),
|
||||
rearRightSpeedSetpoint.to<double>())) +
|
||||
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());
|
||||
}
|
||||
@@ -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 <cmath>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/controller/ProfiledPIDController.h>
|
||||
#include <frc/controller/SimpleMotorFeedforward.h>
|
||||
#include <frc/geometry/Pose2d.h>
|
||||
#include <frc/kinematics/ChassisSpeeds.h>
|
||||
#include <frc/kinematics/MecanumDriveKinematics.h>
|
||||
#include <frc/kinematics/MecanumDriveWheelSpeeds.h>
|
||||
#include <frc/trajectory/Trajectory.h>
|
||||
#include <units/units.h>
|
||||
|
||||
#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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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<CommandBase, MecanumControllerCommand> {
|
||||
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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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<frc::Pose2d()> pose,
|
||||
frc::SimpleMotorFeedforward<units::meters> feedforward,
|
||||
frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
|
||||
frc2::PIDController yController,
|
||||
frc::ProfiledPIDController<units::radians> thetaController,
|
||||
units::meters_per_second_t maxWheelVelocity,
|
||||
std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
|
||||
frc2::PIDController frontLeftController,
|
||||
frc2::PIDController rearLeftController,
|
||||
frc2::PIDController frontRightController,
|
||||
frc2::PIDController rearRightController,
|
||||
std::function<void(units::volt_t, units::volt_t, units::volt_t,
|
||||
units::volt_t)>
|
||||
output,
|
||||
std::initializer_list<Subsystem*> 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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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<frc::Pose2d()> pose,
|
||||
frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
|
||||
frc2::PIDController yController,
|
||||
frc::ProfiledPIDController<units::radians> thetaController,
|
||||
units::meters_per_second_t maxWheelVelocity,
|
||||
std::function<void(units::meters_per_second_t, units::meters_per_second_t,
|
||||
units::meters_per_second_t,
|
||||
units::meters_per_second_t)>
|
||||
output,
|
||||
std::initializer_list<Subsystem*> requirements);
|
||||
|
||||
void Initialize() override;
|
||||
|
||||
void Execute() override;
|
||||
|
||||
void End(bool interrupted) override;
|
||||
|
||||
bool IsFinished() override;
|
||||
|
||||
private:
|
||||
frc::Trajectory m_trajectory;
|
||||
std::function<frc::Pose2d()> m_pose;
|
||||
frc::SimpleMotorFeedforward<units::meters> m_feedforward;
|
||||
frc::MecanumDriveKinematics m_kinematics;
|
||||
std::unique_ptr<frc2::PIDController> m_xController;
|
||||
std::unique_ptr<frc2::PIDController> m_yController;
|
||||
std::unique_ptr<frc::ProfiledPIDController<units::radians>> m_thetaController;
|
||||
const units::meters_per_second_t m_maxWheelVelocity;
|
||||
std::unique_ptr<frc2::PIDController> m_frontLeftController;
|
||||
std::unique_ptr<frc2::PIDController> m_rearLeftController;
|
||||
std::unique_ptr<frc2::PIDController> m_frontRightController;
|
||||
std::unique_ptr<frc2::PIDController> m_rearRightController;
|
||||
std::function<frc::MecanumDriveWheelSpeeds()> m_currentWheelSpeeds;
|
||||
std::function<void(units::meters_per_second_t, units::meters_per_second_t,
|
||||
units::meters_per_second_t, units::meters_per_second_t)>
|
||||
m_outputVel;
|
||||
std::function<void(units::volt_t, units::volt_t, units::volt_t,
|
||||
units::volt_t)>
|
||||
m_outputVolts;
|
||||
|
||||
bool m_usePID;
|
||||
frc2::Timer m_timer;
|
||||
frc::MecanumDriveWheelSpeeds m_prevSpeeds;
|
||||
units::second_t m_prevTime;
|
||||
frc::Pose2d m_finalPose;
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -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 <cmath>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/controller/ProfiledPIDController.h>
|
||||
#include <frc/geometry/Pose2d.h>
|
||||
#include <frc/kinematics/ChassisSpeeds.h>
|
||||
#include <frc/kinematics/SwerveDriveKinematics.h>
|
||||
#include <frc/kinematics/SwerveModuleState.h>
|
||||
#include <frc/trajectory/Trajectory.h>
|
||||
#include <units/units.h>
|
||||
|
||||
#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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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 <size_t NumModules>
|
||||
class SwerveControllerCommand
|
||||
: public CommandHelper<CommandBase, SwerveControllerCommand<NumModules>> {
|
||||
using voltsecondspermeter =
|
||||
units::compound_unit<units::voltage::volt, units::second,
|
||||
units::inverse<units::meter>>;
|
||||
using voltsecondssquaredpermeter =
|
||||
units::compound_unit<units::voltage::volt, units::squared<units::second>,
|
||||
units::inverse<units::meter>>;
|
||||
|
||||
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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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<frc::Pose2d()> pose,
|
||||
frc::SwerveDriveKinematics<NumModules> kinematics,
|
||||
frc2::PIDController xController, frc2::PIDController yController,
|
||||
frc::ProfiledPIDController<units::radians> thetaController,
|
||||
std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
|
||||
output,
|
||||
std::initializer_list<Subsystem*> requirements);
|
||||
|
||||
void Initialize() override;
|
||||
|
||||
void Execute() override;
|
||||
|
||||
void End(bool interrupted) override;
|
||||
|
||||
bool IsFinished() override;
|
||||
|
||||
private:
|
||||
frc::Trajectory m_trajectory;
|
||||
std::function<frc::Pose2d()> m_pose;
|
||||
frc::SwerveDriveKinematics<NumModules> m_kinematics;
|
||||
std::unique_ptr<frc2::PIDController> m_xController;
|
||||
std::unique_ptr<frc2::PIDController> m_yController;
|
||||
std::unique_ptr<frc::ProfiledPIDController<units::radians>> m_thetaController;
|
||||
std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
|
||||
m_outputStates;
|
||||
|
||||
frc2::Timer m_timer;
|
||||
units::second_t m_prevTime;
|
||||
frc::Pose2d m_finalPose;
|
||||
};
|
||||
} // namespace frc2
|
||||
|
||||
#include "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 <memory>
|
||||
|
||||
namespace frc2 {
|
||||
|
||||
template <size_t NumModules>
|
||||
SwerveControllerCommand<NumModules>::SwerveControllerCommand(
|
||||
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
|
||||
frc::SwerveDriveKinematics<NumModules> kinematics,
|
||||
frc2::PIDController xController, frc2::PIDController yController,
|
||||
frc::ProfiledPIDController<units::radians> thetaController,
|
||||
std::function<void(std::array<frc::SwerveModuleState, NumModules>)> output,
|
||||
std::initializer_list<Subsystem*> requirements)
|
||||
: m_trajectory(trajectory),
|
||||
m_pose(pose),
|
||||
m_kinematics(kinematics),
|
||||
m_xController(std::make_unique<frc2::PIDController>(xController)),
|
||||
m_yController(std::make_unique<frc2::PIDController>(yController)),
|
||||
m_thetaController(
|
||||
std::make_unique<frc::ProfiledPIDController<units::radians>>(
|
||||
thetaController)),
|
||||
m_outputStates(output) {
|
||||
this->AddRequirements(requirements);
|
||||
}
|
||||
|
||||
template <size_t NumModules>
|
||||
void SwerveControllerCommand<NumModules>::Initialize() {
|
||||
m_finalPose = m_trajectory.Sample(m_trajectory.TotalTime()).pose;
|
||||
|
||||
m_timer.Reset();
|
||||
m_timer.Start();
|
||||
}
|
||||
|
||||
template <size_t NumModules>
|
||||
void SwerveControllerCommand<NumModules>::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<double>()),
|
||||
(m_desiredPose.Translation().X().template to<double>())));
|
||||
auto targetYVel = units::meters_per_second_t(m_yController->Calculate(
|
||||
(m_pose().Translation().Y().template to<double>()),
|
||||
(m_desiredPose.Translation().Y().template to<double>())));
|
||||
|
||||
// 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 <size_t NumModules>
|
||||
void SwerveControllerCommand<NumModules>::End(bool interrupted) {
|
||||
m_timer.Stop();
|
||||
}
|
||||
|
||||
template <size_t NumModules>
|
||||
bool SwerveControllerCommand<NumModules>::IsFinished() {
|
||||
return m_timer.HasPeriodPassed(m_trajectory.TotalTime());
|
||||
}
|
||||
|
||||
} // namespace frc2
|
||||
@@ -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<Pose2d>();
|
||||
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)
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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<Pose2d>();
|
||||
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)
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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 <frc2/Timer.h>
|
||||
#include <frc2/command/MecanumControllerCommand.h>
|
||||
#include <frc2/command/Subsystem.h>
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/controller/ProfiledPIDController.h>
|
||||
#include <frc/geometry/Rotation2d.h>
|
||||
#include <frc/geometry/Translation2d.h>
|
||||
#include <frc/kinematics/MecanumDriveKinematics.h>
|
||||
#include <frc/kinematics/MecanumDriveOdometry.h>
|
||||
#include <frc/trajectory/TrajectoryGenerator.h>
|
||||
#include <wpi/math>
|
||||
|
||||
#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<units::radians,
|
||||
units::inverse<units::squared<units::second>>>;
|
||||
|
||||
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<units::radians> m_rotController{
|
||||
1, 0, 0,
|
||||
frc::TrapezoidProfile<units::radians>::Constraints{
|
||||
9_rad_per_s, units::unit_t<radians_per_second_squared_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);
|
||||
}
|
||||
@@ -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 <frc2/Timer.h>
|
||||
#include <frc2/command/Subsystem.h>
|
||||
#include <frc2/command/SwerveControllerCommand.h>
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/controller/ProfiledPIDController.h>
|
||||
#include <frc/geometry/Rotation2d.h>
|
||||
#include <frc/geometry/Translation2d.h>
|
||||
#include <frc/kinematics/SwerveDriveKinematics.h>
|
||||
#include <frc/kinematics/SwerveDriveOdometry.h>
|
||||
#include <frc/kinematics/SwerveModuleState.h>
|
||||
#include <frc/trajectory/TrajectoryGenerator.h>
|
||||
#include <wpi/math>
|
||||
|
||||
#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<units::radians,
|
||||
units::inverse<units::squared<units::second>>>;
|
||||
|
||||
protected:
|
||||
frc2::Timer m_timer;
|
||||
frc::Rotation2d m_angle{0_rad};
|
||||
|
||||
std::array<frc::SwerveModuleState, 4> m_moduleStates{
|
||||
frc::SwerveModuleState{}, frc::SwerveModuleState{},
|
||||
frc::SwerveModuleState{}, frc::SwerveModuleState{}};
|
||||
|
||||
frc::ProfiledPIDController<units::radians> m_rotController{
|
||||
1, 0, 0,
|
||||
frc::TrapezoidProfile<units::radians>::Constraints{
|
||||
9_rad_per_s, units::unit_t<radians_per_second_squared_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<frc::SwerveModuleState, 4> 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);
|
||||
}
|
||||
@@ -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 {};
|
||||
}
|
||||
@@ -121,6 +121,22 @@ class SwerveDriveKinematics {
|
||||
template <typename... ModuleStates>
|
||||
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<SwerveModuleState, NumModules> moduleStates);
|
||||
|
||||
/**
|
||||
* Normalizes the wheel speeds using some max attainable speed. Sometimes,
|
||||
* after inverse kinematics, the requested speed from a/several modules may be
|
||||
|
||||
@@ -63,6 +63,13 @@ ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
|
||||
"locations provided in constructor.");
|
||||
|
||||
std::array<SwerveModuleState, NumModules> moduleStates{wheelStates...};
|
||||
|
||||
return this->ToChassisSpeeds(moduleStates);
|
||||
}
|
||||
|
||||
template <size_t NumModules>
|
||||
ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
|
||||
std::array<SwerveModuleState, NumModules> moduleStates) {
|
||||
Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix;
|
||||
|
||||
for (size_t i = 0; i < NumModules; i++) {
|
||||
|
||||
@@ -14,7 +14,11 @@
|
||||
#include <units/units.h>
|
||||
|
||||
#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 <size_t NumModules>
|
||||
void SetKinematics(SwerveDriveKinematics<NumModules>& kinematics) {
|
||||
AddConstraint(SwerveDriveKinematicsConstraint(kinematics, m_maxVelocity));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the starting velocity of the trajectory.
|
||||
* @return The starting velocity of the trajectory.
|
||||
|
||||
@@ -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 <cmath>
|
||||
|
||||
#include <units/units.h>
|
||||
|
||||
#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
|
||||
@@ -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 <cmath>
|
||||
|
||||
#include <units/units.h>
|
||||
|
||||
#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 <size_t NumModules>
|
||||
class SwerveDriveKinematicsConstraint : public TrajectoryConstraint {
|
||||
public:
|
||||
SwerveDriveKinematicsConstraint(
|
||||
frc::SwerveDriveKinematics<NumModules> 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<NumModules> m_kinematics;
|
||||
units::meters_per_second_t m_maxSpeed;
|
||||
};
|
||||
} // namespace frc
|
||||
|
||||
#include "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 <size_t NumModules>
|
||||
SwerveDriveKinematicsConstraint<NumModules>::SwerveDriveKinematicsConstraint(
|
||||
frc::SwerveDriveKinematics<NumModules> kinematics,
|
||||
units::meters_per_second_t maxSpeed)
|
||||
: m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
|
||||
|
||||
template <size_t NumModules>
|
||||
units::meters_per_second_t
|
||||
SwerveDriveKinematicsConstraint<NumModules>::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 <size_t NumModules>
|
||||
TrajectoryConstraint::MinMax
|
||||
SwerveDriveKinematicsConstraint<NumModules>::MinMaxAcceleration(
|
||||
const Pose2d& pose, curvature_t curvature,
|
||||
units::meters_per_second_t speed) {
|
||||
return {};
|
||||
}
|
||||
|
||||
} // namespace frc
|
||||
@@ -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 <frc/smartdashboard/SmartDashboard.h>
|
||||
#include <frc2/command/CommandScheduler.h>
|
||||
|
||||
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.
|
||||
*
|
||||
* <p> 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<Robot>(); }
|
||||
#endif
|
||||
@@ -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 <frc/controller/PIDController.h>
|
||||
#include <frc/geometry/Translation2d.h>
|
||||
#include <frc/shuffleboard/Shuffleboard.h>
|
||||
#include <frc/trajectory/Trajectory.h>
|
||||
#include <frc/trajectory/TrajectoryGenerator.h>
|
||||
#include <frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h>
|
||||
#include <frc2/command/InstantCommand.h>
|
||||
#include <frc2/command/MecanumControllerCommand.h>
|
||||
#include <frc2/command/SequentialCommandGroup.h>
|
||||
#include <frc2/command/button/JoystickButton.h>
|
||||
|
||||
#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<units::meters>(ks, kv, ka),
|
||||
DriveConstants::kDriveKinematics,
|
||||
|
||||
frc2::PIDController(AutoConstants::kPXController, 0, 0),
|
||||
frc2::PIDController(AutoConstants::kPYController, 0, 0),
|
||||
frc::ProfiledPIDController<units::radians>(
|
||||
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); }, {}));
|
||||
}
|
||||
@@ -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 <frc/geometry/Rotation2d.h>
|
||||
#include <units/units.h>
|
||||
|
||||
#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())));
|
||||
}
|
||||
@@ -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 <frc/geometry/Translation2d.h>
|
||||
#include <frc/kinematics/MecanumDriveKinematics.h>
|
||||
#include <frc/trajectory/TrapezoidProfile.h>
|
||||
#include <units/units.h>
|
||||
#include <wpi/math>
|
||||
|
||||
#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<double>(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<units::radians,
|
||||
units::inverse<units::squared<units::second>>>;
|
||||
|
||||
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<radians_per_second_squared_t>(3);
|
||||
|
||||
constexpr double kPXController = 0.5;
|
||||
constexpr double kPYController = 0.5;
|
||||
constexpr double kPThetaController = 0.5;
|
||||
|
||||
constexpr frc::TrapezoidProfile<units::radians>::Constraints
|
||||
kThetaControllerConstraints{kMaxAngularSpeed, kMaxAngularAcceleration};
|
||||
|
||||
} // namespace AutoConstants
|
||||
|
||||
namespace OIConstants {
|
||||
constexpr int kDriverControllerPort = 1;
|
||||
} // namespace OIConstants
|
||||
@@ -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 <frc/TimedRobot.h>
|
||||
#include <frc2/command/Command.h>
|
||||
|
||||
#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;
|
||||
};
|
||||
@@ -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 <frc/XboxController.h>
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/controller/ProfiledPIDController.h>
|
||||
#include <frc/smartdashboard/SendableChooser.h>
|
||||
#include <frc2/command/Command.h>
|
||||
#include <frc2/command/InstantCommand.h>
|
||||
#include <frc2/command/PIDCommand.h>
|
||||
#include <frc2/command/ParallelRaceGroup.h>
|
||||
#include <frc2/command/RunCommand.h>
|
||||
|
||||
#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<frc2::Command*> m_chooser;
|
||||
|
||||
void ConfigureButtonBindings();
|
||||
};
|
||||
@@ -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 <frc/ADXRS450_Gyro.h>
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/PWMVictorSPX.h>
|
||||
#include <frc/drive/MecanumDrive.h>
|
||||
#include <frc/geometry/Pose2d.h>
|
||||
#include <frc/geometry/Rotation2d.h>
|
||||
#include <frc/interfaces/Gyro.h>
|
||||
#include <frc/kinematics/MecanumDriveOdometry.h>
|
||||
#include <frc/kinematics/MecanumDriveWheelSpeeds.h>
|
||||
#include <frc2/command/SubsystemBase.h>
|
||||
|
||||
#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;
|
||||
};
|
||||
@@ -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 <frc/smartdashboard/SmartDashboard.h>
|
||||
#include <frc2/command/CommandScheduler.h>
|
||||
|
||||
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.
|
||||
*
|
||||
* <p> 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<Robot>(); }
|
||||
#endif
|
||||
@@ -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 <frc/controller/PIDController.h>
|
||||
#include <frc/geometry/Translation2d.h>
|
||||
#include <frc/shuffleboard/Shuffleboard.h>
|
||||
#include <frc/trajectory/Trajectory.h>
|
||||
#include <frc/trajectory/TrajectoryGenerator.h>
|
||||
#include <frc2/command/InstantCommand.h>
|
||||
#include <frc2/command/SequentialCommandGroup.h>
|
||||
#include <frc2/command/SwerveControllerCommand.h>
|
||||
#include <frc2/command/button/JoystickButton.h>
|
||||
#include <units/units.h>
|
||||
|
||||
#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<units::radians>(
|
||||
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);
|
||||
},
|
||||
{}));
|
||||
}
|
||||
@@ -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 <frc/geometry/Rotation2d.h>
|
||||
#include <units/units.h>
|
||||
|
||||
#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<frc::SwerveModuleState, 4> 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())));
|
||||
}
|
||||
@@ -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 <frc/geometry/Rotation2d.h>
|
||||
#include <wpi/math>
|
||||
|
||||
#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<double>());
|
||||
|
||||
// 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();
|
||||
}
|
||||
@@ -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 <frc/geometry/Translation2d.h>
|
||||
#include <frc/kinematics/SwerveDriveKinematics.h>
|
||||
#include <frc/trajectory/TrapezoidProfile.h>
|
||||
#include <units/units.h>
|
||||
#include <wpi/math>
|
||||
|
||||
#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<double>(kEncoderCPR);
|
||||
|
||||
constexpr double kTurningEncoderDistancePerPulse =
|
||||
// Assumes the encoders are directly mounted on the wheel shafts
|
||||
(wpi::math::pi * 2) / static_cast<double>(kEncoderCPR);
|
||||
|
||||
constexpr double kPModuleTurningController = 1;
|
||||
constexpr double kPModuleDriveController = 1;
|
||||
} // namespace ModuleConstants
|
||||
|
||||
namespace AutoConstants {
|
||||
using radians_per_second_squared_t =
|
||||
units::compound_unit<units::radians,
|
||||
units::inverse<units::squared<units::second>>>;
|
||||
|
||||
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<radians_per_second_squared_t>(3.142);
|
||||
|
||||
constexpr double kPXController = 0.5;
|
||||
constexpr double kPYController = 0.5;
|
||||
constexpr double kPThetaController = 0.5;
|
||||
|
||||
//
|
||||
|
||||
constexpr frc::TrapezoidProfile<units::radians>::Constraints
|
||||
kThetaControllerConstraints{kMaxAngularSpeed, kMaxAngularAcceleration};
|
||||
|
||||
} // namespace AutoConstants
|
||||
|
||||
namespace OIConstants {
|
||||
constexpr int kDriverControllerPort = 1;
|
||||
} // namespace OIConstants
|
||||
@@ -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 <frc/TimedRobot.h>
|
||||
#include <frc2/command/Command.h>
|
||||
|
||||
#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;
|
||||
};
|
||||
@@ -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 <frc/XboxController.h>
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/controller/ProfiledPIDController.h>
|
||||
#include <frc/smartdashboard/SendableChooser.h>
|
||||
#include <frc2/command/Command.h>
|
||||
#include <frc2/command/InstantCommand.h>
|
||||
#include <frc2/command/PIDCommand.h>
|
||||
#include <frc2/command/ParallelRaceGroup.h>
|
||||
#include <frc2/command/RunCommand.h>
|
||||
|
||||
#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<frc2::Command*> m_chooser;
|
||||
|
||||
void ConfigureButtonBindings();
|
||||
};
|
||||
@@ -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 <frc/ADXRS450_Gyro.h>
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/PWMVictorSPX.h>
|
||||
#include <frc/drive/MecanumDrive.h>
|
||||
#include <frc/geometry/Pose2d.h>
|
||||
#include <frc/geometry/Rotation2d.h>
|
||||
#include <frc/interfaces/Gyro.h>
|
||||
#include <frc/kinematics/ChassisSpeeds.h>
|
||||
#include <frc/kinematics/SwerveDriveKinematics.h>
|
||||
#include <frc/kinematics/SwerveDriveOdometry.h>
|
||||
#include <frc2/command/SubsystemBase.h>
|
||||
|
||||
#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<frc::SwerveModuleState, 4> 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;
|
||||
};
|
||||
@@ -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 <frc/Encoder.h>
|
||||
#include <frc/Spark.h>
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/controller/ProfiledPIDController.h>
|
||||
#include <frc/geometry/Rotation2d.h>
|
||||
#include <frc/kinematics/SwerveModuleState.h>
|
||||
#include <frc/trajectory/TrapezoidProfile.h>
|
||||
#include <wpi/math>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
class SwerveModule {
|
||||
using radians_per_second_squared_t =
|
||||
units::compound_unit<units::radians,
|
||||
units::inverse<units::squared<units::second>>>;
|
||||
|
||||
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<radians_per_second_squared_t>
|
||||
kModuleMaxAngularAcceleration =
|
||||
units::unit_t<radians_per_second_squared_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<units::radians> m_turningPIDController{
|
||||
ModuleConstants::kPModuleTurningController,
|
||||
0.0,
|
||||
0.0,
|
||||
{kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration}};
|
||||
};
|
||||
@@ -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
|
||||
}
|
||||
]
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
]
|
||||
|
||||
@@ -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.
|
||||
*
|
||||
* <p>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);
|
||||
|
||||
}
|
||||
}
|
||||
@@ -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.
|
||||
*
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
public static void main(String... args) {
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
}
|
||||
@@ -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.
|
||||
*
|
||||
* <p>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() {
|
||||
}
|
||||
}
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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.
|
||||
*
|
||||
* <p>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);
|
||||
|
||||
}
|
||||
}
|
||||
@@ -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.
|
||||
*
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
public static void main(String... args) {
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
}
|
||||
@@ -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.
|
||||
*
|
||||
* <p>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() {
|
||||
}
|
||||
}
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user