diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a0e9a78..c671420 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -152,10 +152,10 @@ public class RobotContainer { // }, m_robotSwerveDrive)); m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - m_robotSwerveDrive.oneModuleTest( - m_robotMap.rightFront, + m_robotSwerveDrive.driveWithInput( getDeadbandedDriverController().getLeft(), - getDeadbandedDriverController().getRight()); + getDeadbandedDriverController().getRight(), + true); }, m_robotSwerveDrive)); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 7769faf..c6f0224 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -14,6 +14,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.LEDConstants; @@ -29,9 +30,9 @@ import frc4388.utility.RobotGyro; * testing and modularization. */ public class RobotMap { - // private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14); - // public RobotGyro gyro = new RobotGyro(m_pigeon2); - public RobotGyro gyro = null; + private Pigeon2 m_pigeon2 = new Pigeon2(14); + public RobotGyro gyro = new RobotGyro(m_pigeon2); + // public RobotGyro gyro = null; public SwerveModule leftFront; public SwerveModule rightFront; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 06a9cf1..40b30e5 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -68,38 +68,38 @@ public class SwerveDrive extends SubsystemBase { boolean stopped = false; public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - // if (fieldRelative) { + if (fieldRelative) { - // double rot = 0; + 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; - // } + // ! 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); + // SmartDashboard.putBoolean("drift correction", true); - // rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; + 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)); + // 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)); + // 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) { @@ -168,14 +168,14 @@ public class SwerveDrive extends SubsystemBase { * 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 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(); diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index 294dd6c..2f966d7 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -8,11 +8,13 @@ package frc4388.utility; import com.ctre.phoenix.sensors.WPI_Pigeon2; +import com.ctre.phoenix6.hardware.Pigeon2; import com.kauailabs.navx.frc.AHRS; // import edu.wpi.first.wpilibj.GyroBase; import edu.wpi.first.wpilibj.interfaces.Gyro; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; /** * Gyro class that allows for interchangeable use between a pigeon and a navX @@ -20,7 +22,7 @@ import edu.wpi.first.math.MathUtil; public class RobotGyro implements Gyro { private RobotTime m_robotTime = RobotTime.getInstance(); - private WPI_Pigeon2 m_pigeon = null; + private Pigeon2 m_pigeon = null; private AHRS m_navX = null; public boolean m_isGyroAPigeon; //true if pigeon, false if navX @@ -34,7 +36,7 @@ public class RobotGyro implements Gyro { * Creates a Gyro based on a pigeon * @param gyro the gyroscope to use for Gyro */ - public RobotGyro(WPI_Pigeon2 gyro) { + public RobotGyro(Pigeon2 gyro) { m_pigeon = gyro; m_isGyroAPigeon = true; } @@ -54,8 +56,8 @@ public class RobotGyro implements Gyro { public void resetZeroValues() { if (!m_isGyroAPigeon) return; - pitchZero = m_pigeon.getPitch(); - rollZero = m_pigeon.getRoll(); + // pitchZero = m_pigeon.getPitch(); + // rollZero = m_pigeon.getRoll(); } /** @@ -181,10 +183,15 @@ public class RobotGyro implements Gyro { * Roll is within [-90,+90] degrees. */ private double[] getPigeonAngles() { - double[] ypr = new double[3]; - m_pigeon.getYawPitchRoll(ypr); + m_pigeon.getAngle(); + var rotation = m_pigeon.getRotation3d(); - return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)}; + return new double[] {rotation.getX(), (rotation.getY() - pitchZero), (rotation.getZ() - rollZero)}; + } + + @Override + public Rotation2d getRotation2d() { + return m_pigeon.getRotation2d(); } @Override @@ -253,7 +260,7 @@ public class RobotGyro implements Gyro { } } - public WPI_Pigeon2 getPigeon(){ + public Pigeon2 getPigeon(){ return m_pigeon; }