mirror of
https://github.com/Team4388/2024AcrossTheRidgebotiverse.git
synced 2026-06-09 00:38:02 -06:00
Daniel Carta
251 lines
9.0 KiB
Java
251 lines
9.0 KiB
Java
// 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 frc4388.robot.subsystems;
|
|
|
|
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.kinematics.SwerveModuleState;
|
|
import edu.wpi.first.math.util.Units;
|
|
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|
import frc4388.robot.Constants.SwerveDriveConstants;
|
|
import frc4388.utility.RobotGyro;
|
|
|
|
public class SwerveDrive extends SubsystemBase {
|
|
|
|
private SwerveModule leftFront;
|
|
private SwerveModule rightFront;
|
|
private SwerveModule leftBack;
|
|
private SwerveModule rightBack;
|
|
|
|
private SwerveModule[] modules;
|
|
|
|
private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
|
|
private Translation2d rightFrontLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
|
|
private Translation2d leftBackLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
|
|
private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
|
|
|
|
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);
|
|
|
|
private RobotGyro gyro;
|
|
|
|
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; // * slow by default
|
|
public double rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
|
|
public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
|
|
|
|
public double rotTarget = 0.0;
|
|
public Rotation2d orientRotTarget = new Rotation2d();
|
|
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
|
|
|
/** Creates a new SwerveDrive. */
|
|
public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) {
|
|
this.leftFront = leftFront;
|
|
this.rightFront = rightFront;
|
|
this.leftBack = leftBack;
|
|
this.rightBack = rightBack;
|
|
|
|
this.gyro = gyro;
|
|
|
|
this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
|
|
}
|
|
|
|
boolean stopped = false;
|
|
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
|
if (fieldRelative) {
|
|
|
|
double rot = 0;
|
|
|
|
// ! drift correction
|
|
if (rightStick.getNorm() > 0.05) {
|
|
rotTarget = gyro.getAngle();
|
|
rot = rightStick.getX();
|
|
// SmartDashboard.putBoolean("drift correction", false);
|
|
stopped = false;
|
|
} else if(leftStick.getNorm() > 0.05) {
|
|
if (!stopped) {
|
|
stopModules();
|
|
stopped = true;
|
|
}
|
|
|
|
// SmartDashboard.putBoolean("drift correction", true);
|
|
|
|
rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
|
|
|
|
}
|
|
|
|
// Use the left joystick to set speed. Apply a cubic curve and the set max speed.
|
|
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
|
|
// Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
|
|
|
|
// Convert field-relative speeds to robot-relative speeds.
|
|
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * rotSpeedAdjust, gyro.getRotation2d());//.times(-1));
|
|
} else { // Create robot-relative speeds.
|
|
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
|
|
}
|
|
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
|
|
}
|
|
|
|
public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
|
if (fieldRelative) {
|
|
|
|
// ! drift correction
|
|
if (rightStick.getNorm() > 0.05) {
|
|
rotTarget = gyro.getAngle();
|
|
// SmartDashboard.putBoolean("drift correction", false);
|
|
stopped = false;
|
|
} else if(leftStick.getNorm() > 0.05) {
|
|
if (!stopped) {
|
|
stopModules();
|
|
stopped = true;
|
|
}
|
|
|
|
// SmartDashboard.putBoolean("drift correction", true);
|
|
|
|
|
|
}
|
|
|
|
// Use the left joystick to set speed. Apply a cubic curve and the set max speed.
|
|
Translation2d speed = leftStick.times(leftStick.getNorm() * autoSpeedAdjust);
|
|
// Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
|
|
|
|
// Convert field-relative speeds to robot-relative speeds.
|
|
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED, gyro.getRotation2d());//.times(-1));
|
|
} else { // Create robot-relative speeds.
|
|
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED);
|
|
}
|
|
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
|
|
}
|
|
|
|
public void driveWithInputOrientation(Translation2d leftStick, double rightX, double rightY, boolean fieldRelative) {
|
|
|
|
Translation2d rightStick = new Translation2d(-rightX, rightY);
|
|
|
|
if(fieldRelative) {
|
|
if(rightStick.getNorm() > 0.5) {
|
|
orientRotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1));
|
|
|
|
Rotation2d tmp = orientRotTarget.minus(gyro.getRotation2d().minus(new Rotation2d(Math.PI)).interpolate(orientRotTarget, 0.5));
|
|
double min = tmp.getDegrees();
|
|
min = Math.max(Math.abs(min), 2);
|
|
if(tmp.getDegrees() < 0)
|
|
min*=-1;
|
|
tmp = new Rotation2d(min * Math.PI / 180);
|
|
}
|
|
|
|
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
|
|
|
|
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d()).times(1);
|
|
} else { // Create robot-relative speeds.
|
|
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
|
|
}
|
|
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
|
|
}
|
|
|
|
/**
|
|
* Set each module of the swerve drive to the corresponding desired state.
|
|
* @param desiredStates Array of module states to set.
|
|
*/
|
|
public void setModuleStates(SwerveModuleState[] desiredStates) {
|
|
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
|
|
for (int i = 0; i < desiredStates.length; i++) {
|
|
SwerveModule module = modules[i];
|
|
SwerveModuleState state = desiredStates[i];
|
|
module.setDesiredState(state);
|
|
}
|
|
}
|
|
|
|
public boolean rotateToTarget(double angle) {
|
|
double currentAngle = getGyroAngle();
|
|
double error = angle - currentAngle;
|
|
|
|
driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true);
|
|
|
|
if (Math.abs(angle - getGyroAngle()) < 5.0) {
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
public double getGyroAngle() {
|
|
return gyro.getAngle();
|
|
}
|
|
|
|
public void add180() {
|
|
gyro.reset(gyro.getAngle()+180);
|
|
rotTarget = gyro.getAngle();
|
|
|
|
}
|
|
|
|
public void resetGyro() {
|
|
gyro.reset();
|
|
rotTarget = gyro.getAngle();
|
|
}
|
|
|
|
public void resetGyroFlip() {
|
|
gyro.resetFlip();
|
|
rotTarget = gyro.getAngle();
|
|
}
|
|
|
|
public void resetGyroRightBlue() {
|
|
gyro.resetRightSideBlue();
|
|
rotTarget = gyro.getAngle();
|
|
}
|
|
|
|
public void resetGyroRightAmp() {
|
|
gyro.resetAmpSide();
|
|
rotTarget = gyro.getAngle();
|
|
}
|
|
|
|
public void stopModules() {
|
|
for (SwerveModule module : this.modules) {
|
|
module.stop();
|
|
}
|
|
}
|
|
|
|
public SwerveDriveKinematics getKinematics() {
|
|
return this.kinematics;
|
|
}
|
|
|
|
public boolean getSpeedState() {
|
|
|
|
return false;
|
|
}
|
|
|
|
@Override
|
|
public void periodic() {
|
|
// This method will be called once per scheduler run\
|
|
// SmartDashboard.putNumber("Gyro", getGyroAngle());
|
|
}
|
|
|
|
private int GEAR = SwerveDriveConstants.SPEEDS.length /2;
|
|
public void shiftDown() {
|
|
if(GEAR > 0){
|
|
GEAR--;
|
|
this.speedAdjust = SwerveDriveConstants.SPEEDS[GEAR];
|
|
}
|
|
}
|
|
|
|
public void shiftUp() {
|
|
if(GEAR < SwerveDriveConstants.SPEEDS.length){
|
|
GEAR++;
|
|
this.speedAdjust = SwerveDriveConstants.SPEEDS[GEAR];
|
|
}
|
|
}
|
|
|
|
public void shiftUpRot() {
|
|
rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED;
|
|
}
|
|
|
|
public void shiftDownRot() {
|
|
rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
|
|
}
|
|
|
|
|
|
|
|
}
|