From 27602056df23ae727422965677cb78fca2772ca0 Mon Sep 17 00:00:00 2001 From: Connorppeach Date: Tue, 14 Jan 2025 17:26:32 -0700 Subject: [PATCH 1/7] add partial pathplanner support --- .../java/frc4388/robot/RobotContainer.java | 39 ++- src/main/java/frc4388/robot/SwerveDrive.java | 330 ++++++++++++++++++ 2 files changed, 351 insertions(+), 18 deletions(-) create mode 100644 src/main/java/frc4388/robot/SwerveDrive.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6f8887c..3f49fb8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -14,12 +14,10 @@ import java.util.ArrayList; import java.util.List; import edu.wpi.first.cameraserver.CameraServer; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.GenericHID; import frc4388.utility.controller.XboxController; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.robot.Constants.OIConstants; -import frc4388.robot.Constants.SwerveDriveConstants; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -27,14 +25,17 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.Commands; // Autos import frc4388.utility.controller.VirtualController; +import frc4388.robot.commands.GotoPositionCommand; import frc4388.robot.commands.Swerve.neoJoystickPlayback; import frc4388.robot.commands.Swerve.neoJoystickRecorder; - +import com.pathplanner.lib.commands.PathPlannerAuto; // Subsystems // import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.Vision; import frc4388.robot.subsystems.SwerveDrive; // Utilites @@ -55,8 +56,11 @@ public class RobotContainer { /* Subsystems */ // private final LED m_robotLED = new LED(); + public final Vision m_vision = new Vision(m_robotMap.camera); + + public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); + // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); - public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); @@ -95,7 +99,7 @@ public class RobotContainer { m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), getDeadbandedDriverController().getRight(), - false); + true); }, m_robotSwerveDrive) .withName("SwerveDrive DefaultCommand")); m_robotSwerveDrive.setToSlow(); @@ -146,18 +150,7 @@ public class RobotContainer { DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); - - // @ /* Trim Test Buttons */ - - new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.stepUp())); - - new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.stepDown())); - - new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.load())); - + // ! /* Speed */ new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); @@ -222,9 +215,19 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - return autoPlayback; + //return autoPlayback; + //return new GotoPositionCommand(m_robotSwerveDrive, m_vision); + try{ + // Load the path you want to follow using its name in the GUI + return new PathPlannerAuto("Example Auto"); + } catch (Exception e) { + DriverStation.reportError("Big oops: " + e.getMessage(), e.getStackTrace()); + return Commands.none(); + } } + + /** * A button binding for two controllers, preferably an {@link DeadbandedXboxController Xbox Controller} and {@link VirtualController Virtual Xbox Controller} * @param joystickA A controller diff --git a/src/main/java/frc4388/robot/SwerveDrive.java b/src/main/java/frc4388/robot/SwerveDrive.java new file mode 100644 index 0000000..0253db8 --- /dev/null +++ b/src/main/java/frc4388/robot/SwerveDrive.java @@ -0,0 +1,330 @@ +// 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 java.util.Optional; + +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.swerve.SwerveDrivetrain; +import com.ctre.phoenix6.swerve.SwerveRequest; + +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.SwerveModuleState; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Commands; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.robot.Constants.VisionConstants; +import frc4388.utility.Status; +import frc4388.utility.Subsystem; +import frc4388.utility.Status.ReportLevel; + +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.config.RobotConfig; + +public class SwerveDrive extends Subsystem { + private SwerveDrivetrain swerveDriveTrain; + + private Vision vision; + + private int gear_index = SwerveDriveConstants.STARTING_GEAR; + private boolean stopped = false; + + public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index]; + public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED; + public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to 25% + + public double rotTarget = 0.0; + public Rotation2d orientRotTarget = new Rotation2d(); + public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); + + /** Creates a new SwerveDrive. */ + public SwerveDrive(SwerveDrivetrain swerveDriveTrain, Vision vision) { + // public SwerveDrive(SwerveDrivetrain swerveDriveTrain) { + super(); + + + RobotConfig config; + try{ + config = RobotConfig.fromGUISettings(); + } catch (Exception e) { + // Handle exception as needed + config = null; + } + AutoBuilder.configure( + () -> swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(null), // Robot pose supplier + swerveDriveTrain::resetPose, // Method to reset odometry (will be called if your auto has a starting pose) + () -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + (speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds() + .withSpeeds(speeds) + ), // 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(5.0, 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 + ); + + + this.swerveDriveTrain = swerveDriveTrain; + this.vision = vision; + } + + // public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){ + // // double ang = Math.atan2(rightStick.getY(), rightStick.getX()); + // // rightStick.getAngle() + // double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + Math.pow(leftStick.getY(), 2)); + // // System.out.println(ang); + // // module.go(ang); + // // Rotation2d rot = Rotation2d.fromRadians(ang); + // Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY()); + // SwerveModuleState state = new SwerveModuleState(speed, rot); + // module.setDesiredState(state); + // } + + public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: + stopModules(); // stop the swerve + + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput + return; // don't bother doing swerve drive math and return early. + + leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); + + if (fieldRelative) { + swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() + .withVelocityX(leftStick.getX()*-speedAdjust) + .withVelocityY(leftStick.getY()*speedAdjust) + .withRotationalRate(rightStick.getX()*rotSpeedAdjust) + ); + // double rot = 0; + + // ! drift correction + // dependant on if the new odomitry system acounts for rotational drift, this may not be needed. + // if (rightStick.getNorm() > 0.05) { + // rotTarget = swerveDriveTrain.getRotation3d().toRotation2d().getDegrees(); + // swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() + // .withVelocityX(leftStick.getX()*speedAdjust) + // .withVelocityY(leftStick.getY()*speedAdjust) + // .withRotationalRate(rightStick.getY()*rotSpeedAdjust) + // ); + + // // 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. + // chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1)); + } else { // Create robot-relative speeds. + swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() + .withVelocityX(leftStick.getX()*-speedAdjust) + .withVelocityY(leftStick.getY()*speedAdjust) + .withRotationalRate(rightStick.getX()*rotSpeedAdjust) + ); + } + } + + public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical reason to have a robot relitive version of this, and no pre provided version + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: + stopModules(); // stop the swerve + + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput + return; // don't bother doing swerve drive math and return early. + + leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); + + swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX()*speedAdjust) + .withVelocityY(leftStick.getY()*speedAdjust) + .withTargetDirection(rightStick.getAngle()) + ); + } + + public boolean rotateToTarget(double angle) { + swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(0) + .withVelocityY(0) + .withTargetDirection(Rotation2d.fromDegrees(angle)) + ); + + if (Math.abs(angle - getGyroAngle()) < 5.0) { + return true; + } + + return false; + } + + public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) { + // if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: + // stopModules(); // stop the swerve + + // if (leftStick.getNorm() < 0.05) //if no imput + // return; // don't bother doing swerve drive math and return early. + + leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); + + swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX()*-speedAdjust) + .withVelocityY(leftStick.getY()*speedAdjust) + .withTargetDirection(rot) + ); + // double + } + + public double getGyroAngle() { + return swerveDriveTrain.getRotation3d().getAngle(); + } + + public void resetGyro() { + swerveDriveTrain.tareEverything(); + } + + public void stopModules() { + swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run\ + SmartDashboard.putNumber("Gyro", getGyroAngle()); + SmartDashboard.putNumber("RotTartget", rotTarget); + + double time = Vision.getTime(); + + vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time)); + + if(vision.isTag()){ + swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time); + } + + // if(e.isPresent()) + } + + private void reset_index() { + gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the robot start in?) + } + + public void shiftDown() { + if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index + int i = gear_index - 1; + if (i == -1) i = 0; + setPercentOutput(SwerveDriveConstants.GEARS[i]); + gear_index = i; + } + + public void shiftUp() { + if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index + int i = gear_index + 1; + if (i == SwerveDriveConstants.GEARS.length) i = SwerveDriveConstants.GEARS.length - 1; + setPercentOutput(SwerveDriveConstants.GEARS[i]); + gear_index = i; + } + + public void setPercentOutput(double speed) { + speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; + gear_index = -1; + } + + public void setToSlow() { + setPercentOutput(SwerveDriveConstants.SLOW_SPEED); + gear_index = 0; + } + + public void setToFast() { + setPercentOutput(SwerveDriveConstants.FAST_SPEED); + gear_index = 1; + } + + public void setToTurbo() { + setPercentOutput(SwerveDriveConstants.TURBO_SPEED); + gear_index = 2; + } + + public void shiftUpRot() { + rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED; + } + + public void shiftDownRot() { + rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; + } + + @Override + public String getSubsystemName() { + return "Swerve Drive Controller"; + } + + ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") + .getLayout(getSubsystemName(), BuiltInLayouts.kList) + .withSize(2, 2); + + GenericEntry sbGyro = subsystemLayout + .add("Gyro angle", 0) + .withWidget(BuiltInWidgets.kGyro) + .getEntry(); + + GenericEntry sbShiftState = subsystemLayout + .add("Shift State", 0) + .withWidget(BuiltInWidgets.kNumberBar) + .getEntry(); + + @Override + public void queryStatus() { + sbGyro.setDouble(getGyroAngle()); + sbShiftState.setDouble(this.speedAdjust); + + //TODO: Add more status things + } + + @Override + public Status diagnosticStatus() { + Status status = new Status(); + + status.addReport(ReportLevel.ERROR, "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there."); + + return status; + } +} From cdfa020ab69dbffe51735821fd1686f7ab016913 Mon Sep 17 00:00:00 2001 From: Connorppeach Date: Wed, 15 Jan 2025 16:28:25 -0700 Subject: [PATCH 2/7] update robot container --- src/main/java/frc4388/robot/RobotContainer.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b2b4e9f..81827c1 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -227,11 +227,11 @@ public class RobotContainer { //return new GotoPositionCommand(m_robotSwerveDrive, m_vision); //try{ // // Load the path you want to follow using its name in the GUI - / return new PathPlannerAuto("Example Auto"); - } catch (Exception e) { - DriverStation.reportError("Big oops: " + e.getMessage(), e.getStackTrace()); - return Commands.none(); - } + // return new PathPlannerAuto("Example Auto"); + //} catch (Exception e) { + // DriverStation.reportError("Big oops: " + e.getMessage(), e.getStackTrace()); + // return Commands.none(); + //} // zach told me to do the below comment //return new GotoPositionCommand(m_robotSwerveDrive, m_vision); return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos); From bb0581533b42102e536ec0995e206e6e3fe95ade Mon Sep 17 00:00:00 2001 From: Connorppeach Date: Wed, 15 Jan 2025 16:31:53 -0700 Subject: [PATCH 3/7] test --- src/main/java/frc4388/robot/SwerveDrive.java | 330 ------------- .../frc4388/robot/subsystems/SwerveDrive.java | 450 ++++++++++-------- 2 files changed, 245 insertions(+), 535 deletions(-) delete mode 100644 src/main/java/frc4388/robot/SwerveDrive.java diff --git a/src/main/java/frc4388/robot/SwerveDrive.java b/src/main/java/frc4388/robot/SwerveDrive.java deleted file mode 100644 index 0253db8..0000000 --- a/src/main/java/frc4388/robot/SwerveDrive.java +++ /dev/null @@ -1,330 +0,0 @@ -// 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 java.util.Optional; - -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.swerve.SwerveDrivetrain; -import com.ctre.phoenix6.swerve.SwerveRequest; - -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.SwerveModuleState; -import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.Commands; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.robot.Constants.VisionConstants; -import frc4388.utility.Status; -import frc4388.utility.Subsystem; -import frc4388.utility.Status.ReportLevel; - -import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.config.RobotConfig; - -public class SwerveDrive extends Subsystem { - private SwerveDrivetrain swerveDriveTrain; - - private Vision vision; - - private int gear_index = SwerveDriveConstants.STARTING_GEAR; - private boolean stopped = false; - - public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index]; - public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED; - public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to 25% - - public double rotTarget = 0.0; - public Rotation2d orientRotTarget = new Rotation2d(); - public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); - - /** Creates a new SwerveDrive. */ - public SwerveDrive(SwerveDrivetrain swerveDriveTrain, Vision vision) { - // public SwerveDrive(SwerveDrivetrain swerveDriveTrain) { - super(); - - - RobotConfig config; - try{ - config = RobotConfig.fromGUISettings(); - } catch (Exception e) { - // Handle exception as needed - config = null; - } - AutoBuilder.configure( - () -> swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(null), // Robot pose supplier - swerveDriveTrain::resetPose, // Method to reset odometry (will be called if your auto has a starting pose) - () -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - (speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds() - .withSpeeds(speeds) - ), // 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(5.0, 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 - ); - - - this.swerveDriveTrain = swerveDriveTrain; - this.vision = vision; - } - - // public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){ - // // double ang = Math.atan2(rightStick.getY(), rightStick.getX()); - // // rightStick.getAngle() - // double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + Math.pow(leftStick.getY(), 2)); - // // System.out.println(ang); - // // module.go(ang); - // // Rotation2d rot = Rotation2d.fromRadians(ang); - // Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY()); - // SwerveModuleState state = new SwerveModuleState(speed, rot); - // module.setDesiredState(state); - // } - - public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: - stopModules(); // stop the swerve - - if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput - return; // don't bother doing swerve drive math and return early. - - leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); - - if (fieldRelative) { - swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() - .withVelocityX(leftStick.getX()*-speedAdjust) - .withVelocityY(leftStick.getY()*speedAdjust) - .withRotationalRate(rightStick.getX()*rotSpeedAdjust) - ); - // double rot = 0; - - // ! drift correction - // dependant on if the new odomitry system acounts for rotational drift, this may not be needed. - // if (rightStick.getNorm() > 0.05) { - // rotTarget = swerveDriveTrain.getRotation3d().toRotation2d().getDegrees(); - // swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() - // .withVelocityX(leftStick.getX()*speedAdjust) - // .withVelocityY(leftStick.getY()*speedAdjust) - // .withRotationalRate(rightStick.getY()*rotSpeedAdjust) - // ); - - // // 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. - // chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1)); - } else { // Create robot-relative speeds. - swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() - .withVelocityX(leftStick.getX()*-speedAdjust) - .withVelocityY(leftStick.getY()*speedAdjust) - .withRotationalRate(rightStick.getX()*rotSpeedAdjust) - ); - } - } - - public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical reason to have a robot relitive version of this, and no pre provided version - if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: - stopModules(); // stop the swerve - - if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput - return; // don't bother doing swerve drive math and return early. - - leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); - - swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX()*speedAdjust) - .withVelocityY(leftStick.getY()*speedAdjust) - .withTargetDirection(rightStick.getAngle()) - ); - } - - public boolean rotateToTarget(double angle) { - swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(0) - .withVelocityY(0) - .withTargetDirection(Rotation2d.fromDegrees(angle)) - ); - - if (Math.abs(angle - getGyroAngle()) < 5.0) { - return true; - } - - return false; - } - - public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) { - // if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: - // stopModules(); // stop the swerve - - // if (leftStick.getNorm() < 0.05) //if no imput - // return; // don't bother doing swerve drive math and return early. - - leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); - - swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX()*-speedAdjust) - .withVelocityY(leftStick.getY()*speedAdjust) - .withTargetDirection(rot) - ); - // double - } - - public double getGyroAngle() { - return swerveDriveTrain.getRotation3d().getAngle(); - } - - public void resetGyro() { - swerveDriveTrain.tareEverything(); - } - - public void stopModules() { - swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run\ - SmartDashboard.putNumber("Gyro", getGyroAngle()); - SmartDashboard.putNumber("RotTartget", rotTarget); - - double time = Vision.getTime(); - - vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time)); - - if(vision.isTag()){ - swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time); - } - - // if(e.isPresent()) - } - - private void reset_index() { - gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the robot start in?) - } - - public void shiftDown() { - if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index - int i = gear_index - 1; - if (i == -1) i = 0; - setPercentOutput(SwerveDriveConstants.GEARS[i]); - gear_index = i; - } - - public void shiftUp() { - if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index - int i = gear_index + 1; - if (i == SwerveDriveConstants.GEARS.length) i = SwerveDriveConstants.GEARS.length - 1; - setPercentOutput(SwerveDriveConstants.GEARS[i]); - gear_index = i; - } - - public void setPercentOutput(double speed) { - speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; - gear_index = -1; - } - - public void setToSlow() { - setPercentOutput(SwerveDriveConstants.SLOW_SPEED); - gear_index = 0; - } - - public void setToFast() { - setPercentOutput(SwerveDriveConstants.FAST_SPEED); - gear_index = 1; - } - - public void setToTurbo() { - setPercentOutput(SwerveDriveConstants.TURBO_SPEED); - gear_index = 2; - } - - public void shiftUpRot() { - rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED; - } - - public void shiftDownRot() { - rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; - } - - @Override - public String getSubsystemName() { - return "Swerve Drive Controller"; - } - - ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") - .getLayout(getSubsystemName(), BuiltInLayouts.kList) - .withSize(2, 2); - - GenericEntry sbGyro = subsystemLayout - .add("Gyro angle", 0) - .withWidget(BuiltInWidgets.kGyro) - .getEntry(); - - GenericEntry sbShiftState = subsystemLayout - .add("Shift State", 0) - .withWidget(BuiltInWidgets.kNumberBar) - .getEntry(); - - @Override - public void queryStatus() { - sbGyro.setDouble(getGyroAngle()); - sbShiftState.setDouble(this.speedAdjust); - - //TODO: Add more status things - } - - @Override - public Status diagnosticStatus() { - Status status = new Status(); - - status.addReport(ReportLevel.ERROR, "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there."); - - return status; - } -} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 14bf6a8..733e342 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -31,257 +31,297 @@ import frc4388.utility.Status; import frc4388.utility.Subsystem; import frc4388.utility.Status.ReportLevel; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.config.RobotConfig; + public class SwerveDrive extends Subsystem { - private SwerveDrivetrain swerveDriveTrain; + private SwerveDrivetrain swerveDriveTrain; - private Vision vision; + private Vision vision; - private int gear_index = SwerveDriveConstants.STARTING_GEAR; - private boolean stopped = false; + private int gear_index = SwerveDriveConstants.STARTING_GEAR; + private boolean stopped = false; - public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index]; - public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED; - public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to 25% + public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index]; + public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED; + public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to 25% - public double rotTarget = 0.0; - public Rotation2d orientRotTarget = new Rotation2d(); - public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); + public double rotTarget = 0.0; + public Rotation2d orientRotTarget = new Rotation2d(); + public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); - /** Creates a new SwerveDrive. */ - public SwerveDrive(SwerveDrivetrain swerveDriveTrain, Vision vision) { - // public SwerveDrive(SwerveDrivetrain swerveDriveTrain) { - super(); + /** Creates a new SwerveDrive. */ + public SwerveDrive(SwerveDrivetrain swerveDriveTrain, Vision vision) { + // public SwerveDrive(SwerveDrivetrain swerveDriveTrain) { + super(); - this.swerveDriveTrain = swerveDriveTrain; - this.vision = vision; - } + this.swerveDriveTrain = swerveDriveTrain; + this.vision = vision; - // public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){ - // // double ang = Math.atan2(rightStick.getY(), rightStick.getX()); - // // rightStick.getAngle() - // double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + Math.pow(leftStick.getY(), 2)); - // // System.out.println(ang); - // // module.go(ang); - // // Rotation2d rot = Rotation2d.fromRadians(ang); - // Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY()); - // SwerveModuleState state = new SwerveModuleState(speed, rot); - // module.setDesiredState(state); - // } + RobotConfig config; + try{ + config = RobotConfig.fromGUISettings(); + } catch (Exception e) { + // Handle exception as needed + config = null; + } + AutoBuilder.configure( + () -> swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(null), // Robot pose supplier + swerveDriveTrain::resetPose, // Method to reset odometry (will be called if your auto has a starting pose) + () -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + (speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds() + .withSpeeds(speeds) + ), // 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(5.0, 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 - public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: - stopModules(); // stop the swerve + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this // Reference to this subsystem to set requirements + ) + + } + + // public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){ + // // double ang = Math.atan2(rightStick.getY(), rightStick.getX()); + // // rightStick.getAngle() + // double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + Math.pow(leftStick.getY(), 2)); + // // System.out.println(ang); + // // module.go(ang); + // // Rotation2d rot = Rotation2d.fromRadians(ang); + // Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY()); + // SwerveModuleState state = new SwerveModuleState(speed, rot); + // module.setDesiredState(state); + // } + + public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: + stopModules(); // stop the swerve - if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput - return; // don't bother doing swerve drive math and return early. + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput + return; // don't bother doing swerve drive math and return early. - leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); + leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); - if (fieldRelative) { - swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() - .withVelocityX(leftStick.getX()*speedAdjust) - .withVelocityY(leftStick.getY()*speedAdjust) - .withRotationalRate(rightStick.getX()*rotSpeedAdjust) - ); - // double rot = 0; + if (fieldRelative) { + swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() + .withVelocityX(leftStick.getX()*speedAdjust) + .withVelocityY(leftStick.getY()*speedAdjust) + .withRotationalRate(rightStick.getX()*rotSpeedAdjust) + ); + // double rot = 0; - // ! drift correction - // dependant on if the new odomitry system acounts for rotational drift, this may not be needed. - // if (rightStick.getNorm() > 0.05) { - // rotTarget = swerveDriveTrain.getRotation3d().toRotation2d().getDegrees(); - // swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() - // .withVelocityX(leftStick.getX()*speedAdjust) - // .withVelocityY(leftStick.getY()*speedAdjust) - // .withRotationalRate(rightStick.getY()*rotSpeedAdjust) - // ); + // ! drift correction + // dependant on if the new odomitry system acounts for rotational drift, this may not be needed. + // if (rightStick.getNorm() > 0.05) { + // rotTarget = swerveDriveTrain.getRotation3d().toRotation2d().getDegrees(); + // swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() + // .withVelocityX(leftStick.getX()*speedAdjust) + // .withVelocityY(leftStick.getY()*speedAdjust) + // .withRotationalRate(rightStick.getY()*rotSpeedAdjust) + // ); - // // SmartDashboard.putBoolean("drift correction", false); - // stopped = false; - // } else if(leftStick.getNorm() > 0.05) { - // if (!stopped) { - // stopModules(); - // stopped = true; - // } + // // 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. - // chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1)); - } else { // Create robot-relative speeds. - swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() - .withVelocityX(leftStick.getX()*-speedAdjust) - .withVelocityY(leftStick.getY()*speedAdjust) - .withRotationalRate(rightStick.getX()*rotSpeedAdjust) - ); + // // Convert field-relative speeds to robot-relative speeds. + // // chassisSpeeds = chassisSpeeds. + // chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1)); + } else { // Create robot-relative speeds. + swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() + .withVelocityX(leftStick.getX()*-speedAdjust) + .withVelocityY(leftStick.getY()*speedAdjust) + .withRotationalRate(rightStick.getX()*rotSpeedAdjust) + ); + } } - } - public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical reason to have a robot relitive version of this, and no pre provided version - if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: - stopModules(); // stop the swerve + public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical reason to have a robot relitive version of this, and no pre provided version + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: + stopModules(); // stop the swerve - if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput - return; // don't bother doing swerve drive math and return early. + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput + return; // don't bother doing swerve drive math and return early. - leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); + leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); - swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX()*speedAdjust) - .withVelocityY(leftStick.getY()*speedAdjust) - .withTargetDirection(rightStick.getAngle()) - ); - } - - public boolean rotateToTarget(double angle) { - swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(0) - .withVelocityY(0) - .withTargetDirection(Rotation2d.fromDegrees(angle)) - ); - - if (Math.abs(angle - getGyroAngle()) < 5.0) { - return true; + swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX()*speedAdjust) + .withVelocityY(leftStick.getY()*speedAdjust) + .withTargetDirection(rightStick.getAngle()) + ); } - return false; - } + public boolean rotateToTarget(double angle) { + swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(0) + .withVelocityY(0) + .withTargetDirection(Rotation2d.fromDegrees(angle)) + ); - public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) { - // if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: - // stopModules(); // stop the swerve - - // if (leftStick.getNorm() < 0.05) //if no imput - // return; // don't bother doing swerve drive math and return early. + if (Math.abs(angle - getGyroAngle()) < 5.0) { + return true; + } - leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); - - swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX()*-speedAdjust) - .withVelocityY(leftStick.getY()*speedAdjust) - .withTargetDirection(rot) - ); - // double - } - - public double getGyroAngle() { - return swerveDriveTrain.getRotation3d().getAngle(); - } - - public void resetGyro() { - swerveDriveTrain.tareEverything(); - } - - public void stopModules() { - // swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run\ - SmartDashboard.putNumber("Gyro", getGyroAngle()); - SmartDashboard.putNumber("RotTartget", rotTarget); - - double time = Vision.getTime(); - - vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time)); - - if(vision.isTag()){ - swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time); + return false; } - // if(e.isPresent()) - } + public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) { + // if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: + // stopModules(); // stop the swerve + + // if (leftStick.getNorm() < 0.05) //if no imput + // return; // don't bother doing swerve drive math and return early. - private void reset_index() { - gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the robot start in?) - } + leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); + + swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX()*-speedAdjust) + .withVelocityY(leftStick.getY()*speedAdjust) + .withTargetDirection(rot) + ); + // double + } - public void shiftDown() { - if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index - int i = gear_index - 1; - if (i == -1) i = 0; - setPercentOutput(SwerveDriveConstants.GEARS[i]); - gear_index = i; - } + public double getGyroAngle() { + return swerveDriveTrain.getRotation3d().getAngle(); + } - public void shiftUp() { - if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index - int i = gear_index + 1; - if (i == SwerveDriveConstants.GEARS.length) i = SwerveDriveConstants.GEARS.length - 1; - setPercentOutput(SwerveDriveConstants.GEARS[i]); - gear_index = i; - } + public void resetGyro() { + swerveDriveTrain.tareEverything(); + } - public void setPercentOutput(double speed) { - speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; - gear_index = -1; - } + public void stopModules() { + // swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); + } - public void setToSlow() { - setPercentOutput(SwerveDriveConstants.SLOW_SPEED); - gear_index = 0; - } + @Override + public void periodic() { + // This method will be called once per scheduler run\ + SmartDashboard.putNumber("Gyro", getGyroAngle()); + SmartDashboard.putNumber("RotTartget", rotTarget); - public void setToFast() { - setPercentOutput(SwerveDriveConstants.FAST_SPEED); - gear_index = 1; - } + double time = Vision.getTime(); - public void setToTurbo() { - setPercentOutput(SwerveDriveConstants.TURBO_SPEED); - gear_index = 2; - } + vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time)); - public void shiftUpRot() { - rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED; - } + if(vision.isTag()){ + swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time); + } - public void shiftDownRot() { - rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; - } + // if(e.isPresent()) + } - @Override - public String getSubsystemName() { - return "Swerve Drive Controller"; - } + private void reset_index() { + gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the robot start in?) + } - ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") - .getLayout(getSubsystemName(), BuiltInLayouts.kList) - .withSize(2, 2); + public void shiftDown() { + if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index + int i = gear_index - 1; + if (i == -1) i = 0; + setPercentOutput(SwerveDriveConstants.GEARS[i]); + gear_index = i; + } - GenericEntry sbGyro = subsystemLayout - .add("Gyro angle", 0) - .withWidget(BuiltInWidgets.kGyro) - .getEntry(); + public void shiftUp() { + if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index + int i = gear_index + 1; + if (i == SwerveDriveConstants.GEARS.length) i = SwerveDriveConstants.GEARS.length - 1; + setPercentOutput(SwerveDriveConstants.GEARS[i]); + gear_index = i; + } - GenericEntry sbShiftState = subsystemLayout - .add("Shift State", 0) - .withWidget(BuiltInWidgets.kNumberBar) - .getEntry(); + public void setPercentOutput(double speed) { + speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; + gear_index = -1; + } - @Override - public void queryStatus() { - sbGyro.setDouble(getGyroAngle()); - sbShiftState.setDouble(this.speedAdjust); + public void setToSlow() { + setPercentOutput(SwerveDriveConstants.SLOW_SPEED); + gear_index = 0; + } + + public void setToFast() { + setPercentOutput(SwerveDriveConstants.FAST_SPEED); + gear_index = 1; + } + + public void setToTurbo() { + setPercentOutput(SwerveDriveConstants.TURBO_SPEED); + gear_index = 2; + } + + public void shiftUpRot() { + rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED; + } + + public void shiftDownRot() { + rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; + } + + @Override + public String getSubsystemName() { + return "Swerve Drive Controller"; + } + + ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") + .getLayout(getSubsystemName(), BuiltInLayouts.kList) + .withSize(2, 2); + + GenericEntry sbGyro = subsystemLayout + .add("Gyro angle", 0) + .withWidget(BuiltInWidgets.kGyro) + .getEntry(); + + GenericEntry sbShiftState = subsystemLayout + .add("Shift State", 0) + .withWidget(BuiltInWidgets.kNumberBar) + .getEntry(); + + @Override + public void queryStatus() { + sbGyro.setDouble(getGyroAngle()); + sbShiftState.setDouble(this.speedAdjust); - //TODO: Add more status things - } + //TODO: Add more status things + } - @Override - public Status diagnosticStatus() { - Status status = new Status(); + @Override + public Status diagnosticStatus() { + Status status = new Status(); - status.addReport(ReportLevel.ERROR, "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there."); + status.addReport(ReportLevel.ERROR, "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there."); - return status; - } + return status; + } } From 9c409f956b16c2e57ca5a027b6dcab739f15b8b8 Mon Sep 17 00:00:00 2001 From: Connorppeach Date: Wed, 15 Jan 2025 16:49:32 -0700 Subject: [PATCH 4/7] fix --- src/main/java/frc4388/robot/Constants.java | 3 +-- src/main/java/frc4388/robot/subsystems/SwerveDrive.java | 3 ++- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 0108060..bfceb05 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -104,7 +104,6 @@ public final class Constants { public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 1.d, 10); private static final class ModuleSpecificConstants { - //Front Left private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125+.25); private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; @@ -326,4 +325,4 @@ public final class Constants { public static final double LEFT_AXIS_DEADBAND = 0.1; } -} \ No newline at end of file +} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 733e342..1f472df 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -30,6 +30,7 @@ import frc4388.robot.Constants.VisionConstants; import frc4388.utility.Status; import frc4388.utility.Subsystem; import frc4388.utility.Status.ReportLevel; +import edu.wpi.first.wpilibj.DriverStation; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.config.PIDConstants; @@ -92,7 +93,7 @@ public class SwerveDrive extends Subsystem { return false; }, this // Reference to this subsystem to set requirements - ) + ); } From a8bfc1543f35ff41c27d0422cbc5fc46edc4d635 Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Wed, 15 Jan 2025 17:37:18 -0700 Subject: [PATCH 5/7] test path planner --- .../deploy/pathplanner/autos/New Auto.auto | 19 +++++++ src/main/deploy/pathplanner/navgrid.json | 1 + .../pathplanner/paths/Example Path.path | 54 +++++++++++++++++++ src/main/deploy/pathplanner/settings.json | 32 +++++++++++ .../java/frc4388/robot/RobotContainer.java | 16 +++--- .../frc4388/robot/subsystems/SwerveDrive.java | 4 +- 6 files changed, 116 insertions(+), 10 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto create mode 100644 src/main/deploy/pathplanner/navgrid.json create mode 100644 src/main/deploy/pathplanner/paths/Example Path.path create mode 100644 src/main/deploy/pathplanner/settings.json diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto new file mode 100644 index 0000000..70b7ab2 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Example Path" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..23e0db9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path new file mode 100644 index 0000000..23aae84 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.0, + "y": 0.0 + }, + "prevControl": null, + "nextControl": { + "x": 0.25, + "y": 0.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0, + "y": 0.0 + }, + "prevControl": { + "x": 1.75, + "y": 0.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..16b0598 --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,32 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, + "robotMass": 22.6796, + "robotMOI": 6.883, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.048, + "driveGearing": 5.143, + "maxDriveSpeed": 5.45, + "driveMotorType": "krakenX60", + "driveCurrentLimit": 60.0, + "wheelCOF": 1.2, + "flModuleX": 0.273, + "flModuleY": 0.273, + "frModuleX": 0.273, + "frModuleY": -0.273, + "blModuleX": -0.273, + "blModuleY": 0.273, + "brModuleX": -0.273, + "brModuleY": -0.273, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [] +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 81827c1..d9fcbd3 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -225,16 +225,16 @@ public class RobotContainer { public Command getAutonomousCommand() { //return autoPlayback; //return new GotoPositionCommand(m_robotSwerveDrive, m_vision); - //try{ - // // Load the path you want to follow using its name in the GUI - // return new PathPlannerAuto("Example Auto"); - //} catch (Exception e) { - // DriverStation.reportError("Big oops: " + e.getMessage(), e.getStackTrace()); - // return Commands.none(); - //} + try{ + // Load the path you want to follow using its name in the GUI + return new PathPlannerAuto("New Auto"); + } catch (Exception e) { + DriverStation.reportError("Big oops: " + e.getMessage(), e.getStackTrace()); + return Commands.none(); + } // zach told me to do the below comment //return new GotoPositionCommand(m_robotSwerveDrive, m_vision); - return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos); + // return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos); } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 1f472df..ba19149 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -70,7 +70,7 @@ public class SwerveDrive extends Subsystem { config = null; } AutoBuilder.configure( - () -> swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(null), // Robot pose supplier + () -> swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()), // Robot pose supplier swerveDriveTrain::resetPose, // Method to reset odometry (will be called if your auto has a starting pose) () -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE (speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds() @@ -222,7 +222,7 @@ public class SwerveDrive extends Subsystem { } public void stopModules() { - // swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); + swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); } @Override From 1bd3c12327465a9e28ebdace75f1b17ed05b34c6 Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Thu, 16 Jan 2025 19:41:08 -0700 Subject: [PATCH 6/7] fix 2025 module possitions --- .../pathplanner/paths/Example Path.path | 18 +++++----- src/main/java/frc4388/robot/Constants.java | 34 +++++++++---------- .../java/frc4388/robot/RobotContainer.java | 1 + .../frc4388/robot/subsystems/SwerveDrive.java | 14 +++++--- 4 files changed, 37 insertions(+), 30 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path index 23aae84..f723e20 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 0.0, - "y": 0.0 + "x": 2.780674342105263, + "y": 5.112253289473684 }, "prevControl": null, "nextControl": { - "x": 0.25, - "y": 0.0 + "x": 2.7421875, + "y": 6.016694078947369 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.0, - "y": 0.0 + "x": 3.8294407894736837, + "y": 5.862746710526316 }, "prevControl": { - "x": 1.75, - "y": 0.0 + "x": 3.5794407894736837, + "y": 5.862746710526316 }, "nextControl": null, "isLocked": false, @@ -48,7 +48,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 132.26189909327812 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index bfceb05..e2f669e 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -99,41 +99,41 @@ public final class Constants { public static final double MAX_SPEED_FEET_PER_SECOND = MAX_SPEED_MEETERS_PER_SEC * 3.28084; public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; - public static final double FORWARD_OFFSET = 0; // 0, 90, 180, 270 + public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270 public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 1.d, 10); private static final class ModuleSpecificConstants { - private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125+.25); - private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; + private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125); + private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = true; private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; - private static final Distance FRONT_LEFT_XPOS = Inches.of(-HALF_WIDTH); - private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); + private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_HEIGHT); + private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_WIDTH); //Front Right - private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.371337890625+.5+.25); - private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false; + private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.371337890625+.5); + private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; - private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH); - private static final Distance FRONT_RIGHT_YPOS = Inches.of(HALF_HEIGHT); + private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_HEIGHT); + private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_WIDTH); //Back Left - private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.3828125+.25+.5); - private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.3828125+.5); + private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = true; private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_LEFT_ENCODER_INVERTED = false; - private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH); - private static final Distance BACK_LEFT_YPOS = Inches.of(-HALF_HEIGHT); + private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_HEIGHT); + private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_WIDTH); //Back Right - private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.01904296875+.25); - private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; + private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.07666015625); + private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = true; private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; - private static final Distance BACK_RIGHT_XPOS = Inches.of(HALF_WIDTH); - private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); + private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_HEIGHT); + private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_WIDTH); } public static final class IDs { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d9fcbd3..71b9b76 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -14,6 +14,7 @@ import java.util.ArrayList; import java.util.List; import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.GenericHID; import frc4388.utility.controller.XboxController; import frc4388.utility.controller.DeadbandedXboxController; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index ba19149..f50c3f6 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -5,6 +5,7 @@ package frc4388.robot.subsystems; import java.util.Optional; +import java.util.function.DoubleSupplier; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.hardware.CANcoder; @@ -69,8 +70,13 @@ public class SwerveDrive extends Subsystem { // Handle exception as needed config = null; } + // DoubleSupplier a = () -> 1.d; AutoBuilder.configure( - () -> swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()), // Robot pose supplier + () -> { + var pose = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()); + pose = new Pose2d(pose.getX(), pose.getY(), pose.getRotation().times(-1)); + return pose;//getRotation().times(-1) + }, // Robot pose supplier swerveDriveTrain::resetPose, // Method to reset odometry (will be called if your auto has a starting pose) () -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE (speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds() @@ -121,7 +127,7 @@ public class SwerveDrive extends Subsystem { if (fieldRelative) { swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() .withVelocityX(leftStick.getX()*speedAdjust) - .withVelocityY(leftStick.getY()*speedAdjust) + .withVelocityY(-leftStick.getY()*speedAdjust) .withRotationalRate(rightStick.getX()*rotSpeedAdjust) ); // double rot = 0; @@ -159,8 +165,8 @@ public class SwerveDrive extends Subsystem { // chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1)); } else { // Create robot-relative speeds. swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() - .withVelocityX(leftStick.getX()*-speedAdjust) - .withVelocityY(leftStick.getY()*speedAdjust) + .withVelocityX(leftStick.getX()*speedAdjust) + .withVelocityY(-leftStick.getY()*speedAdjust) .withRotationalRate(rightStick.getX()*rotSpeedAdjust) ); } From dd033d076873dd4fd18e795d6882043291643f84 Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Fri, 17 Jan 2025 16:59:32 -0700 Subject: [PATCH 7/7] functional pathplanner... needs autos. --- .../deploy/pathplanner/paths/Example Path.path | 14 +++++++------- src/main/java/frc4388/robot/Constants.java | 3 ++- .../java/frc4388/robot/subsystems/SwerveDrive.java | 2 +- 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path index f723e20..946269c 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.780674342105263, - "y": 5.112253289473684 + "x": 2.415049342105263, + "y": 4.785115131578947 }, "prevControl": null, "nextControl": { - "x": 2.7421875, - "y": 6.016694078947369 + "x": 2.905756578947368, + "y": 4.794736842105262 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 5.862746710526316 }, "prevControl": { - "x": 3.5794407894736837, - "y": 5.862746710526316 + "x": 3.0693256578947365, + "y": 5.872368421052631 }, "nextControl": null, "isLocked": false, @@ -48,7 +48,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 132.26189909327812 + "rotation": -2.4366482468102095 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e2f669e..249a881 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -103,7 +103,8 @@ public final class Constants { public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 1.d, 10); - private static final class ModuleSpecificConstants { + private static final class ModuleSpecificConstants { //2025 + //Front Left private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125); private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = true; private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index f50c3f6..65db294 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -74,7 +74,7 @@ public class SwerveDrive extends Subsystem { AutoBuilder.configure( () -> { var pose = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()); - pose = new Pose2d(pose.getX(), pose.getY(), pose.getRotation().times(-1)); + // pose = new Pose2d(pose.getX(), pose.getY(), pose.getRotation().times(-1)); return pose;//getRotation().times(-1) }, // Robot pose supplier swerveDriveTrain::resetPose, // Method to reset odometry (will be called if your auto has a starting pose)