first commit
This commit is contained in:
Team2890HawkCollective
@@ -0,0 +1,720 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc.robot.subsystems.swervedrive;
|
||||
|
||||
import static edu.wpi.first.units.Units.Meter;
|
||||
|
||||
import com.pathplanner.lib.auto.AutoBuilder;
|
||||
import com.pathplanner.lib.commands.PathPlannerAuto;
|
||||
import com.pathplanner.lib.commands.PathfindingCommand;
|
||||
import com.pathplanner.lib.config.PIDConstants;
|
||||
import com.pathplanner.lib.config.RobotConfig;
|
||||
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
|
||||
import com.pathplanner.lib.path.PathConstraints;
|
||||
import com.pathplanner.lib.path.PathPlannerPath;
|
||||
import com.pathplanner.lib.util.DriveFeedforwards;
|
||||
import com.pathplanner.lib.util.swerve.SwerveSetpoint;
|
||||
import com.pathplanner.lib.util.swerve.SwerveSetpointGenerator;
|
||||
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
|
||||
import edu.wpi.first.math.trajectory.Trajectory;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
|
||||
import frc.robot.Constants;
|
||||
import frc.robot.subsystems.swervedrive.Vision.Cameras;
|
||||
import java.io.File;
|
||||
import java.io.IOException;
|
||||
import java.util.Arrays;
|
||||
import java.util.Optional;
|
||||
import java.util.concurrent.atomic.AtomicReference;
|
||||
import java.util.function.DoubleSupplier;
|
||||
import java.util.function.Supplier;
|
||||
import org.json.simple.parser.ParseException;
|
||||
import org.photonvision.EstimatedRobotPose;
|
||||
import org.photonvision.targeting.PhotonPipelineResult;
|
||||
import swervelib.SwerveController;
|
||||
import swervelib.SwerveDrive;
|
||||
import swervelib.SwerveDriveTest;
|
||||
import swervelib.math.SwerveMath;
|
||||
import swervelib.parser.SwerveControllerConfiguration;
|
||||
import swervelib.parser.SwerveDriveConfiguration;
|
||||
import swervelib.parser.SwerveParser;
|
||||
import swervelib.telemetry.SwerveDriveTelemetry;
|
||||
import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
|
||||
|
||||
public class SwerveSubsystem extends SubsystemBase {
|
||||
/**
|
||||
* Swerve drive object.
|
||||
*/
|
||||
private final SwerveDrive swerveDrive;
|
||||
|
||||
/**
|
||||
* Enable vision odometry updates while driving.
|
||||
*/
|
||||
private final boolean visionDriveTest = false;
|
||||
|
||||
/**
|
||||
* PhotonVision class to keep an accurate odometry.
|
||||
*/
|
||||
private Vision vision;
|
||||
|
||||
/**
|
||||
* Initialize {@link SwerveDrive} with the directory provided.
|
||||
*
|
||||
* @param directory Directory of swerve drive config files.
|
||||
*/
|
||||
public SwerveSubsystem(File directory) {
|
||||
boolean blueAlliance = false;
|
||||
Pose2d startingPose = blueAlliance ? new Pose2d(new Translation2d(Meter.of(1),
|
||||
Meter.of(4)),
|
||||
Rotation2d.fromDegrees(0))
|
||||
: new Pose2d(new Translation2d(Meter.of(16),
|
||||
Meter.of(4)),
|
||||
Rotation2d.fromDegrees(180));
|
||||
// Configure the Telemetry before creating the SwerveDrive to avoid unnecessary
|
||||
// objects being created.
|
||||
SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH;
|
||||
try {
|
||||
swerveDrive = new SwerveParser(directory).createSwerveDrive(Constants.MAX_SPEED, startingPose);
|
||||
// Alternative method if you don't want to supply the conversion factor via JSON
|
||||
// files.
|
||||
// swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed,
|
||||
// angleConversionFactor, driveConversionFactor);
|
||||
} catch (Exception e) {
|
||||
throw new RuntimeException(e);
|
||||
}
|
||||
swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot
|
||||
// via
|
||||
// angle.
|
||||
swerveDrive.setCosineCompensator(false);// !SwerveDriveTelemetry.isSimulation); // Disables cosine compensation
|
||||
// for
|
||||
// simulations since it causes discrepancies not seen in real life.
|
||||
swerveDrive.setAngularVelocityCompensation(true,
|
||||
true,
|
||||
0.1); // Correct for skew that gets worse as angular velocity increases. Start with a
|
||||
// coefficient of 0.1.
|
||||
swerveDrive.setModuleEncoderAutoSynchronize(false,
|
||||
1); // Enable if you want to resynchronize your absolute encoders and motor encoders
|
||||
// periodically when they are not moving.
|
||||
// swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used
|
||||
// over the internal encoder and push the offsets onto it. Throws warning if not
|
||||
// possible
|
||||
if (visionDriveTest) {
|
||||
setupPhotonVision();
|
||||
// Stop the odometry thread if we are using vision that way we can synchronize
|
||||
// updates better.
|
||||
swerveDrive.stopOdometryThread();
|
||||
}
|
||||
setupPathPlanner();
|
||||
}
|
||||
|
||||
/**
|
||||
* Construct the swerve drive.
|
||||
*
|
||||
* @param driveCfg SwerveDriveConfiguration for the swerve.
|
||||
* @param controllerCfg Swerve Controller.
|
||||
*/
|
||||
public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) {
|
||||
swerveDrive = new SwerveDrive(driveCfg,
|
||||
controllerCfg,
|
||||
Constants.MAX_SPEED,
|
||||
new Pose2d(new Translation2d(Meter.of(2), Meter.of(0)),
|
||||
Rotation2d.fromDegrees(0)));
|
||||
}
|
||||
|
||||
/**
|
||||
* Setup the photon vision class.
|
||||
*/
|
||||
public void setupPhotonVision() {
|
||||
vision = new Vision(swerveDrive::getPose, swerveDrive.field);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
|
||||
// When vision is enabled we must manually update odometry in SwerveDrive
|
||||
if (visionDriveTest) {
|
||||
swerveDrive.updateOdometry();
|
||||
vision.updatePoseEstimation(swerveDrive);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void simulationPeriodic() {
|
||||
}
|
||||
|
||||
/**
|
||||
* Setup AutoBuilder for PathPlanner.
|
||||
*/
|
||||
public void setupPathPlanner() {
|
||||
// Load the RobotConfig from the GUI settings. You should probably
|
||||
// store this in your Constants file
|
||||
RobotConfig config;
|
||||
try {
|
||||
config = RobotConfig.fromGUISettings();
|
||||
|
||||
final boolean enableFeedforward = true;
|
||||
// Configure AutoBuilder last
|
||||
AutoBuilder.configure(
|
||||
this::getPose,
|
||||
// Robot pose supplier
|
||||
this::resetOdometry,
|
||||
// Method to reset odometry (will be called if your auto has a starting pose)
|
||||
this::getRobotVelocity,
|
||||
// ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
|
||||
(speedsRobotRelative, moduleFeedForwards) -> {
|
||||
if (enableFeedforward) {
|
||||
swerveDrive.drive(
|
||||
speedsRobotRelative,
|
||||
swerveDrive.kinematics.toSwerveModuleStates(speedsRobotRelative),
|
||||
moduleFeedForwards.linearForces());
|
||||
} else {
|
||||
swerveDrive.setChassisSpeeds(speedsRobotRelative);
|
||||
}
|
||||
},
|
||||
// Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also
|
||||
// optionally outputs individual module feedforwards
|
||||
new PPHolonomicDriveController(
|
||||
// PPHolonomicController is the built in path following controller for holonomic
|
||||
// drive trains
|
||||
new PIDConstants(5.0, 0.0, 0.0),
|
||||
// Translation PID constants
|
||||
new PIDConstants(3.8, 0.0, 0.0)
|
||||
// Rotation PID constants
|
||||
),
|
||||
config,
|
||||
// The robot configuration
|
||||
() -> {
|
||||
// Boolean supplier that controls when the path will be mirrored for the red
|
||||
// alliance
|
||||
// This will flip the path being followed to the red side of the field.
|
||||
// THE ORIGIN WILL REMAIN ON THE BLUE SIDE
|
||||
|
||||
var alliance = DriverStation.getAlliance();
|
||||
if (alliance.isPresent()) {
|
||||
return alliance.get() == DriverStation.Alliance.Red;
|
||||
}
|
||||
return false;
|
||||
},
|
||||
this
|
||||
// Reference to this subsystem to set requirements
|
||||
);
|
||||
|
||||
} catch (Exception e) {
|
||||
// Handle exception as needed
|
||||
e.printStackTrace();
|
||||
}
|
||||
|
||||
// Preload PathPlanner Path finding
|
||||
// IF USING CUSTOM PATHFINDER ADD BEFORE THIS LINE
|
||||
PathfindingCommand.warmupCommand().schedule();
|
||||
}
|
||||
|
||||
/**
|
||||
* Aim the robot at the target returned by PhotonVision.
|
||||
*
|
||||
* @return A {@link Command} which will run the alignment.
|
||||
*/
|
||||
public Command aimAtTarget(Cameras camera) {
|
||||
|
||||
return run(() -> {
|
||||
Optional<PhotonPipelineResult> resultO = camera.getBestResult();
|
||||
if (resultO.isPresent()) {
|
||||
var result = resultO.get();
|
||||
if (result.hasTargets()) {
|
||||
drive(getTargetSpeeds(0,
|
||||
0,
|
||||
Rotation2d.fromDegrees(result.getBestTarget()
|
||||
.getYaw()))); // Not sure if this will work, more math may be required.
|
||||
}
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the path follower with events.
|
||||
*
|
||||
* @param pathName PathPlanner path name.
|
||||
* @return {@link AutoBuilder#followPath(PathPlannerPath)} path command.
|
||||
*/
|
||||
public Command getAutonomousCommand(String pathName) {
|
||||
// Create a path following command using AutoBuilder. This will also trigger
|
||||
// event markers.
|
||||
return new PathPlannerAuto(pathName);
|
||||
}
|
||||
|
||||
/**
|
||||
* Use PathPlanner Path finding to go to a point on the field.
|
||||
*
|
||||
* @param pose Target {@link Pose2d} to go to.
|
||||
* @return PathFinding command
|
||||
*/
|
||||
public Command driveToPose(Pose2d pose) {
|
||||
// Create the constraints to use while pathfinding
|
||||
PathConstraints constraints = new PathConstraints(
|
||||
swerveDrive.getMaximumChassisVelocity(), 4.0,
|
||||
swerveDrive.getMaximumChassisAngularVelocity(), Units.degreesToRadians(720));
|
||||
|
||||
// Since AutoBuilder is configured, we can use it to build pathfinding commands
|
||||
return AutoBuilder.pathfindToPose(
|
||||
pose,
|
||||
constraints,
|
||||
edu.wpi.first.units.Units.MetersPerSecond.of(0) // Goal end velocity in meters/sec
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* Drive with {@link SwerveSetpointGenerator} from 254, implemented by
|
||||
* PathPlanner.
|
||||
*
|
||||
* @param robotRelativeChassisSpeed Robot relative {@link ChassisSpeeds} to
|
||||
* achieve.
|
||||
* @return {@link Command} to run.
|
||||
* @throws IOException If the PathPlanner GUI settings is invalid
|
||||
* @throws ParseException If PathPlanner GUI settings is nonexistent.
|
||||
*/
|
||||
private Command driveWithSetpointGenerator(Supplier<ChassisSpeeds> robotRelativeChassisSpeed)
|
||||
throws IOException, ParseException {
|
||||
SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator(RobotConfig.fromGUISettings(),
|
||||
swerveDrive.getMaximumChassisAngularVelocity());
|
||||
AtomicReference<SwerveSetpoint> prevSetpoint = new AtomicReference<>(
|
||||
new SwerveSetpoint(swerveDrive.getRobotVelocity(),
|
||||
swerveDrive.getStates(),
|
||||
DriveFeedforwards.zeros(swerveDrive.getModules().length)));
|
||||
AtomicReference<Double> previousTime = new AtomicReference<>();
|
||||
|
||||
return startRun(() -> previousTime.set(Timer.getFPGATimestamp()),
|
||||
() -> {
|
||||
double newTime = Timer.getFPGATimestamp();
|
||||
SwerveSetpoint newSetpoint = setpointGenerator.generateSetpoint(prevSetpoint.get(),
|
||||
robotRelativeChassisSpeed.get(),
|
||||
newTime - previousTime.get());
|
||||
swerveDrive.drive(newSetpoint.robotRelativeSpeeds(),
|
||||
newSetpoint.moduleStates(),
|
||||
newSetpoint.feedforwards().linearForces());
|
||||
prevSetpoint.set(newSetpoint);
|
||||
previousTime.set(newTime);
|
||||
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* Drive with 254's Setpoint generator; port written by PathPlanner.
|
||||
*
|
||||
* @param fieldRelativeSpeeds Field-Relative {@link ChassisSpeeds}
|
||||
* @return Command to drive the robot using the setpoint generator.
|
||||
*/
|
||||
public Command driveWithSetpointGeneratorFieldRelative(Supplier<ChassisSpeeds> fieldRelativeSpeeds) {
|
||||
try {
|
||||
return driveWithSetpointGenerator(() -> {
|
||||
return ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds.get(), getHeading());
|
||||
|
||||
});
|
||||
} catch (Exception e) {
|
||||
DriverStation.reportError(e.toString(), true);
|
||||
}
|
||||
return Commands.none();
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Command to characterize the robot drive motors using SysId
|
||||
*
|
||||
* @return SysId Drive Command
|
||||
*/
|
||||
public Command sysIdDriveMotorCommand() {
|
||||
return SwerveDriveTest.generateSysIdCommand(
|
||||
SwerveDriveTest.setDriveSysIdRoutine(
|
||||
new Config(),
|
||||
this, swerveDrive, 12, true),
|
||||
3.0, 5.0, 3.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Command to characterize the robot angle motors using SysId
|
||||
*
|
||||
* @return SysId Angle Command
|
||||
*/
|
||||
public Command sysIdAngleMotorCommand() {
|
||||
return SwerveDriveTest.generateSysIdCommand(
|
||||
SwerveDriveTest.setAngleSysIdRoutine(
|
||||
new Config(),
|
||||
this, swerveDrive),
|
||||
3.0, 5.0, 3.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns a Command that centers the modules of the SwerveDrive subsystem.
|
||||
*
|
||||
* @return a Command that centers the modules of the SwerveDrive subsystem
|
||||
*/
|
||||
public Command centerModulesCommand() {
|
||||
return run(() -> Arrays.asList(swerveDrive.getModules())
|
||||
.forEach(it -> it.setAngle(0.0)));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns a Command that tells the robot to drive forward until the command
|
||||
* ends.
|
||||
*
|
||||
* @return a Command that tells the robot to drive forward until the command
|
||||
* ends
|
||||
*/
|
||||
public Command driveForward() {
|
||||
return run(() -> {
|
||||
swerveDrive.drive(new Translation2d(1, 0), 0, false, false);
|
||||
}).finallyDo(() -> swerveDrive.drive(new Translation2d(0, 0), 0, false, false));
|
||||
}
|
||||
|
||||
/**
|
||||
* Replaces the swerve module feedforward with a new SimpleMotorFeedforward
|
||||
* object.
|
||||
*
|
||||
* @param kS the static gain of the feedforward
|
||||
* @param kV the velocity gain of the feedforward
|
||||
* @param kA the acceleration gain of the feedforward
|
||||
*/
|
||||
public void replaceSwerveModuleFeedforward(double kS, double kV, double kA) {
|
||||
swerveDrive.replaceSwerveModuleFeedforward(new SimpleMotorFeedforward(kS, kV, kA));
|
||||
}
|
||||
|
||||
/**
|
||||
* Command to drive the robot using translative values and heading as angular
|
||||
* velocity.
|
||||
*
|
||||
* @param translationX Translation in the X direction. Cubed for smoother
|
||||
* controls.
|
||||
* @param translationY Translation in the Y direction. Cubed for smoother
|
||||
* controls.
|
||||
* @param angularRotationX Angular velocity of the robot to set. Cubed for
|
||||
* smoother controls.
|
||||
* @return Drive command.
|
||||
*/
|
||||
public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY,
|
||||
DoubleSupplier angularRotationX) {
|
||||
return run(() -> {
|
||||
// Make the robot move
|
||||
swerveDrive.drive(SwerveMath.scaleTranslation(new Translation2d(
|
||||
translationX.getAsDouble() * swerveDrive.getMaximumChassisVelocity(),
|
||||
translationY.getAsDouble() * swerveDrive.getMaximumChassisVelocity()), 0.8),
|
||||
Math.pow(angularRotationX.getAsDouble(), 3) * swerveDrive.getMaximumChassisAngularVelocity(),
|
||||
true,
|
||||
false);
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* Command to drive the robot using translative values and heading as a
|
||||
* setpoint.
|
||||
*
|
||||
* @param translationX Translation in the X direction. Cubed for smoother
|
||||
* controls.
|
||||
* @param translationY Translation in the Y direction. Cubed for smoother
|
||||
* controls.
|
||||
* @param headingX Heading X to calculate angle of the joystick.
|
||||
* @param headingY Heading Y to calculate angle of the joystick.
|
||||
* @return Drive command.
|
||||
*/
|
||||
public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier headingX,
|
||||
DoubleSupplier headingY) {
|
||||
// swerveDrive.setHeadingCorrection(true); // Normally you would want heading
|
||||
// correction for this kind of control.
|
||||
return run(() -> {
|
||||
|
||||
Translation2d scaledInputs = SwerveMath.scaleTranslation(new Translation2d(translationX.getAsDouble(),
|
||||
translationY.getAsDouble()), 0.8);
|
||||
|
||||
// Make the robot move
|
||||
driveFieldOriented(swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), scaledInputs.getY(),
|
||||
headingX.getAsDouble(),
|
||||
headingY.getAsDouble(),
|
||||
swerveDrive.getOdometryHeading().getRadians(),
|
||||
swerveDrive.getMaximumChassisVelocity()));
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* The primary method for controlling the drivebase. Takes a
|
||||
* {@link Translation2d} and a rotation rate, and
|
||||
* calculates and commands module states accordingly. Can use either open-loop
|
||||
* or closed-loop velocity control for
|
||||
* the wheel velocities. Also has field- and robot-relative modes, which affect
|
||||
* how the translation vector is used.
|
||||
*
|
||||
* @param translation {@link Translation2d} that is the commanded linear
|
||||
* velocity of the robot, in meters per
|
||||
* second. In robot-relative mode, positive x is torwards
|
||||
* the bow (front) and positive y is
|
||||
* torwards port (left). In field-relative mode, positive x
|
||||
* is away from the alliance wall
|
||||
* (field North) and positive y is torwards the left wall
|
||||
* when looking through the driver station
|
||||
* glass (field West).
|
||||
* @param rotation Robot angular rate, in radians per second. CCW positive.
|
||||
* Unaffected by field/robot
|
||||
* relativity.
|
||||
* @param fieldRelative Drive mode. True for field-relative, false for
|
||||
* robot-relative.
|
||||
*/
|
||||
public void drive(Translation2d translation, double rotation, boolean fieldRelative) {
|
||||
swerveDrive.drive(translation,
|
||||
rotation,
|
||||
fieldRelative,
|
||||
false); // Open loop is disabled since it shouldn't be used most of the time.
|
||||
}
|
||||
|
||||
/**
|
||||
* Drive the robot given a chassis field oriented velocity.
|
||||
*
|
||||
* @param velocity Velocity according to the field.
|
||||
*/
|
||||
public void driveFieldOriented(ChassisSpeeds velocity) {
|
||||
swerveDrive.driveFieldOriented(velocity);
|
||||
}
|
||||
|
||||
/**
|
||||
* Drive the robot given a chassis field oriented velocity.
|
||||
*
|
||||
* @param velocity Velocity according to the field.
|
||||
*/
|
||||
public Command driveFieldOriented(Supplier<ChassisSpeeds> velocity) {
|
||||
return run(() -> {
|
||||
swerveDrive.driveFieldOriented(velocity.get());
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* Drive according to the chassis robot oriented velocity.
|
||||
*
|
||||
* @param velocity Robot oriented {@link ChassisSpeeds}
|
||||
*/
|
||||
public void drive(ChassisSpeeds velocity) {
|
||||
swerveDrive.drive(velocity);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the swerve drive kinematics object.
|
||||
*
|
||||
* @return {@link SwerveDriveKinematics} of the swerve drive.
|
||||
*/
|
||||
public SwerveDriveKinematics getKinematics() {
|
||||
return swerveDrive.kinematics;
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets odometry to the given pose. Gyro angle and module positions do not
|
||||
* need to be reset when calling this
|
||||
* method. However, if either gyro angle or module position is reset, this must
|
||||
* be called in order for odometry to
|
||||
* keep working.
|
||||
*
|
||||
* @param initialHolonomicPose The pose to set the odometry to
|
||||
*/
|
||||
public void resetOdometry(Pose2d initialHolonomicPose) {
|
||||
swerveDrive.resetOdometry(initialHolonomicPose);
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the current pose (position and rotation) of the robot, as reported by
|
||||
* odometry.
|
||||
*
|
||||
* @return The robot's pose
|
||||
*/
|
||||
public Pose2d getPose() {
|
||||
return swerveDrive.getPose();
|
||||
}
|
||||
|
||||
/**
|
||||
* Set chassis speeds with closed-loop velocity control.
|
||||
*
|
||||
* @param chassisSpeeds Chassis Speeds to set.
|
||||
*/
|
||||
public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) {
|
||||
swerveDrive.setChassisSpeeds(chassisSpeeds);
|
||||
}
|
||||
|
||||
/**
|
||||
* Post the trajectory to the field.
|
||||
*
|
||||
* @param trajectory The trajectory to post.
|
||||
*/
|
||||
public void postTrajectory(Trajectory trajectory) {
|
||||
swerveDrive.postTrajectory(trajectory);
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the gyro angle to zero and resets odometry to the same position, but
|
||||
* facing toward 0.
|
||||
*/
|
||||
public void zeroGyro() {
|
||||
swerveDrive.zeroGyro();
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks if the alliance is red, defaults to false if alliance isn't available.
|
||||
*
|
||||
* @return true if the red alliance, false if blue. Defaults to false if none is
|
||||
* available.
|
||||
*/
|
||||
private boolean isRedAlliance() {
|
||||
var alliance = DriverStation.getAlliance();
|
||||
return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false;
|
||||
}
|
||||
|
||||
/**
|
||||
* This will zero (calibrate) the robot to assume the current position is facing
|
||||
* forward
|
||||
* <p>
|
||||
* If red alliance rotate the robot 180 after the drviebase zero command
|
||||
*/
|
||||
public void zeroGyroWithAlliance() {
|
||||
if (isRedAlliance()) {
|
||||
zeroGyro();
|
||||
// Set the pose 180 degrees
|
||||
resetOdometry(new Pose2d(getPose().getTranslation(), Rotation2d.fromDegrees(180)));
|
||||
} else {
|
||||
zeroGyro();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the drive motors to brake/coast mode.
|
||||
*
|
||||
* @param brake True to set motors to brake mode, false for coast.
|
||||
*/
|
||||
public void setMotorBrake(boolean brake) {
|
||||
swerveDrive.setMotorIdleMode(brake);
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the current yaw angle of the robot, as reported by the swerve pose
|
||||
* estimator in the underlying drivebase.
|
||||
* Note, this is not the raw gyro reading, this may be corrected from calls to
|
||||
* resetOdometry().
|
||||
*
|
||||
* @return The yaw angle
|
||||
*/
|
||||
public Rotation2d getHeading() {
|
||||
return getPose().getRotation();
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the chassis speeds based on controller input of 2 joysticks. One for
|
||||
* speeds in which direction. The other for
|
||||
* the angle of the robot.
|
||||
*
|
||||
* @param xInput X joystick input for the robot to move in the X direction.
|
||||
* @param yInput Y joystick input for the robot to move in the Y direction.
|
||||
* @param headingX X joystick which controls the angle of the robot.
|
||||
* @param headingY Y joystick which controls the angle of the robot.
|
||||
* @return {@link ChassisSpeeds} which can be sent to the Swerve Drive.
|
||||
*/
|
||||
public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headingX, double headingY) {
|
||||
Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput));
|
||||
return swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(),
|
||||
scaledInputs.getY(),
|
||||
headingX,
|
||||
headingY,
|
||||
getHeading().getRadians(),
|
||||
Constants.MAX_SPEED);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the chassis speeds based on controller input of 1 joystick and one angle.
|
||||
* Control the robot at an offset of
|
||||
* 90deg.
|
||||
*
|
||||
* @param xInput X joystick input for the robot to move in the X direction.
|
||||
* @param yInput Y joystick input for the robot to move in the Y direction.
|
||||
* @param angle The angle in as a {@link Rotation2d}.
|
||||
* @return {@link ChassisSpeeds} which can be sent to the Swerve Drive.
|
||||
*/
|
||||
public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d angle) {
|
||||
Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput));
|
||||
|
||||
return swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(),
|
||||
scaledInputs.getY(),
|
||||
angle.getRadians(),
|
||||
getHeading().getRadians(),
|
||||
Constants.MAX_SPEED);
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the current field-relative velocity (x, y and omega) of the robot
|
||||
*
|
||||
* @return A ChassisSpeeds object of the current field-relative velocity
|
||||
*/
|
||||
public ChassisSpeeds getFieldVelocity() {
|
||||
return swerveDrive.getFieldVelocity();
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the current velocity (x, y and omega) of the robot
|
||||
*
|
||||
* @return A {@link ChassisSpeeds} object of the current velocity
|
||||
*/
|
||||
public ChassisSpeeds getRobotVelocity() {
|
||||
return swerveDrive.getRobotVelocity();
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the {@link SwerveController} in the swerve drive.
|
||||
*
|
||||
* @return {@link SwerveController} from the {@link SwerveDrive}.
|
||||
*/
|
||||
public SwerveController getSwerveController() {
|
||||
return swerveDrive.swerveController;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the {@link SwerveDriveConfiguration} object.
|
||||
*
|
||||
* @return The {@link SwerveDriveConfiguration} fpr the current drive.
|
||||
*/
|
||||
public SwerveDriveConfiguration getSwerveDriveConfiguration() {
|
||||
return swerveDrive.swerveDriveConfiguration;
|
||||
}
|
||||
|
||||
/**
|
||||
* Lock the swerve drive to prevent it from moving.
|
||||
*/
|
||||
public void lock() {
|
||||
swerveDrive.lockPose();
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the current pitch angle of the robot, as reported by the imu.
|
||||
*
|
||||
* @return The heading as a {@link Rotation2d} angle
|
||||
*/
|
||||
public Rotation2d getPitch() {
|
||||
return swerveDrive.getPitch();
|
||||
}
|
||||
|
||||
/**
|
||||
* Add a fake vision reading for testing purposes.
|
||||
*/
|
||||
public void addFakeVisionReading() {
|
||||
swerveDrive.addVisionMeasurement(new Pose2d(3, 3, Rotation2d.fromDegrees(65)), Timer.getFPGATimestamp());
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the swerve drive object.
|
||||
*
|
||||
* @return {@link SwerveDrive}
|
||||
*/
|
||||
public SwerveDrive getSwerveDrive() {
|
||||
return swerveDrive;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user