// 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; } }