From aae405408198f0496727b981d115a1b235ad488f Mon Sep 17 00:00:00 2001 From: nathanrsxtn Date: Wed, 4 May 2022 16:30:39 -0600 Subject: [PATCH] Test fix for swerve turning Remove unused commands --- .../java/frc4388/robot/RobotContainer.java | 6 +- .../ButtonBoxCommands/RunMiddleSwitch.java | 57 ------------ .../ButtonBoxCommands/TurretManual.java | 58 ------------ .../commands/ClimberCommands/RunClaw.java | 48 ---------- .../ClimberCommands/RunClimberPath.java | 67 -------------- .../DriveCommands/DriveWithInputForTime.java | 63 ------------- .../DriveCommands/RotateUntilTarget.java | 49 ---------- .../StorageCommands/ManageStorage.java | 68 -------------- .../StorageCommands/SpitOutWrongColor.java | 68 -------------- .../frc4388/robot/subsystems/SwerveDrive.java | 90 +++++++------------ 10 files changed, 32 insertions(+), 542 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/ButtonBoxCommands/RunMiddleSwitch.java delete mode 100644 src/main/java/frc4388/robot/commands/ButtonBoxCommands/TurretManual.java delete mode 100644 src/main/java/frc4388/robot/commands/ClimberCommands/RunClaw.java delete mode 100644 src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java delete mode 100644 src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java delete mode 100644 src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java delete mode 100644 src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java delete mode 100644 src/main/java/frc4388/robot/commands/StorageCommands/SpitOutWrongColor.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 134c8ee..d62126d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -116,11 +116,7 @@ public class RobotContainer { private void configureDefaultCommands() { // Swerve Drive with Input m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - if (driveControlEnabled) { - m_robotSwerveDrive.driveWithInput(getDriverController().getLeft(), getDriverController().getRight(), true); - } else { - m_robotSwerveDrive.driveWithInput(0, 0, 0, 0, false); - } + if (driveControlEnabled) m_robotSwerveDrive.driveWithInput(getDriverController().getLeft(), getDriverController().getRight(), true); }, m_robotSwerveDrive).withName("SwerveDrive DefaultCommand")); // Intake with Triggers diff --git a/src/main/java/frc4388/robot/commands/ButtonBoxCommands/RunMiddleSwitch.java b/src/main/java/frc4388/robot/commands/ButtonBoxCommands/RunMiddleSwitch.java deleted file mode 100644 index 8af8cd4..0000000 --- a/src/main/java/frc4388/robot/commands/ButtonBoxCommands/RunMiddleSwitch.java +++ /dev/null @@ -1,57 +0,0 @@ -package frc4388.robot.commands.ButtonBoxCommands; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -public class RunMiddleSwitch extends CommandBase { - - private static boolean manual = false; - private boolean newManual = false; - private boolean changes = false; - - /** Creates a new RunMiddleSwitch. */ - public RunMiddleSwitch() { - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - - changes = (newManual == manual); - - if (manual) { - printManual(); - } else { - printNormal(); - } - - newManual = manual; - } - - public void printNormal(){ - System.out.println("Normal"); - } - - public void printManual(){ - System.out.println("Manual"); - } - - public static void setManual(boolean set) - { - manual = set; - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return changes; - } -} diff --git a/src/main/java/frc4388/robot/commands/ButtonBoxCommands/TurretManual.java b/src/main/java/frc4388/robot/commands/ButtonBoxCommands/TurretManual.java deleted file mode 100644 index 0ff0be5..0000000 --- a/src/main/java/frc4388/robot/commands/ButtonBoxCommands/TurretManual.java +++ /dev/null @@ -1,58 +0,0 @@ -package frc4388.robot.commands.ButtonBoxCommands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Turret; - -public class TurretManual extends CommandBase { - - // subsystems - private Turret turret; - - // booleans - private static boolean manual = false; - private boolean newManual = false; - private boolean changes = false; - - /** Creates a new TurretManual. */ - public TurretManual(Turret turret) { - // Use addRequirements() here to declare subsystem dependencies. - this.turret = turret; - - addRequirements(this.turret); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - changes = (newManual != manual); - - if (manual) { - System.out.println("Manual Turret"); // TODO: turret manual controls - } else { - System.out.println("Auto Turret"); // TODO: turret auto controls; - } - - newManual = manual; - } - - public static void setManual(boolean set) - { - manual = set; - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - manual = !manual; - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return changes; - } -} diff --git a/src/main/java/frc4388/robot/commands/ClimberCommands/RunClaw.java b/src/main/java/frc4388/robot/commands/ClimberCommands/RunClaw.java deleted file mode 100644 index 8b7c122..0000000 --- a/src/main/java/frc4388/robot/commands/ClimberCommands/RunClaw.java +++ /dev/null @@ -1,48 +0,0 @@ -package frc4388.robot.commands.ClimberCommands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Claws; - -public class RunClaw extends CommandBase { - - // parameters - public Claws m_claws; - public Claws.ClawType clawType; - public boolean open; - - /** - * Creates a new RunClaw, which runs a claw. - * @param sClaws Claws subsystem. - * @param which Which claw to run. - * @param open Whether to open or close the claw. - */ - public RunClaw(Claws sClaws, Claws.ClawType which, boolean open) { - // Use addRequirements() here to declare subsystem dependencies. - m_claws = sClaws; - clawType = which; - this.open = open; - - addRequirements(m_claws); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - // m_claws.runClaw(clawType, open); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - // return m_claws.checkSwitchAndCurrent(clawType); - return false; // TODO: real return - } -} diff --git a/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java b/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java deleted file mode 100644 index cf67ae8..0000000 --- a/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java +++ /dev/null @@ -1,67 +0,0 @@ -package frc4388.robot.commands.ClimberCommands; - -import org.opencv.core.Point; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Claws; -import frc4388.robot.subsystems.Climber; - -public class RunClimberPath extends CommandBase { - Climber climber; - Claws claws; - - Point[] path; - int nextIndex; - - boolean endPath; - - /** Creates a new RunClimberPath. */ - public RunClimberPath(Climber _climber, Claws _claws, Point[] _path) { - path = _path; - endPath = false; - - climber = _climber; - claws = _claws; - addRequirements(climber, claws); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - claws.setOpen(true); - nextIndex = 0; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - // if(!claws.fullyOpen()) - return; - - // Point climberPos = ClimberRewrite.getClimberPosition(climber.getJointAngles()); - - // Vector2D dir = new Vector2D(path[nextIndex]); - // dir.subtract(new Vector2D(climberPos)); - - // if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD && nextIndex < path.length-1) - // nextIndex++; - // else if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD) { - // endPath = true; - // claws.setOpen(false); - // } else if(!endPath) { - // dir = dir.unit(); - // climber.controlWithInput(dir.x, dir.y); - // } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - // return endPath && claws.fullyClosed(); - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java b/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java deleted file mode 100644 index b78fb03..0000000 --- a/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java +++ /dev/null @@ -1,63 +0,0 @@ -package frc4388.robot.commands.DriveCommands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.SwerveDrive; - -public class DriveWithInputForTime extends CommandBase { - - // subsystems - SwerveDrive swerve; - - double[] inputs; - - // timing - long start; - long elapsed; - double duration; - - /** - * DriveWithInput for a specified amount of time. - * @param inputs Inputs used in DriveWithInput (xspeed, yspeed, xrot, yrot). - * @param time Time to DriveWithInput for, in seconds. - */ - public DriveWithInputForTime(SwerveDrive swerve, double[] inputs, double duration) { - // Use addRequirements() here to declare subsystem dependencies. - - if (inputs.length != 4) { - throw new IllegalArgumentException(); - } - - this.swerve = swerve; - this.inputs = inputs; - this.duration = duration * 1000; // ! convert seconds to milliseconds, duh - - addRequirements(this.swerve); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - start = System.currentTimeMillis(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - System.out.println("RUNNING"); - elapsed = System.currentTimeMillis() - start; - this.swerve.driveWithInput(inputs[0], inputs[1], inputs[2], inputs[3], false); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - System.out.println("Duration: " + duration); - System.out.println("Elapsed: " + elapsed); - - return ((double) elapsed >= duration); - } -} diff --git a/src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java b/src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java deleted file mode 100644 index fd337d9..0000000 --- a/src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java +++ /dev/null @@ -1,49 +0,0 @@ -package frc4388.robot.commands.DriveCommands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.SwerveDrive; -import frc4388.robot.subsystems.VisionOdometry; - -public class RotateUntilTarget extends CommandBase { - - // subsystems - SwerveDrive swerve; - VisionOdometry visionOdometry; - - double rotateSpeed; - - /** Creates a new RotateUntilTarget. */ - public RotateUntilTarget(SwerveDrive swerve, VisionOdometry visionOdometry, double rotateSpeed) { - // Use addRequirements() here to declare subsystem dependencies. - - this.swerve = swerve; - this.visionOdometry = visionOdometry; - - this.rotateSpeed = rotateSpeed; - - addRequirements(this.swerve, this.visionOdometry); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - this.visionOdometry.setLEDs(true); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - this.swerve.driveWithInput(0.0, 0.0, rotateSpeed, true); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return this.visionOdometry.getTargetPoints() == null; - // return this.visionOdometry.m_camera.getLatestResult().hasTargets(); - } -} diff --git a/src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java b/src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java deleted file mode 100644 index 8132354..0000000 --- a/src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java +++ /dev/null @@ -1,68 +0,0 @@ -package frc4388.robot.commands.StorageCommands; - -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.BoomBoom; -import frc4388.robot.subsystems.Storage; -import frc4388.robot.subsystems.Turret; - -public class ManageStorage extends CommandBase { - - // subsystems - private Storage storage; - private BoomBoom drum; - private Turret turret; - - private Alliance alliance; - private boolean rightColor; - - /** Creates a new ManageStorage. */ - public ManageStorage(Storage storage, BoomBoom drum, Turret turret) { - // Use addRequirements() here to declare subsystem dependencies. - - this.storage = storage; - this.drum = drum; - this.turret = turret; - - rightColor = true; - - addRequirements(this.storage, this.drum, this.turret); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - checkColor(); - - if (rightColor) { - this.storage.manageStorage(); - } else { - - // * CommandScheduler.getInstance().schedule(new ExampleCommand()); - // * new ExampleCommand().schedule(); - // * new ExampleCommand().execute(); (accompanied by initialize and onFinished) - - new SpitOutWrongColor(this.storage, this.drum, this.turret); // ? is this how you run a command inside a command - } - } - - private void checkColor() { - // this.alliance = this.storage.getColor(); - // rightColor = this.alliance.equals(Robot.alliance); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/StorageCommands/SpitOutWrongColor.java b/src/main/java/frc4388/robot/commands/StorageCommands/SpitOutWrongColor.java deleted file mode 100644 index 60d30f6..0000000 --- a/src/main/java/frc4388/robot/commands/StorageCommands/SpitOutWrongColor.java +++ /dev/null @@ -1,68 +0,0 @@ -package frc4388.robot.commands.StorageCommands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.ShooterConstants; -import frc4388.robot.subsystems.BoomBoom; -import frc4388.robot.subsystems.Storage; -import frc4388.robot.subsystems.Turret; - -public class SpitOutWrongColor extends CommandBase { - - // subsystems - private Storage storage; - private BoomBoom drum; - private Turret turret; - - // time (in milliseconds) - private long initialTime; - private long elapsedTime; - private long threshold; - - private double initialTurret; - private int spitVelocity; - - /** Creates a new SpitOutWrongColor. */ - public SpitOutWrongColor(Storage storage, BoomBoom drum, Turret turret) { - // Use addRequirements() here to declare subsystem dependencies. - - this.storage = storage; - this.drum = drum; - this.turret = turret; - - addRequirements(this.storage, this.drum, this.turret); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - initialTime = System.currentTimeMillis(); - elapsedTime = 0; - threshold = 2000; - - initialTurret = this.turret.getEncoderPosition(); - spitVelocity = 2000; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - elapsedTime = System.currentTimeMillis() - initialTime; - - this.storage.runStorage(0.9); - this.turret.gotoMidpoint(); - this.drum.runDrumShooterVelocityPID(spitVelocity); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - this.storage.runStorage(0.0); - this.turret.runShooterRotatePID(initialTurret * ShooterConstants.TURRET_DEGREES_PER_ROT); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return (elapsedTime >= threshold); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 1fe6d56..d86dd7c 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -43,16 +43,16 @@ public class SwerveDrive extends SubsystemBase { m_backRight = backRight; m_gyro = gyro; - modules = new SwerveModule[] {m_frontLeft, m_frontRight, m_backLeft, m_backRight}; - + modules = new SwerveModule[] { m_frontLeft, m_frontRight, m_backLeft, m_backRight }; + // m_poseEstimator = new SwerveDrivePoseEstimator( - // getRegGyro(),//m_gyro.getRotation2d(), - // new Pose2d(), - // m_kinematics, - // VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)), // TODO: tune - // VecBuilder.fill(Units.degreesToRadians(1)), // TODO: tune - // VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); // TODO: tune - + // getRegGyro(),//m_gyro.getRotation2d(), + // new Pose2d(), + // m_kinematics, + // VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)), // TODO: tune + // VecBuilder.fill(Units.degreesToRadians(1)), // TODO: tune + // VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); // TODO: tune + m_odometry = new SwerveDriveOdometry(SwerveDriveConstants.DRIVE_KINEMATICS, m_gyro.getRotation2d()); m_gyro.reset(); @@ -67,11 +67,10 @@ public class SwerveDrive extends SubsystemBase { /** * Method to drive the robot using joystick info. * @link https://github.com/ZachOrr/MK3-Swerve-Example - * @param speeds[0] Speed of the robot in the x direction (forward). - * @param speeds[1] 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. + * @param speeds[0] Speed of the robot in the x direction (forward). + * @param speeds[1] 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. */ public void driveWithInput(Translation2d speed, double rot, boolean fieldRelative) { ignoreAngles = (speed.getX() == 0) && (speed.getY() == 0) && (rot == 0); @@ -81,41 +80,19 @@ public class SwerveDrive extends SubsystemBase { double xSpeedMetersPerSecond = speed.getX(); double ySpeedMetersPerSecond = speed.getY(); - chassisSpeeds = fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - -rot * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2))) - : new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond, - -rot * SwerveDriveConstants.ROTATION_SPEED * 2); + chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, -rot * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI * 2) + (Math.PI / 2))) : new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond, -rot * SwerveDriveConstants.ROTATION_SPEED * 2); SwerveModuleState[] states = SwerveDriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates(chassisSpeeds); setModuleStates(states); } - public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) { - Translation2d speed = new Translation2d(leftX, leftY); - Translation2d head = new Translation2d(rightX, rightY); - driveWithInput(speed, head, fieldRelative); - } - - // new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180))) public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - ignoreAngles = leftStick.getX() == 0 && leftStick.getY() == 0 && rightStick.getX() == 0 && rightStick.getY() == 0; - leftStick = leftStick.times(leftStick.getNorm() * speedAdjust); - if (Math.abs(rightStick.getX()) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightStick.getY()) > OIConstants.RIGHT_AXIS_DEADBAND) - rotTarget = -(Math.atan2(rightStick.getY(), rightStick.getX()) - Math.PI / 2); - double rot = ignoreAngles ? rotTarget - m_gyro.getRotation2d().getRadians() : 0; - chassisSpeeds = fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(leftStick.getX(), leftStick.getY(), rot * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI * 2) + (Math.PI / 2))) - : new ChassisSpeeds(leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED * 2); - SwerveModuleState[] states = SwerveDriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates(chassisSpeeds); - // if (ignoreAngles) { - // SwerveModuleState[] lockedStates = new SwerveModuleState[states.length]; - // for (int i = 0; i < states.length; i ++) { - // SwerveModuleState state = states[i]; - // lockedStates[i]= new SwerveModuleState(0, state.angle); - // } - // setModuleStates(lockedStates); - // } - setModuleStates(states); + if (fieldRelative) { + if (rightStick.getNorm() > OIConstants.RIGHT_AXIS_DEADBAND) rotTarget = -Math.atan2(rightStick.getY(), rightStick.getX()); + double rotDifference = rotTarget - m_gyro.getRotation2d().getRadians(); + leftStick = leftStick.times(leftStick.getNorm() * speedAdjust); + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(leftStick.getX(), leftStick.getY(), rotDifference * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI * 2) + (Math.PI / 2))); + } else chassisSpeeds = new ChassisSpeeds(leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED * 2); + setModuleStates(SwerveDriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates(chassisSpeeds)); } /** @@ -124,8 +101,7 @@ public class SwerveDrive extends SubsystemBase { * @param desiredStates Array of module states to set. */ public void setModuleStates(SwerveModuleState[] desiredStates) { - SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, - Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC)); + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC)); // int i = 2; { for (int i = 0; i < desiredStates.length; i++) { SwerveModule module = modules[i]; @@ -199,7 +175,7 @@ public class SwerveDrive extends SubsystemBase { * * @return Rotation2d object holding current gyro in radians */ - public Rotation2d getRegGyro() { + public Rotation2d getRegGyro() { // * test chassis regression // double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527; // * new robot regression @@ -218,20 +194,16 @@ public class SwerveDrive extends SubsystemBase { * Updates the field relative position of the robot. */ public void updateOdometry() { - Rotation2d actualDWI = new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2)); //+ (Math.PI/2)); - Rotation2d actual = new Rotation2d(m_gyro.getRotation2d().getRadians()); + Rotation2d actualDWI = new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI * 2)); // + (Math.PI/2)); + Rotation2d actual = new Rotation2d(m_gyro.getRotation2d().getRadians()); SmartDashboard.putNumber("AUTO ACTUAL GYRO", actual.getDegrees()); SmartDashboard.putNumber("AUTO DWI GYRO", actual.getDegrees()); - m_odometry.update( actual,//m_gyro.getRotation2d(),//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()), - modules[0].getState(), - modules[1].getState(), - modules[2].getState(), - modules[3].getState()); + m_odometry.update(actual, // m_gyro.getRotation2d(),//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()), + modules[0].getState(), modules[1].getState(), modules[2].getState(), modules[3].getState()); } - - + /** * Resets pigeon. */ @@ -263,11 +235,11 @@ public class SwerveDrive extends SubsystemBase { } } - public double getCurrent(){ + public double getCurrent() { return m_frontLeft.getCurrent() + m_frontRight.getCurrent() + m_backRight.getCurrent() + m_backLeft.getCurrent(); } - public double getVoltage(){ + public double getVoltage() { return m_frontLeft.getVoltage() + m_frontRight.getVoltage() + m_backRight.getVoltage() + m_backLeft.getVoltage(); } -} \ No newline at end of file +}