From dcf055233c1fb4c06a99b4fe46b904a45f609bf1 Mon Sep 17 00:00:00 2001 From: "maxwell.lee.372007" Date: Sat, 9 Mar 2024 14:00:46 -0500 Subject: [PATCH 01/23] driving work --- .../java/frc/robot/subsystems/TurdPod.java | 24 +++++++------------ 1 file changed, 8 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/TurdPod.java b/src/main/java/frc/robot/subsystems/TurdPod.java index 31f1195..bece312 100644 --- a/src/main/java/frc/robot/subsystems/TurdPod.java +++ b/src/main/java/frc/robot/subsystems/TurdPod.java @@ -4,24 +4,16 @@ package frc.robot.subsystems; -import com.revrobotics.AnalogInput; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkMaxPIDController; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import com.revrobotics.SparkPIDController; +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANSparkLowLevel.MotorType; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.AnalogEncoder; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.PWM; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -34,7 +26,7 @@ public class TurdPod extends SubsystemBase { private final RelativeEncoder azimuthEncoder; private final RelativeEncoder driveEncoder; - private final SparkMaxPIDController azimuthPID; + private final SparkPIDController azimuthPID; private double azimuthDriveSpeedMultiplier; private double speed = 0; @@ -83,8 +75,8 @@ public void setPID(double P, double I, double D, double IZone, double outputRang azimuthPID.setPositionPIDWrappingMaxInput(Math.PI); azimuthPID.setPositionPIDWrappingMinInput(-Math.PI); azimuthPID.setPositionPIDWrappingEnabled(true); - azimuthPID.setSmartMotionAllowedClosedLoopError(0, 0); - azimuth.setClosedLoopRampRate(.1); + // azimuthPID.setSmartMotionAllowedClosedLoopError(0, 0); + azimuth.setClosedLoopRampRate(0.35); azimuthDriveSpeedMultiplier = ADMult; } @@ -94,7 +86,7 @@ public SwerveModulePosition getPodPosition() { public void setPodState(SwerveModuleState state) { state = SwerveModuleState.optimize(state, new Rotation2d(azimuthEncoder.getPosition())); // does not account for rotations between 180 and 360? - azimuthPID.setReference(state.angle.getRadians(), CANSparkMax.ControlType.kPosition); + azimuthPID.setReference(state.angle.getRadians(), ControlType.kPosition); speed = Math.abs(state.speedMetersPerSecond) < .01 ? 0 : state.speedMetersPerSecond * Constants.driveSpeedToPower; SmartDashboard.putNumber("state.angle.getRadians()", state.angle.getRadians()); @@ -112,7 +104,7 @@ public double getAbsoluteEncoder() { @Override public void periodic() { SmartDashboard.putNumber("getabsoluteEncoder() " + absoluteEncoder.getChannel(), getAbsoluteEncoder()); - drive.set(speed + (azimuth.getAppliedOutput() * azimuthDriveSpeedMultiplier)); + drive.set(speed);// + (azimuth.getAppliedOutput() * azimuthDriveSpeedMultiplier)); SmartDashboard.putNumber("azimuthEncoder.getPosition() " + azimuth.getDeviceId(), azimuthEncoder.getPosition()); SmartDashboard.putNumber("drive pos " + drive.getDeviceId(), driveEncoder.getPosition()); SmartDashboard.putNumber("azimuth.getAppliedOutput()" + azimuth.getDeviceId(), azimuth.getAppliedOutput()); //getAppliedOutput()); From a055a353dd14e6d9d1f725ef9508b4c5b730b243 Mon Sep 17 00:00:00 2001 From: "maxwell.lee.372007" Date: Sat, 9 Mar 2024 14:49:33 -0500 Subject: [PATCH 02/23] revive autoturn --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 10 ++++++- .../java/frc/robot/commands/TurdDrive.java | 10 +++++-- .../java/frc/robot/subsystems/TurdPod.java | 2 +- .../java/frc/robot/subsystems/TurdSwerve.java | 27 ++++++++++++++++--- 5 files changed, 43 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7695487..76d7afb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -50,7 +50,7 @@ public final class Constants { public static final double azimuthkI = 0.0; public static final double azimuthkD = 0.003; public static final double azimuthkIz = 0; - public static final double azimuthDriveSpeedMultiplier = 0.5; + public static final double azimuthDriveSpeedMultiplier = 0;//0.5; public static final IdleMode azimuthMode = IdleMode.kBrake; public static final IdleMode driveMode = IdleMode.kCoast; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index db22d17..00a6c11 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,8 +7,11 @@ import java.util.function.Supplier; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.Odometry; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -21,13 +24,18 @@ public class RobotContainer { public static final XboxController driver = new XboxController(Constants.driverPort); // public static final TurdPod leftPod = new TurdPod(Constants.leftAzimuthID, Constants.leftDriveID, Constants.leftAbsoluteEncoderID, Constants.leftAzimuthInvert,Constants.rightAzimuthInvert, Constants.leftAbsoluteEncoderOffset); public static final TurdSwerve swerve = new TurdSwerve(); + public RobotContainer() { + final var Odometry = Shuffleboard.getTab("Odometry"); configureBindings(); Supplier driverRightJoystick = () -> new Translation2d(driver.getRightX(), driver.getRightY()); Supplier driverLeftJoystick = () -> new Translation2d(driver.getLeftX(), driver.getLeftY()); Supplier buttonStart = () -> driver.getStartButton(); - swerve.setDefaultCommand(new TurdDrive(swerve, driverLeftJoystick, driverRightJoystick, buttonStart)); + Supplier DPAD = () -> driver.getPOV(); + swerve.setDefaultCommand(new TurdDrive(swerve, driverLeftJoystick, driverRightJoystick, buttonStart, DPAD)); + swerve.addDashboardWidgets(Odometry); + } private void configureBindings() { diff --git a/src/main/java/frc/robot/commands/TurdDrive.java b/src/main/java/frc/robot/commands/TurdDrive.java index 5a9d244..3516402 100644 --- a/src/main/java/frc/robot/commands/TurdDrive.java +++ b/src/main/java/frc/robot/commands/TurdDrive.java @@ -11,6 +11,7 @@ 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.math.util.Units; import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.TurdPod; @@ -22,13 +23,15 @@ public class TurdDrive extends Command { Supplier joystickRight; Supplier joystickLeft; Supplier resetPods; + Supplier DPAD; Rotation2d rotation = new Rotation2d(); - public TurdDrive(TurdSwerve swerve, Supplier joystickLeft, Supplier joystickRight, Supplier resetPods) { + public TurdDrive(TurdSwerve swerve, Supplier joystickLeft, Supplier joystickRight, Supplier resetPods, Supplier DPAD) { this.swerve = swerve; this.joystickRight = joystickRight; this.joystickLeft = joystickLeft; this.resetPods = resetPods; + this.DPAD = DPAD; addRequirements(swerve); } @@ -42,10 +45,13 @@ public void execute() { if (resetPods.get()) { swerve.resetPods(); } + if (DPAD.get() != -1) { + swerve.targetAngle = -Units.degreesToRadians(DPAD.get()); + } boolean deadband = Math.abs(joystickRight.get().getX()) + Math.abs(joystickRight.get().getY()) < 0.1; double speedX = deadband ? 0 : -joystickRight.get().getX(); double speedY = deadband ? 0 : joystickRight.get().getY(); - double speedOmega = -joystickLeft.get().getX() * Math.abs(joystickLeft.get().getX()); + double speedOmega = Math.abs(joystickLeft.get().getX()) > 0.07 ? -joystickLeft.get().getX() * Math.abs(joystickLeft.get().getX()) : 0; ChassisSpeeds speeds = new ChassisSpeeds(speedX, speedY, speedOmega); swerve.setRobotSpeeds(speeds); } diff --git a/src/main/java/frc/robot/subsystems/TurdPod.java b/src/main/java/frc/robot/subsystems/TurdPod.java index bece312..76ca926 100644 --- a/src/main/java/frc/robot/subsystems/TurdPod.java +++ b/src/main/java/frc/robot/subsystems/TurdPod.java @@ -104,7 +104,7 @@ public double getAbsoluteEncoder() { @Override public void periodic() { SmartDashboard.putNumber("getabsoluteEncoder() " + absoluteEncoder.getChannel(), getAbsoluteEncoder()); - drive.set(speed);// + (azimuth.getAppliedOutput() * azimuthDriveSpeedMultiplier)); + drive.set(speed + (azimuth.getAppliedOutput() * azimuthDriveSpeedMultiplier)); SmartDashboard.putNumber("azimuthEncoder.getPosition() " + azimuth.getDeviceId(), azimuthEncoder.getPosition()); SmartDashboard.putNumber("drive pos " + drive.getDeviceId(), driveEncoder.getPosition()); SmartDashboard.putNumber("azimuth.getAppliedOutput()" + azimuth.getDeviceId(), azimuth.getAppliedOutput()); //getAppliedOutput()); diff --git a/src/main/java/frc/robot/subsystems/TurdSwerve.java b/src/main/java/frc/robot/subsystems/TurdSwerve.java index b190607..edbe55e 100644 --- a/src/main/java/frc/robot/subsystems/TurdSwerve.java +++ b/src/main/java/frc/robot/subsystems/TurdSwerve.java @@ -16,6 +16,7 @@ import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -38,10 +39,15 @@ public class TurdSwerve extends SubsystemBase { private GenericEntry ADMult = tab.add("azimuth-drive speed multiplier", Constants.azimuthDriveSpeedMultiplier).getEntry(); private PIDController GyroPID = new PIDController(Constants.gyroP, Constants.gyroI, Constants.gyroD); - private double targetAngle = 0; + public double targetAngle = 0; private Rotation2d gyroResetAngle = new Rotation2d(); + + + private final Field2d field2d = new Field2d(); + public TurdSwerve() { + GyroPID.enableContinuousInput(0.0, 2*Math.PI); // gyro.configAllSettings(new Pigeon2Configuration()); } @@ -71,10 +77,13 @@ public void setLeftPod(SwerveModuleState state) { } public void setRobotSpeeds(ChassisSpeeds chassisSpeeds) { - targetAngle += chassisSpeeds.omegaRadiansPerSecond / 10; - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, chassisSpeeds.omegaRadiansPerSecond * 3 /*GyroPID.calculate(getGyro().getRadians(), targetAngle)*/, getGyro()); + boolean manualTurn = Math.abs(chassisSpeeds.omegaRadiansPerSecond) > 0.1; + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, manualTurn ? chassisSpeeds.omegaRadiansPerSecond * 3.0 : GyroPID.calculate(getGyro().getRadians(), targetAngle), getGyro()); SwerveModuleState[] states = Constants.drivetrainKinematics.toSwerveModuleStates(chassisSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(states, Constants.podMaxSpeed); + if (manualTurn) { + targetAngle = getGyro().getRadians() + (chassisSpeeds.omegaRadiansPerSecond / 2.0); + } leftPod.setPodState(states[0]); rightPod.setPodState(states[1]); @@ -85,4 +94,16 @@ public void periodic() { odometer.update(getGyro(), new SwerveModulePosition[] {leftPod.getPodPosition(), rightPod.getPodPosition()}); SmartDashboard.putNumber("pigeon", getGyro().getDegrees()); } + + private String getFomattedPose() { + var pose = odometer.getPoseMeters(); + return String.format( + "(%.3f, %.3f) %.2f degrees", + pose.getX(), pose.getY(), pose.getRotation().getDegrees()); + } + + public void addDashboardWidgets(ShuffleboardTab tab) { + // tab.add("Field", field2d).withPosition(0, 0).withSize(6, 4); + tab.addString("Pose", this::getFomattedPose).withPosition(6, 2).withSize(2, 1); + } } From 9e9cabec8c8040d3cb52266e40ca913171d60e9d Mon Sep 17 00:00:00 2001 From: "maxwell.lee.372007" Date: Fri, 15 Mar 2024 13:55:19 -0400 Subject: [PATCH 03/23] untested odometry visualization --- src/main/java/frc/robot/subsystems/TurdSwerve.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/TurdSwerve.java b/src/main/java/frc/robot/subsystems/TurdSwerve.java index edbe55e..289b4cf 100644 --- a/src/main/java/frc/robot/subsystems/TurdSwerve.java +++ b/src/main/java/frc/robot/subsystems/TurdSwerve.java @@ -93,6 +93,7 @@ public void setRobotSpeeds(ChassisSpeeds chassisSpeeds) { public void periodic() { odometer.update(getGyro(), new SwerveModulePosition[] {leftPod.getPodPosition(), rightPod.getPodPosition()}); SmartDashboard.putNumber("pigeon", getGyro().getDegrees()); + field2d.setRobotPose(odometer.getPoseMeters()); } private String getFomattedPose() { @@ -103,7 +104,7 @@ private String getFomattedPose() { } public void addDashboardWidgets(ShuffleboardTab tab) { - // tab.add("Field", field2d).withPosition(0, 0).withSize(6, 4); + tab.add("Field", field2d).withPosition(0, 0).withSize(6, 4); tab.addString("Pose", this::getFomattedPose).withPosition(6, 2).withSize(2, 1); } } From 9cc9f54a126a44c5040613b7bac60a2afed39e37 Mon Sep 17 00:00:00 2001 From: "maxwell.lee.372007" Date: Fri, 15 Mar 2024 14:06:31 -0400 Subject: [PATCH 04/23] resetOdometry button --- src/main/java/frc/robot/subsystems/TurdSwerve.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/subsystems/TurdSwerve.java b/src/main/java/frc/robot/subsystems/TurdSwerve.java index 289b4cf..0bb7aff 100644 --- a/src/main/java/frc/robot/subsystems/TurdSwerve.java +++ b/src/main/java/frc/robot/subsystems/TurdSwerve.java @@ -61,6 +61,7 @@ public void resetPods() { rightPod.resetPod(); leftPod.setPID(azimuthP.getDouble(Constants.azimuthkP), azimuthI.getDouble(Constants.azimuthkI), azimuthD.getDouble(Constants.azimuthkD), azimuthIzone.getDouble(Constants.azimuthkIz), Constants.azimuthMaxOutput, ADMult.getDouble(Constants.azimuthDriveSpeedMultiplier)); rightPod.setPID(azimuthP.getDouble(Constants.azimuthkP), azimuthI.getDouble(Constants.azimuthkI), azimuthD.getDouble(Constants.azimuthkD), azimuthIzone.getDouble(Constants.azimuthkIz), Constants.azimuthMaxOutput, ADMult.getDouble(Constants.azimuthDriveSpeedMultiplier)); + resetOdometry(new Pose2d()); } public Rotation2d getGyro() { From 20b744a474e9121abcb66df24c788ef409a79ce2 Mon Sep 17 00:00:00 2001 From: "maxwell.lee.372007" Date: Fri, 15 Mar 2024 15:34:28 -0400 Subject: [PATCH 05/23] odometry field representation working from side of field --- src/main/java/frc/robot/subsystems/TurdSwerve.java | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/TurdSwerve.java b/src/main/java/frc/robot/subsystems/TurdSwerve.java index 0bb7aff..dc2a5e4 100644 --- a/src/main/java/frc/robot/subsystems/TurdSwerve.java +++ b/src/main/java/frc/robot/subsystems/TurdSwerve.java @@ -8,11 +8,14 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -52,7 +55,7 @@ public TurdSwerve() { } public void resetOdometry(Pose2d pose) { - odometer.resetPosition(getGyro(), new SwerveModulePosition[] {leftPod.getPodPosition(), rightPod.getPodPosition()}, pose); + odometer.resetPosition(new Rotation2d(Math.PI), new SwerveModulePosition[] {leftPod.getPodPosition(), rightPod.getPodPosition()}, pose); } public void resetPods() { @@ -61,7 +64,7 @@ public void resetPods() { rightPod.resetPod(); leftPod.setPID(azimuthP.getDouble(Constants.azimuthkP), azimuthI.getDouble(Constants.azimuthkI), azimuthD.getDouble(Constants.azimuthkD), azimuthIzone.getDouble(Constants.azimuthkIz), Constants.azimuthMaxOutput, ADMult.getDouble(Constants.azimuthDriveSpeedMultiplier)); rightPod.setPID(azimuthP.getDouble(Constants.azimuthkP), azimuthI.getDouble(Constants.azimuthkI), azimuthD.getDouble(Constants.azimuthkD), azimuthIzone.getDouble(Constants.azimuthkIz), Constants.azimuthMaxOutput, ADMult.getDouble(Constants.azimuthDriveSpeedMultiplier)); - resetOdometry(new Pose2d()); + resetOdometry(new Pose2d(new Translation2d(12.0, 4.2), new Rotation2d())); } public Rotation2d getGyro() { @@ -94,14 +97,14 @@ public void setRobotSpeeds(ChassisSpeeds chassisSpeeds) { public void periodic() { odometer.update(getGyro(), new SwerveModulePosition[] {leftPod.getPodPosition(), rightPod.getPodPosition()}); SmartDashboard.putNumber("pigeon", getGyro().getDegrees()); - field2d.setRobotPose(odometer.getPoseMeters()); + field2d.setRobotPose(odometer.getPoseMeters().transformBy(new Transform2d(new Translation2d(), new Rotation2d(-Math.PI/2.0)))); } private String getFomattedPose() { var pose = odometer.getPoseMeters(); return String.format( "(%.3f, %.3f) %.2f degrees", - pose.getX(), pose.getY(), pose.getRotation().getDegrees()); + pose.getX(), pose.getY(), pose.getRotation().plus(new Rotation2d(-Math.PI/2.0)).getDegrees()); } public void addDashboardWidgets(ShuffleboardTab tab) { From b74769edce690dce9eab31dfd84d1321a6a752be Mon Sep 17 00:00:00 2001 From: "maxwell.lee.372007" Date: Sat, 16 Mar 2024 22:11:32 -0400 Subject: [PATCH 06/23] untested driverstation based odom --- src/main/java/frc/robot/subsystems/TurdSwerve.java | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/TurdSwerve.java b/src/main/java/frc/robot/subsystems/TurdSwerve.java index dc2a5e4..3f79113 100644 --- a/src/main/java/frc/robot/subsystems/TurdSwerve.java +++ b/src/main/java/frc/robot/subsystems/TurdSwerve.java @@ -17,6 +17,8 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.Field2d; @@ -43,6 +45,7 @@ public class TurdSwerve extends SubsystemBase { private PIDController GyroPID = new PIDController(Constants.gyroP, Constants.gyroI, Constants.gyroD); public double targetAngle = 0; + private double odoAngleOffset = Math.PI * 0.5; private Rotation2d gyroResetAngle = new Rotation2d(); @@ -55,7 +58,8 @@ public TurdSwerve() { } public void resetOdometry(Pose2d pose) { - odometer.resetPosition(new Rotation2d(Math.PI), new SwerveModulePosition[] {leftPod.getPodPosition(), rightPod.getPodPosition()}, pose); + odoAngleOffset = DriverStation.getAlliance().get() == Alliance.Red ? Math.PI * 0.5 : Math.PI * 1.5; + odometer.resetPosition(new Rotation2d(odoAngleOffset), new SwerveModulePosition[] {leftPod.getPodPosition(), rightPod.getPodPosition()}, pose); } public void resetPods() { @@ -97,7 +101,7 @@ public void setRobotSpeeds(ChassisSpeeds chassisSpeeds) { public void periodic() { odometer.update(getGyro(), new SwerveModulePosition[] {leftPod.getPodPosition(), rightPod.getPodPosition()}); SmartDashboard.putNumber("pigeon", getGyro().getDegrees()); - field2d.setRobotPose(odometer.getPoseMeters().transformBy(new Transform2d(new Translation2d(), new Rotation2d(-Math.PI/2.0)))); + field2d.setRobotPose(odometer.getPoseMeters().transformBy(new Transform2d(new Translation2d(), new Rotation2d(odoAngleOffset + (Math.PI * 0.5))))); } private String getFomattedPose() { From f467180fb55a9bd438030421db8e92f5176b16b0 Mon Sep 17 00:00:00 2001 From: "maxwell.lee.372007" Date: Tue, 26 Mar 2024 12:54:17 -0400 Subject: [PATCH 07/23] reset zeroes button (RB + Y) --- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/RobotContainer.java | 3 ++- .../java/frc/robot/commands/TurdDrive.java | 19 ++++++++++++------- .../java/frc/robot/subsystems/TurdPod.java | 17 +++++++++++++++++ .../java/frc/robot/subsystems/TurdSwerve.java | 16 +++++++++++++--- 5 files changed, 46 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 76d7afb..cff29bb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,8 +27,8 @@ public final class Constants { public static final int pigeonID = 6; - public static final double leftAbsoluteEncoderOffset = -1.304;//absolute encoder reading at position - public static final double rightAbsoluteEncoderOffset = -0.865;// gears facing inwards: fwd/bck TODO: less janky alignment + public static final double leftAbsoluteEncoderOffset = 4.714347390458646;//absolute encoder reading at position + public static final double rightAbsoluteEncoderOffset = 5.420739537358221;// gears facing inwards: fwd/bck TODO: less janky alignment public static final boolean leftAzimuthInvert = false; public static final boolean rightAzimuthInvert = false; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 00a6c11..409ba40 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -33,7 +33,8 @@ public RobotContainer() { Supplier driverLeftJoystick = () -> new Translation2d(driver.getLeftX(), driver.getLeftY()); Supplier buttonStart = () -> driver.getStartButton(); Supplier DPAD = () -> driver.getPOV(); - swerve.setDefaultCommand(new TurdDrive(swerve, driverLeftJoystick, driverRightJoystick, buttonStart, DPAD)); + Supplier resetZeroes = () -> driver.getRightBumper() && driver.getYButton(); + swerve.setDefaultCommand(new TurdDrive(swerve, driverLeftJoystick, driverRightJoystick, buttonStart, DPAD, resetZeroes)); swerve.addDashboardWidgets(Odometry); } diff --git a/src/main/java/frc/robot/commands/TurdDrive.java b/src/main/java/frc/robot/commands/TurdDrive.java index 3516402..4052316 100644 --- a/src/main/java/frc/robot/commands/TurdDrive.java +++ b/src/main/java/frc/robot/commands/TurdDrive.java @@ -20,17 +20,17 @@ public class TurdDrive extends Command { TurdSwerve swerve; - Supplier joystickRight; - Supplier joystickLeft; - Supplier resetPods; + Supplier joystickRight, joystickLeft; + Supplier resetPods, resetZero; Supplier DPAD; Rotation2d rotation = new Rotation2d(); - public TurdDrive(TurdSwerve swerve, Supplier joystickLeft, Supplier joystickRight, Supplier resetPods, Supplier DPAD) { + public TurdDrive(TurdSwerve swerve, Supplier joystickLeft, Supplier joystickRight, Supplier resetPods, Supplier DPAD, Supplier resetZero) { this.swerve = swerve; this.joystickRight = joystickRight; this.joystickLeft = joystickLeft; this.resetPods = resetPods; + this.resetZero = resetZero; this.DPAD = DPAD; addRequirements(swerve); } @@ -42,18 +42,23 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (resetPods.get()) { - swerve.resetPods(); - } if (DPAD.get() != -1) { swerve.targetAngle = -Units.degreesToRadians(DPAD.get()); } + if (resetZero.get()){ + swerve.resetZero(); + swerve.stop(); + } else if (resetPods.get()) { + swerve.resetPods(); + swerve.stop(); + } else { boolean deadband = Math.abs(joystickRight.get().getX()) + Math.abs(joystickRight.get().getY()) < 0.1; double speedX = deadband ? 0 : -joystickRight.get().getX(); double speedY = deadband ? 0 : joystickRight.get().getY(); double speedOmega = Math.abs(joystickLeft.get().getX()) > 0.07 ? -joystickLeft.get().getX() * Math.abs(joystickLeft.get().getX()) : 0; ChassisSpeeds speeds = new ChassisSpeeds(speedX, speedY, speedOmega); swerve.setRobotSpeeds(speeds); + } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/TurdPod.java b/src/main/java/frc/robot/subsystems/TurdPod.java index 76ca926..de3f03a 100644 --- a/src/main/java/frc/robot/subsystems/TurdPod.java +++ b/src/main/java/frc/robot/subsystems/TurdPod.java @@ -14,6 +14,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.AnalogEncoder; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -66,6 +67,22 @@ public void resetPod() { driveEncoder.setPosition(0); azimuthEncoder.setPosition(getAbsoluteEncoder()); } + + public String getPod() { + return azimuth.getDeviceId() == Constants.leftAzimuthID ? "Left" : "Right"; + } + + public void resetZero() { + absoluteEncoderOffset = (absoluteEncoder.getAbsolutePosition() * 2*Math.PI); + SmartDashboard.putNumber((getPod() + " Encoder Offset"), absoluteEncoderOffset); + resetPod(); + } + + public void stop() { + azimuth.set(0); + drive.set(0); + } + public void setPID(double P, double I, double D, double IZone, double outputRange, double ADMult) { if (P != azimuthPID.getP()) {azimuthPID.setP(P);} if (I != azimuthPID.getI()) {azimuthPID.setI(I);} diff --git a/src/main/java/frc/robot/subsystems/TurdSwerve.java b/src/main/java/frc/robot/subsystems/TurdSwerve.java index 3f79113..70f0f4f 100644 --- a/src/main/java/frc/robot/subsystems/TurdSwerve.java +++ b/src/main/java/frc/robot/subsystems/TurdSwerve.java @@ -71,6 +71,16 @@ public void resetPods() { resetOdometry(new Pose2d(new Translation2d(12.0, 4.2), new Rotation2d())); } + public void resetZero() { + leftPod.resetZero(); + rightPod.resetZero(); + } + + public void stop() { + leftPod.stop(); + rightPod.stop(); + } + public Rotation2d getGyro() { return new Rotation2d(-gyro.getAngle()*Math.PI/180).minus(gyroResetAngle); } @@ -80,9 +90,9 @@ public void resetGyro() { targetAngle = 0; } - public void setLeftPod(SwerveModuleState state) { - leftPod.setPodState(state); - } + // public void setLeftPod(SwerveModuleState state) { + // leftPod.setPodState(state); + // } public void setRobotSpeeds(ChassisSpeeds chassisSpeeds) { boolean manualTurn = Math.abs(chassisSpeeds.omegaRadiansPerSecond) > 0.1; From 124335a5551a5574236959c3cbd3d789a41da2b5 Mon Sep 17 00:00:00 2001 From: "maxwell.lee.372007" Date: Tue, 26 Mar 2024 13:11:52 -0400 Subject: [PATCH 08/23] revert zero button (RB + X) --- src/main/java/frc/robot/RobotContainer.java | 7 ++++--- src/main/java/frc/robot/commands/TurdDrive.java | 8 ++++++-- src/main/java/frc/robot/subsystems/TurdPod.java | 5 +++++ src/main/java/frc/robot/subsystems/TurdSwerve.java | 7 ++++++- 4 files changed, 21 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 409ba40..322c939 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,6 +10,7 @@ import edu.wpi.first.math.kinematics.Odometry; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -31,12 +32,12 @@ public RobotContainer() { configureBindings(); Supplier driverRightJoystick = () -> new Translation2d(driver.getRightX(), driver.getRightY()); Supplier driverLeftJoystick = () -> new Translation2d(driver.getLeftX(), driver.getLeftY()); - Supplier buttonStart = () -> driver.getStartButton(); + Supplier resetPods = () -> driver.getStartButton(); Supplier DPAD = () -> driver.getPOV(); Supplier resetZeroes = () -> driver.getRightBumper() && driver.getYButton(); - swerve.setDefaultCommand(new TurdDrive(swerve, driverLeftJoystick, driverRightJoystick, buttonStart, DPAD, resetZeroes)); + Supplier revertZeroes = () -> driver.getRightBumper() && driver.getXButton(); + swerve.setDefaultCommand(new TurdDrive(swerve, driverLeftJoystick, driverRightJoystick, resetPods, DPAD, resetZeroes, revertZeroes)); swerve.addDashboardWidgets(Odometry); - } private void configureBindings() { diff --git a/src/main/java/frc/robot/commands/TurdDrive.java b/src/main/java/frc/robot/commands/TurdDrive.java index 4052316..8f6bafb 100644 --- a/src/main/java/frc/robot/commands/TurdDrive.java +++ b/src/main/java/frc/robot/commands/TurdDrive.java @@ -21,16 +21,17 @@ public class TurdDrive extends Command { TurdSwerve swerve; Supplier joystickRight, joystickLeft; - Supplier resetPods, resetZero; + Supplier resetPods, resetZero, revertZero; Supplier DPAD; Rotation2d rotation = new Rotation2d(); - public TurdDrive(TurdSwerve swerve, Supplier joystickLeft, Supplier joystickRight, Supplier resetPods, Supplier DPAD, Supplier resetZero) { + public TurdDrive(TurdSwerve swerve, Supplier joystickLeft, Supplier joystickRight, Supplier resetPods, Supplier DPAD, Supplier resetZero, Supplier revertZero) { this.swerve = swerve; this.joystickRight = joystickRight; this.joystickLeft = joystickLeft; this.resetPods = resetPods; this.resetZero = resetZero; + this.revertZero = revertZero; this.DPAD = DPAD; addRequirements(swerve); } @@ -51,6 +52,9 @@ public void execute() { } else if (resetPods.get()) { swerve.resetPods(); swerve.stop(); + } else if (revertZero.get()) { + swerve.revertZero(); + swerve.stop(); } else { boolean deadband = Math.abs(joystickRight.get().getX()) + Math.abs(joystickRight.get().getY()) < 0.1; double speedX = deadband ? 0 : -joystickRight.get().getX(); diff --git a/src/main/java/frc/robot/subsystems/TurdPod.java b/src/main/java/frc/robot/subsystems/TurdPod.java index de3f03a..f25709f 100644 --- a/src/main/java/frc/robot/subsystems/TurdPod.java +++ b/src/main/java/frc/robot/subsystems/TurdPod.java @@ -78,6 +78,11 @@ public void resetZero() { resetPod(); } + public void revertZero() { + absoluteEncoderOffset = azimuth.getDeviceId() == Constants.leftAzimuthID ? Constants.leftAbsoluteEncoderOffset : Constants.rightAbsoluteEncoderOffset; + resetPod(); + } + public void stop() { azimuth.set(0); drive.set(0); diff --git a/src/main/java/frc/robot/subsystems/TurdSwerve.java b/src/main/java/frc/robot/subsystems/TurdSwerve.java index 70f0f4f..8e34ebd 100644 --- a/src/main/java/frc/robot/subsystems/TurdSwerve.java +++ b/src/main/java/frc/robot/subsystems/TurdSwerve.java @@ -45,7 +45,7 @@ public class TurdSwerve extends SubsystemBase { private PIDController GyroPID = new PIDController(Constants.gyroP, Constants.gyroI, Constants.gyroD); public double targetAngle = 0; - private double odoAngleOffset = Math.PI * 0.5; + private double odoAngleOffset = Math.PI * 0.0; private Rotation2d gyroResetAngle = new Rotation2d(); @@ -76,6 +76,11 @@ public void resetZero() { rightPod.resetZero(); } + public void revertZero() { + leftPod.revertZero(); + rightPod.revertZero(); + } + public void stop() { leftPod.stop(); rightPod.stop(); From 662a7d241045bd90dae913ea76a30da6792bd3d4 Mon Sep 17 00:00:00 2001 From: "maxwell.lee.372007" Date: Tue, 26 Mar 2024 13:21:06 -0400 Subject: [PATCH 09/23] odometry angle fix --- src/main/java/frc/robot/commands/TurdDrive.java | 4 +++- src/main/java/frc/robot/subsystems/TurdSwerve.java | 6 +++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/TurdDrive.java b/src/main/java/frc/robot/commands/TurdDrive.java index 8f6bafb..11a066c 100644 --- a/src/main/java/frc/robot/commands/TurdDrive.java +++ b/src/main/java/frc/robot/commands/TurdDrive.java @@ -38,7 +38,9 @@ public TurdDrive(TurdSwerve swerve, Supplier joystickLeft, Suppli // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + swerve.resetGyro(); + } // Called every time the scheduler runs while the command is scheduled. @Override diff --git a/src/main/java/frc/robot/subsystems/TurdSwerve.java b/src/main/java/frc/robot/subsystems/TurdSwerve.java index 8e34ebd..bd1458e 100644 --- a/src/main/java/frc/robot/subsystems/TurdSwerve.java +++ b/src/main/java/frc/robot/subsystems/TurdSwerve.java @@ -68,7 +68,7 @@ public void resetPods() { rightPod.resetPod(); leftPod.setPID(azimuthP.getDouble(Constants.azimuthkP), azimuthI.getDouble(Constants.azimuthkI), azimuthD.getDouble(Constants.azimuthkD), azimuthIzone.getDouble(Constants.azimuthkIz), Constants.azimuthMaxOutput, ADMult.getDouble(Constants.azimuthDriveSpeedMultiplier)); rightPod.setPID(azimuthP.getDouble(Constants.azimuthkP), azimuthI.getDouble(Constants.azimuthkI), azimuthD.getDouble(Constants.azimuthkD), azimuthIzone.getDouble(Constants.azimuthkIz), Constants.azimuthMaxOutput, ADMult.getDouble(Constants.azimuthDriveSpeedMultiplier)); - resetOdometry(new Pose2d(new Translation2d(12.0, 4.2), new Rotation2d())); + resetOdometry(new Pose2d(new Translation2d(8.0, 4.2), new Rotation2d())); } public void resetZero() { @@ -116,14 +116,14 @@ public void setRobotSpeeds(ChassisSpeeds chassisSpeeds) { public void periodic() { odometer.update(getGyro(), new SwerveModulePosition[] {leftPod.getPodPosition(), rightPod.getPodPosition()}); SmartDashboard.putNumber("pigeon", getGyro().getDegrees()); - field2d.setRobotPose(odometer.getPoseMeters().transformBy(new Transform2d(new Translation2d(), new Rotation2d(odoAngleOffset + (Math.PI * 0.5))))); + field2d.setRobotPose(odometer.getPoseMeters().transformBy(new Transform2d(new Translation2d(), new Rotation2d(odoAngleOffset + Math.PI)))); } private String getFomattedPose() { var pose = odometer.getPoseMeters(); return String.format( "(%.3f, %.3f) %.2f degrees", - pose.getX(), pose.getY(), pose.getRotation().plus(new Rotation2d(-Math.PI/2.0)).getDegrees()); + pose.getX(), pose.getY(), pose.getRotation().plus(new Rotation2d(odoAngleOffset)).getDegrees()); } public void addDashboardWidgets(ShuffleboardTab tab) { From 21b2614926fb842c58bf3810083250b0adf1ce41 Mon Sep 17 00:00:00 2001 From: "maxwell.lee.372007" Date: Fri, 29 Mar 2024 00:15:26 -0400 Subject: [PATCH 10/23] turdswerve new pod --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index cff29bb..c8f1cd2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,7 +27,7 @@ public final class Constants { public static final int pigeonID = 6; - public static final double leftAbsoluteEncoderOffset = 4.714347390458646;//absolute encoder reading at position + public static final double leftAbsoluteEncoderOffset = 3.45;//absolute encoder reading at position public static final double rightAbsoluteEncoderOffset = 5.420739537358221;// gears facing inwards: fwd/bck TODO: less janky alignment public static final boolean leftAzimuthInvert = false; From fce39098d832d8b8e180fdcf06ea33326ec8f9ce Mon Sep 17 00:00:00 2001 From: "maxwell.lee.372007" Date: Fri, 29 Mar 2024 23:27:29 -0400 Subject: [PATCH 11/23] disable autoturn --- src/main/java/frc/robot/subsystems/TurdSwerve.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/TurdSwerve.java b/src/main/java/frc/robot/subsystems/TurdSwerve.java index bd1458e..388eef2 100644 --- a/src/main/java/frc/robot/subsystems/TurdSwerve.java +++ b/src/main/java/frc/robot/subsystems/TurdSwerve.java @@ -100,7 +100,7 @@ public void resetGyro() { // } public void setRobotSpeeds(ChassisSpeeds chassisSpeeds) { - boolean manualTurn = Math.abs(chassisSpeeds.omegaRadiansPerSecond) > 0.1; + boolean manualTurn = true;//Math.abs(chassisSpeeds.omegaRadiansPerSecond) > 0.1; chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, manualTurn ? chassisSpeeds.omegaRadiansPerSecond * 3.0 : GyroPID.calculate(getGyro().getRadians(), targetAngle), getGyro()); SwerveModuleState[] states = Constants.drivetrainKinematics.toSwerveModuleStates(chassisSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(states, Constants.podMaxSpeed); From b7fc2cc3ebf3deef00d9b49564276eb55566978d Mon Sep 17 00:00:00 2001 From: "maxwell.lee.372007" Date: Tue, 16 Apr 2024 15:31:11 -0400 Subject: [PATCH 12/23] speedycommand --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/RobotContainer.java | 10 ++--- .../frc/robot/commands/SpeedyCommand.java | 41 +++++++++++++++++++ .../java/frc/robot/commands/TurdDrive.java | 5 +-- .../java/frc/robot/subsystems/TurdPod.java | 5 ++- .../java/frc/robot/subsystems/TurdSwerve.java | 6 ++- 6 files changed, 56 insertions(+), 12 deletions(-) create mode 100644 src/main/java/frc/robot/commands/SpeedyCommand.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c8f1cd2..91e5371 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -41,6 +41,7 @@ public final class Constants { public static final int azimuthAmpLimit = 35; public static final int driveAmpLimit = 25; + public static final int driveTopAmpLimit = 90; public static final double podMaxSpeed = 1; public static final double azimuthMaxOutput = 0.25; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 322c939..e556532 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,22 +7,19 @@ import java.util.function.Supplier; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.Odometry; -import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.commands.SpeedyCommand; import frc.robot.commands.TurdDrive; -import frc.robot.subsystems.TurdPod; import frc.robot.subsystems.TurdSwerve; public class RobotContainer { public static final XboxController driver = new XboxController(Constants.driverPort); + public static final CommandXboxController driverCommand = new CommandXboxController(Constants.driverPort); // public static final TurdPod leftPod = new TurdPod(Constants.leftAzimuthID, Constants.leftDriveID, Constants.leftAbsoluteEncoderID, Constants.leftAzimuthInvert,Constants.rightAzimuthInvert, Constants.leftAbsoluteEncoderOffset); public static final TurdSwerve swerve = new TurdSwerve(); @@ -41,6 +38,7 @@ public RobotContainer() { } private void configureBindings() { + driverCommand.leftBumper().whileTrue(new SpeedyCommand(swerve)); // new JoystickButton(driver, 4).onTrue(swerve.resetPods()); } diff --git a/src/main/java/frc/robot/commands/SpeedyCommand.java b/src/main/java/frc/robot/commands/SpeedyCommand.java new file mode 100644 index 0000000..b31a460 --- /dev/null +++ b/src/main/java/frc/robot/commands/SpeedyCommand.java @@ -0,0 +1,41 @@ +// 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 frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.TurdSwerve; + +public class SpeedyCommand extends Command { + private TurdSwerve swerve; + /** Creates a new SpeedyCommand. */ + public SpeedyCommand(TurdSwerve swerve) { + this.swerve = swerve; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + swerve.setAmpLimit(Constants.driveTopAmpLimit); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + swerve.setAmpLimit(Constants.driveAmpLimit); + + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/TurdDrive.java b/src/main/java/frc/robot/commands/TurdDrive.java index 11a066c..cb92112 100644 --- a/src/main/java/frc/robot/commands/TurdDrive.java +++ b/src/main/java/frc/robot/commands/TurdDrive.java @@ -4,17 +4,13 @@ package frc.robot.commands; -import java.util.function.DoubleSupplier; import java.util.function.Supplier; 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.math.util.Units; -import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.TurdPod; import frc.robot.subsystems.TurdSwerve; public class TurdDrive extends Command { @@ -45,6 +41,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + if (DPAD.get() != -1) { swerve.targetAngle = -Units.degreesToRadians(DPAD.get()); } diff --git a/src/main/java/frc/robot/subsystems/TurdPod.java b/src/main/java/frc/robot/subsystems/TurdPod.java index f25709f..3c263e6 100644 --- a/src/main/java/frc/robot/subsystems/TurdPod.java +++ b/src/main/java/frc/robot/subsystems/TurdPod.java @@ -14,7 +14,6 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.AnalogEncoder; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -62,6 +61,10 @@ public TurdPod(int azimuthID, int driveID, int absoluteEncoderID, boolean azimut resetPod(); } + + public void setAmpLimit(int ampLimit) { + drive.setSmartCurrentLimit(ampLimit); + } public void resetPod() { driveEncoder.setPosition(0); diff --git a/src/main/java/frc/robot/subsystems/TurdSwerve.java b/src/main/java/frc/robot/subsystems/TurdSwerve.java index 388eef2..e1787e9 100644 --- a/src/main/java/frc/robot/subsystems/TurdSwerve.java +++ b/src/main/java/frc/robot/subsystems/TurdSwerve.java @@ -15,7 +15,6 @@ import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -57,6 +56,11 @@ public TurdSwerve() { // gyro.configAllSettings(new Pigeon2Configuration()); } + public void setAmpLimit(int ampLimit) { + leftPod.setAmpLimit(ampLimit); + rightPod.setAmpLimit(ampLimit); + } + public void resetOdometry(Pose2d pose) { odoAngleOffset = DriverStation.getAlliance().get() == Alliance.Red ? Math.PI * 0.5 : Math.PI * 1.5; odometer.resetPosition(new Rotation2d(odoAngleOffset), new SwerveModulePosition[] {leftPod.getPodPosition(), rightPod.getPodPosition()}, pose); From 16bdda1dcd6a81a05a46ecdfbf8c7ff955c6d657 Mon Sep 17 00:00:00 2001 From: "maxwell.lee.372007" Date: Tue, 16 Apr 2024 20:19:13 -0400 Subject: [PATCH 13/23] inc max speed to full, implement rate limiter of 1/3 sec to full speed --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/commands/SpeedyCommand.java | 2 ++ src/main/java/frc/robot/commands/TurdDrive.java | 4 +++- src/main/java/frc/robot/subsystems/TurdPod.java | 7 ++++++- src/main/java/frc/robot/subsystems/TurdSwerve.java | 10 +++++++++- 5 files changed, 21 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 91e5371..80fb5e0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -46,6 +46,7 @@ public final class Constants { public static final double podMaxSpeed = 1; public static final double azimuthMaxOutput = 0.25; public static double driveSpeedToPower = 0.25; + public static double driveTopSpeedToPower = 1.0; public static final double azimuthkP = 0.4; public static final double azimuthkI = 0.0; diff --git a/src/main/java/frc/robot/commands/SpeedyCommand.java b/src/main/java/frc/robot/commands/SpeedyCommand.java index b31a460..81261e4 100644 --- a/src/main/java/frc/robot/commands/SpeedyCommand.java +++ b/src/main/java/frc/robot/commands/SpeedyCommand.java @@ -20,6 +20,7 @@ public SpeedyCommand(TurdSwerve swerve) { @Override public void initialize() { swerve.setAmpLimit(Constants.driveTopAmpLimit); + swerve.setDriveSpeedtoPower(Constants.driveTopSpeedToPower); } // Called every time the scheduler runs while the command is scheduled. @@ -30,6 +31,7 @@ public void execute() {} @Override public void end(boolean interrupted) { swerve.setAmpLimit(Constants.driveAmpLimit); + swerve.setDriveSpeedtoPower(Constants.driveSpeedToPower); } diff --git a/src/main/java/frc/robot/commands/TurdDrive.java b/src/main/java/frc/robot/commands/TurdDrive.java index cb92112..895103e 100644 --- a/src/main/java/frc/robot/commands/TurdDrive.java +++ b/src/main/java/frc/robot/commands/TurdDrive.java @@ -55,9 +55,11 @@ public void execute() { swerve.revertZero(); swerve.stop(); } else { - boolean deadband = Math.abs(joystickRight.get().getX()) + Math.abs(joystickRight.get().getY()) < 0.1; + boolean deadband = Math.abs(joystickRight.get().getX()) + Math.abs(joystickRight.get().getY()) < 0.05; double speedX = deadband ? 0 : -joystickRight.get().getX(); double speedY = deadband ? 0 : joystickRight.get().getY(); + // double speedX = deadband ? 0 : 3.0 * Math.abs(joystickRight.get().getX()) * -joystickRight.get().getX(); + // double speedY = deadband ? 0 : 3.0 * Math.abs(joystickRight.get().getY()) * joystickRight.get().getY(); double speedOmega = Math.abs(joystickLeft.get().getX()) > 0.07 ? -joystickLeft.get().getX() * Math.abs(joystickLeft.get().getX()) : 0; ChassisSpeeds speeds = new ChassisSpeeds(speedX, speedY, speedOmega); swerve.setRobotSpeeds(speeds); diff --git a/src/main/java/frc/robot/subsystems/TurdPod.java b/src/main/java/frc/robot/subsystems/TurdPod.java index 3c263e6..fa70f90 100644 --- a/src/main/java/frc/robot/subsystems/TurdPod.java +++ b/src/main/java/frc/robot/subsystems/TurdPod.java @@ -31,6 +31,7 @@ public class TurdPod extends SubsystemBase { private double azimuthDriveSpeedMultiplier; private double speed = 0; private double absoluteEncoderOffset; + private double driveSpeedToPower = Constants.driveSpeedToPower; public TurdPod(int azimuthID, int driveID, int absoluteEncoderID, boolean azimuthInvert, boolean driveInvert, double absoluteEncoderOffset) { @@ -64,6 +65,10 @@ public TurdPod(int azimuthID, int driveID, int absoluteEncoderID, boolean azimut public void setAmpLimit(int ampLimit) { drive.setSmartCurrentLimit(ampLimit); + } + + public void setDriveSpeedtoPower(double driveSpeedToPower) { + this.driveSpeedToPower = driveSpeedToPower; } public void resetPod() { @@ -112,7 +117,7 @@ public SwerveModulePosition getPodPosition() { public void setPodState(SwerveModuleState state) { state = SwerveModuleState.optimize(state, new Rotation2d(azimuthEncoder.getPosition())); // does not account for rotations between 180 and 360? azimuthPID.setReference(state.angle.getRadians(), ControlType.kPosition); - speed = Math.abs(state.speedMetersPerSecond) < .01 ? 0 : state.speedMetersPerSecond * Constants.driveSpeedToPower; + speed = Math.abs(state.speedMetersPerSecond) < .01 ? 0 : state.speedMetersPerSecond * driveSpeedToPower; SmartDashboard.putNumber("state.angle.getRadians()", state.angle.getRadians()); double error = (state.angle.getRadians() - azimuthEncoder.getPosition()) % (2*Math.PI); diff --git a/src/main/java/frc/robot/subsystems/TurdSwerve.java b/src/main/java/frc/robot/subsystems/TurdSwerve.java index e1787e9..520e6fd 100644 --- a/src/main/java/frc/robot/subsystems/TurdSwerve.java +++ b/src/main/java/frc/robot/subsystems/TurdSwerve.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; @@ -26,6 +27,8 @@ import frc.robot.Constants; public class TurdSwerve extends SubsystemBase { + private final SlewRateLimiter xLimiter = new SlewRateLimiter(3.0); + private final SlewRateLimiter yLimiter = new SlewRateLimiter(3.0); private final Pigeon2 gyro = new Pigeon2(Constants.pigeonID); private final TurdPod leftPod = new TurdPod(Constants.leftAzimuthID, Constants.leftDriveID, Constants.leftAbsoluteEncoderID, Constants.leftAzimuthInvert, Constants.leftDriveInvert, Constants.leftAbsoluteEncoderOffset); private final TurdPod rightPod = new TurdPod(Constants.rightAzimuthID, Constants.rightDriveID, Constants.rightAbsoluteEncoderID, Constants.rightAzimuthInvert, Constants.rightDriveInvert, Constants.rightAbsoluteEncoderOffset); @@ -61,6 +64,11 @@ public void setAmpLimit(int ampLimit) { rightPod.setAmpLimit(ampLimit); } + public void setDriveSpeedtoPower(double driveSpeedToPower) { + leftPod.setDriveSpeedtoPower(driveSpeedToPower); + rightPod.setDriveSpeedtoPower(driveSpeedToPower); + } + public void resetOdometry(Pose2d pose) { odoAngleOffset = DriverStation.getAlliance().get() == Alliance.Red ? Math.PI * 0.5 : Math.PI * 1.5; odometer.resetPosition(new Rotation2d(odoAngleOffset), new SwerveModulePosition[] {leftPod.getPodPosition(), rightPod.getPodPosition()}, pose); @@ -105,7 +113,7 @@ public void resetGyro() { public void setRobotSpeeds(ChassisSpeeds chassisSpeeds) { boolean manualTurn = true;//Math.abs(chassisSpeeds.omegaRadiansPerSecond) > 0.1; - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, manualTurn ? chassisSpeeds.omegaRadiansPerSecond * 3.0 : GyroPID.calculate(getGyro().getRadians(), targetAngle), getGyro()); + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(xLimiter.calculate(chassisSpeeds.vxMetersPerSecond), yLimiter.calculate(chassisSpeeds.vyMetersPerSecond), manualTurn ? chassisSpeeds.omegaRadiansPerSecond * 3.0 : GyroPID.calculate(getGyro().getRadians(), targetAngle), getGyro()); SwerveModuleState[] states = Constants.drivetrainKinematics.toSwerveModuleStates(chassisSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(states, Constants.podMaxSpeed); if (manualTurn) { From b48f20978041e35cc5754a5cef768fb0057b8bc7 Mon Sep 17 00:00:00 2001 From: "maxwell.lee.372007" Date: Tue, 16 Apr 2024 20:23:42 -0400 Subject: [PATCH 14/23] drive motor ramp rate --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/subsystems/TurdPod.java | 2 ++ 2 files changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 80fb5e0..6314b57 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -67,5 +67,6 @@ public final class Constants { public static final SwerveDriveKinematics drivetrainKinematics = new SwerveDriveKinematics(robotCenter.minus(leftPodPosition), robotCenter.minus(rightPodPosition)); public static final int driverPort = 0; + public static final double driveMotorRampRate = 0.2; } diff --git a/src/main/java/frc/robot/subsystems/TurdPod.java b/src/main/java/frc/robot/subsystems/TurdPod.java index fa70f90..90ced53 100644 --- a/src/main/java/frc/robot/subsystems/TurdPod.java +++ b/src/main/java/frc/robot/subsystems/TurdPod.java @@ -58,6 +58,8 @@ public TurdPod(int azimuthID, int driveID, int absoluteEncoderID, boolean azimut azimuth.setIdleMode(Constants.azimuthMode); drive.setIdleMode(Constants.driveMode); + drive.setOpenLoopRampRate(Constants.driveMotorRampRate); + azimuthPID = azimuth.getPIDController(); resetPod(); From 89354c474579315847e3080a9b1b02aa329102fe Mon Sep 17 00:00:00 2001 From: "maxwell.lee.372007" Date: Wed, 17 Apr 2024 00:54:18 -0400 Subject: [PATCH 15/23] adjust absolute encoder offsets, all sensors seem to work normally --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6314b57..93a13ab 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -28,7 +28,7 @@ public final class Constants { public static final int pigeonID = 6; public static final double leftAbsoluteEncoderOffset = 3.45;//absolute encoder reading at position - public static final double rightAbsoluteEncoderOffset = 5.420739537358221;// gears facing inwards: fwd/bck TODO: less janky alignment + public static final double rightAbsoluteEncoderOffset = 5.420739537358221 - 4.705;// gears facing inwards: fwd/bck TODO: less janky alignment public static final boolean leftAzimuthInvert = false; public static final boolean rightAzimuthInvert = false; From 6408a16655308151df784633305c7272da8e2a82 Mon Sep 17 00:00:00 2001 From: "maxwell.lee.372007" Date: Mon, 20 May 2024 07:41:57 -0400 Subject: [PATCH 16/23] clean up commands --- src/main/java/frc/robot/RobotContainer.java | 20 +++++---- .../java/frc/robot/commands/ResetZeroes.java | 43 +++++++++++++++++++ .../java/frc/robot/commands/RevertZeroes.java | 43 +++++++++++++++++++ .../java/frc/robot/commands/TurdDrive.java | 18 +------- 4 files changed, 99 insertions(+), 25 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ResetZeroes.java create mode 100644 src/main/java/frc/robot/commands/RevertZeroes.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e556532..5d5b6f8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,14 +11,17 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.commands.ResetZeroes; +import frc.robot.commands.RevertZeroes; import frc.robot.commands.SpeedyCommand; import frc.robot.commands.TurdDrive; import frc.robot.subsystems.TurdSwerve; public class RobotContainer { - public static final XboxController driver = new XboxController(Constants.driverPort); + public static final XboxController driverRaw = new XboxController(Constants.driverPort); public static final CommandXboxController driverCommand = new CommandXboxController(Constants.driverPort); // public static final TurdPod leftPod = new TurdPod(Constants.leftAzimuthID, Constants.leftDriveID, Constants.leftAbsoluteEncoderID, Constants.leftAzimuthInvert,Constants.rightAzimuthInvert, Constants.leftAbsoluteEncoderOffset); public static final TurdSwerve swerve = new TurdSwerve(); @@ -27,19 +30,18 @@ public class RobotContainer { public RobotContainer() { final var Odometry = Shuffleboard.getTab("Odometry"); configureBindings(); - Supplier driverRightJoystick = () -> new Translation2d(driver.getRightX(), driver.getRightY()); - Supplier driverLeftJoystick = () -> new Translation2d(driver.getLeftX(), driver.getLeftY()); - Supplier resetPods = () -> driver.getStartButton(); - Supplier DPAD = () -> driver.getPOV(); - Supplier resetZeroes = () -> driver.getRightBumper() && driver.getYButton(); - Supplier revertZeroes = () -> driver.getRightBumper() && driver.getXButton(); - swerve.setDefaultCommand(new TurdDrive(swerve, driverLeftJoystick, driverRightJoystick, resetPods, DPAD, resetZeroes, revertZeroes)); + Supplier driverRightJoystick = () -> new Translation2d(driverRaw.getRightX(), driverRaw.getRightY()); + Supplier driverLeftJoystick = () -> new Translation2d(driverRaw.getLeftX(), driverRaw.getLeftY()); + Supplier DPAD = () -> driverRaw.getPOV(); + swerve.setDefaultCommand(new TurdDrive(swerve, driverLeftJoystick, driverRightJoystick, DPAD)); swerve.addDashboardWidgets(Odometry); } private void configureBindings() { driverCommand.leftBumper().whileTrue(new SpeedyCommand(swerve)); - // new JoystickButton(driver, 4).onTrue(swerve.resetPods()); + driverCommand.rightBumper().and(() -> driverRaw.getYButton()).whileTrue(new ResetZeroes(swerve)); + driverCommand.rightBumper().and(() -> driverRaw.getXButton()).whileTrue(new RevertZeroes(swerve)); + driverCommand.start().whileTrue(new InstantCommand(swerve::resetPods)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/ResetZeroes.java b/src/main/java/frc/robot/commands/ResetZeroes.java new file mode 100644 index 0000000..04ad65c --- /dev/null +++ b/src/main/java/frc/robot/commands/ResetZeroes.java @@ -0,0 +1,43 @@ +// 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 frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.TurdSwerve; + +public class ResetZeroes extends Command { + private TurdSwerve swerve; + /** Creates a new SpeedyCommand. */ + public ResetZeroes(TurdSwerve swerve) { + this.swerve = swerve; + addRequirements(swerve); + // 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() { + swerve.resetZero(); + swerve.stop(); + } + + // 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/frc/robot/commands/RevertZeroes.java b/src/main/java/frc/robot/commands/RevertZeroes.java new file mode 100644 index 0000000..4f4e7cd --- /dev/null +++ b/src/main/java/frc/robot/commands/RevertZeroes.java @@ -0,0 +1,43 @@ +// 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 frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.TurdSwerve; + +public class RevertZeroes extends Command { + private TurdSwerve swerve; + /** Creates a new SpeedyCommand. */ + public RevertZeroes(TurdSwerve swerve) { + this.swerve = swerve; + addRequirements(swerve); + // 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() { + swerve.revertZero(); + swerve.stop(); + } + + // 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/frc/robot/commands/TurdDrive.java b/src/main/java/frc/robot/commands/TurdDrive.java index 895103e..2ca045d 100644 --- a/src/main/java/frc/robot/commands/TurdDrive.java +++ b/src/main/java/frc/robot/commands/TurdDrive.java @@ -17,17 +17,13 @@ public class TurdDrive extends Command { TurdSwerve swerve; Supplier joystickRight, joystickLeft; - Supplier resetPods, resetZero, revertZero; Supplier DPAD; Rotation2d rotation = new Rotation2d(); - public TurdDrive(TurdSwerve swerve, Supplier joystickLeft, Supplier joystickRight, Supplier resetPods, Supplier DPAD, Supplier resetZero, Supplier revertZero) { + public TurdDrive(TurdSwerve swerve, Supplier joystickLeft, Supplier joystickRight, Supplier DPAD) { this.swerve = swerve; this.joystickRight = joystickRight; this.joystickLeft = joystickLeft; - this.resetPods = resetPods; - this.resetZero = resetZero; - this.revertZero = revertZero; this.DPAD = DPAD; addRequirements(swerve); } @@ -45,16 +41,6 @@ public void execute() { if (DPAD.get() != -1) { swerve.targetAngle = -Units.degreesToRadians(DPAD.get()); } - if (resetZero.get()){ - swerve.resetZero(); - swerve.stop(); - } else if (resetPods.get()) { - swerve.resetPods(); - swerve.stop(); - } else if (revertZero.get()) { - swerve.revertZero(); - swerve.stop(); - } else { boolean deadband = Math.abs(joystickRight.get().getX()) + Math.abs(joystickRight.get().getY()) < 0.05; double speedX = deadband ? 0 : -joystickRight.get().getX(); double speedY = deadband ? 0 : joystickRight.get().getY(); @@ -63,7 +49,7 @@ public void execute() { double speedOmega = Math.abs(joystickLeft.get().getX()) > 0.07 ? -joystickLeft.get().getX() * Math.abs(joystickLeft.get().getX()) : 0; ChassisSpeeds speeds = new ChassisSpeeds(speedX, speedY, speedOmega); swerve.setRobotSpeeds(speeds); - } + } // Called once the command ends or is interrupted. From 7ac127d7a569594ae19def053b920f52214caea5 Mon Sep 17 00:00:00 2001 From: "maxwell.lee.372007" Date: Mon, 20 May 2024 08:04:20 -0400 Subject: [PATCH 17/23] clean up speedycommand --- src/main/java/frc/robot/Constants.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 9 ++-- .../java/frc/robot/commands/ResetZeroes.java | 1 - .../java/frc/robot/commands/RevertZeroes.java | 1 - .../frc/robot/commands/SpeedyCommand.java | 43 ------------------- .../java/frc/robot/commands/TurdDrive.java | 15 +++++-- .../java/frc/robot/subsystems/TurdPod.java | 6 +-- .../java/frc/robot/subsystems/TurdSwerve.java | 13 +++--- 8 files changed, 28 insertions(+), 64 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/SpeedyCommand.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 93a13ab..f128cdb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -43,10 +43,10 @@ public final class Constants { public static final int driveAmpLimit = 25; public static final int driveTopAmpLimit = 90; + public static final double robotMaxSpeed = 0.25; public static final double podMaxSpeed = 1; public static final double azimuthMaxOutput = 0.25; - public static double driveSpeedToPower = 0.25; - public static double driveTopSpeedToPower = 1.0; + public static double driveSpeedToPower = 1.0; public static final double azimuthkP = 0.4; public static final double azimuthkI = 0.0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5d5b6f8..fe73462 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,7 +15,6 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.ResetZeroes; import frc.robot.commands.RevertZeroes; -import frc.robot.commands.SpeedyCommand; import frc.robot.commands.TurdDrive; import frc.robot.subsystems.TurdSwerve; @@ -33,15 +32,15 @@ public RobotContainer() { Supplier driverRightJoystick = () -> new Translation2d(driverRaw.getRightX(), driverRaw.getRightY()); Supplier driverLeftJoystick = () -> new Translation2d(driverRaw.getLeftX(), driverRaw.getLeftY()); Supplier DPAD = () -> driverRaw.getPOV(); - swerve.setDefaultCommand(new TurdDrive(swerve, driverLeftJoystick, driverRightJoystick, DPAD)); + Supplier robotMaxSpeed = (driverRaw.getLeftBumper() ? () -> 1.0 : () -> Constants.robotMaxSpeed); + swerve.setDefaultCommand(new TurdDrive(swerve, driverLeftJoystick, driverRightJoystick, DPAD, robotMaxSpeed)); swerve.addDashboardWidgets(Odometry); } private void configureBindings() { - driverCommand.leftBumper().whileTrue(new SpeedyCommand(swerve)); - driverCommand.rightBumper().and(() -> driverRaw.getYButton()).whileTrue(new ResetZeroes(swerve)); + driverCommand.rightBumper().and(() -> driverRaw.getYButton()).onTrue(new ResetZeroes(swerve)); driverCommand.rightBumper().and(() -> driverRaw.getXButton()).whileTrue(new RevertZeroes(swerve)); - driverCommand.start().whileTrue(new InstantCommand(swerve::resetPods)); + driverCommand.start().whileTrue(new InstantCommand(swerve::resetPods, swerve)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/ResetZeroes.java b/src/main/java/frc/robot/commands/ResetZeroes.java index 04ad65c..71a4914 100644 --- a/src/main/java/frc/robot/commands/ResetZeroes.java +++ b/src/main/java/frc/robot/commands/ResetZeroes.java @@ -10,7 +10,6 @@ public class ResetZeroes extends Command { private TurdSwerve swerve; - /** Creates a new SpeedyCommand. */ public ResetZeroes(TurdSwerve swerve) { this.swerve = swerve; addRequirements(swerve); diff --git a/src/main/java/frc/robot/commands/RevertZeroes.java b/src/main/java/frc/robot/commands/RevertZeroes.java index 4f4e7cd..06e1392 100644 --- a/src/main/java/frc/robot/commands/RevertZeroes.java +++ b/src/main/java/frc/robot/commands/RevertZeroes.java @@ -10,7 +10,6 @@ public class RevertZeroes extends Command { private TurdSwerve swerve; - /** Creates a new SpeedyCommand. */ public RevertZeroes(TurdSwerve swerve) { this.swerve = swerve; addRequirements(swerve); diff --git a/src/main/java/frc/robot/commands/SpeedyCommand.java b/src/main/java/frc/robot/commands/SpeedyCommand.java deleted file mode 100644 index 81261e4..0000000 --- a/src/main/java/frc/robot/commands/SpeedyCommand.java +++ /dev/null @@ -1,43 +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 frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.subsystems.TurdSwerve; - -public class SpeedyCommand extends Command { - private TurdSwerve swerve; - /** Creates a new SpeedyCommand. */ - public SpeedyCommand(TurdSwerve swerve) { - this.swerve = swerve; - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - swerve.setAmpLimit(Constants.driveTopAmpLimit); - swerve.setDriveSpeedtoPower(Constants.driveTopSpeedToPower); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - swerve.setAmpLimit(Constants.driveAmpLimit); - swerve.setDriveSpeedtoPower(Constants.driveSpeedToPower); - - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/TurdDrive.java b/src/main/java/frc/robot/commands/TurdDrive.java index 2ca045d..338d493 100644 --- a/src/main/java/frc/robot/commands/TurdDrive.java +++ b/src/main/java/frc/robot/commands/TurdDrive.java @@ -11,6 +11,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; import frc.robot.subsystems.TurdSwerve; public class TurdDrive extends Command { @@ -18,13 +19,15 @@ public class TurdDrive extends Command { TurdSwerve swerve; Supplier joystickRight, joystickLeft; Supplier DPAD; + Supplier robotMaxSpeed; Rotation2d rotation = new Rotation2d(); - public TurdDrive(TurdSwerve swerve, Supplier joystickLeft, Supplier joystickRight, Supplier DPAD) { + public TurdDrive(TurdSwerve swerve, Supplier joystickLeft, Supplier joystickRight, Supplier DPAD, Supplier robotMaxSpeed) { this.swerve = swerve; this.joystickRight = joystickRight; this.joystickLeft = joystickLeft; this.DPAD = DPAD; + this.robotMaxSpeed = robotMaxSpeed; addRequirements(swerve); } @@ -41,9 +44,15 @@ public void execute() { if (DPAD.get() != -1) { swerve.targetAngle = -Units.degreesToRadians(DPAD.get()); } + + if (robotMaxSpeed.get() > 0.95) { + swerve.setAmpLimit(Constants.driveTopAmpLimit); + } else { + swerve.setAmpLimit(Constants.driveAmpLimit); + } boolean deadband = Math.abs(joystickRight.get().getX()) + Math.abs(joystickRight.get().getY()) < 0.05; - double speedX = deadband ? 0 : -joystickRight.get().getX(); - double speedY = deadband ? 0 : joystickRight.get().getY(); + double speedX = deadband ? 0 : -joystickRight.get().getX() * robotMaxSpeed.get(); + double speedY = deadband ? 0 : joystickRight.get().getY() * robotMaxSpeed.get(); // double speedX = deadband ? 0 : 3.0 * Math.abs(joystickRight.get().getX()) * -joystickRight.get().getX(); // double speedY = deadband ? 0 : 3.0 * Math.abs(joystickRight.get().getY()) * joystickRight.get().getY(); double speedOmega = Math.abs(joystickLeft.get().getX()) > 0.07 ? -joystickLeft.get().getX() * Math.abs(joystickLeft.get().getX()) : 0; diff --git a/src/main/java/frc/robot/subsystems/TurdPod.java b/src/main/java/frc/robot/subsystems/TurdPod.java index 90ced53..924f3f4 100644 --- a/src/main/java/frc/robot/subsystems/TurdPod.java +++ b/src/main/java/frc/robot/subsystems/TurdPod.java @@ -69,9 +69,9 @@ public void setAmpLimit(int ampLimit) { drive.setSmartCurrentLimit(ampLimit); } - public void setDriveSpeedtoPower(double driveSpeedToPower) { - this.driveSpeedToPower = driveSpeedToPower; - } + // public void setDriveSpeedtoPower(double driveSpeedToPower) { + // this.driveSpeedToPower = driveSpeedToPower; + // } public void resetPod() { driveEncoder.setPosition(0); diff --git a/src/main/java/frc/robot/subsystems/TurdSwerve.java b/src/main/java/frc/robot/subsystems/TurdSwerve.java index 520e6fd..cf9dda3 100644 --- a/src/main/java/frc/robot/subsystems/TurdSwerve.java +++ b/src/main/java/frc/robot/subsystems/TurdSwerve.java @@ -27,8 +27,8 @@ import frc.robot.Constants; public class TurdSwerve extends SubsystemBase { - private final SlewRateLimiter xLimiter = new SlewRateLimiter(3.0); - private final SlewRateLimiter yLimiter = new SlewRateLimiter(3.0); + private final SlewRateLimiter xLimiter = new SlewRateLimiter(0.75); + private final SlewRateLimiter yLimiter = new SlewRateLimiter(0.75); private final Pigeon2 gyro = new Pigeon2(Constants.pigeonID); private final TurdPod leftPod = new TurdPod(Constants.leftAzimuthID, Constants.leftDriveID, Constants.leftAbsoluteEncoderID, Constants.leftAzimuthInvert, Constants.leftDriveInvert, Constants.leftAbsoluteEncoderOffset); private final TurdPod rightPod = new TurdPod(Constants.rightAzimuthID, Constants.rightDriveID, Constants.rightAbsoluteEncoderID, Constants.rightAzimuthInvert, Constants.rightDriveInvert, Constants.rightAbsoluteEncoderOffset); @@ -64,10 +64,10 @@ public void setAmpLimit(int ampLimit) { rightPod.setAmpLimit(ampLimit); } - public void setDriveSpeedtoPower(double driveSpeedToPower) { - leftPod.setDriveSpeedtoPower(driveSpeedToPower); - rightPod.setDriveSpeedtoPower(driveSpeedToPower); - } + // public void setDriveSpeedtoPower(double driveSpeedToPower) { + // leftPod.setDriveSpeedtoPower(driveSpeedToPower); + // rightPod.setDriveSpeedtoPower(driveSpeedToPower); + // } public void resetOdometry(Pose2d pose) { odoAngleOffset = DriverStation.getAlliance().get() == Alliance.Red ? Math.PI * 0.5 : Math.PI * 1.5; @@ -113,6 +113,7 @@ public void resetGyro() { public void setRobotSpeeds(ChassisSpeeds chassisSpeeds) { boolean manualTurn = true;//Math.abs(chassisSpeeds.omegaRadiansPerSecond) > 0.1; + // chassisSpeeds = chassisSpeeds.times(robotMaxSpeed); chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(xLimiter.calculate(chassisSpeeds.vxMetersPerSecond), yLimiter.calculate(chassisSpeeds.vyMetersPerSecond), manualTurn ? chassisSpeeds.omegaRadiansPerSecond * 3.0 : GyroPID.calculate(getGyro().getRadians(), targetAngle), getGyro()); SwerveModuleState[] states = Constants.drivetrainKinematics.toSwerveModuleStates(chassisSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(states, Constants.podMaxSpeed); From dfb47c8be361985f792b3b55473b92970460b279 Mon Sep 17 00:00:00 2001 From: "maxwell.lee.372007" Date: Mon, 20 May 2024 08:18:43 -0400 Subject: [PATCH 18/23] clean up constants --- src/main/java/frc/robot/RobotContainer.java | 1 + .../java/frc/robot/commands/ResetZeroes.java | 2 +- .../java/frc/robot/commands/RevertZeroes.java | 2 +- .../java/frc/robot/commands/TurdDrive.java | 2 +- .../java/frc/robot/constants/Constants.java | 48 +++++++++++++++++++ .../RobotMap.java} | 46 +++++------------- .../java/frc/robot/subsystems/TurdPod.java | 14 +++--- .../java/frc/robot/subsystems/TurdSwerve.java | 13 ++--- 8 files changed, 79 insertions(+), 49 deletions(-) create mode 100644 src/main/java/frc/robot/constants/Constants.java rename src/main/java/frc/robot/{Constants.java => constants/RobotMap.java} (59%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fe73462..4337535 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -16,6 +16,7 @@ import frc.robot.commands.ResetZeroes; import frc.robot.commands.RevertZeroes; import frc.robot.commands.TurdDrive; +import frc.robot.constants.Constants; import frc.robot.subsystems.TurdSwerve; public class RobotContainer { diff --git a/src/main/java/frc/robot/commands/ResetZeroes.java b/src/main/java/frc/robot/commands/ResetZeroes.java index 71a4914..2c6ebc8 100644 --- a/src/main/java/frc/robot/commands/ResetZeroes.java +++ b/src/main/java/frc/robot/commands/ResetZeroes.java @@ -5,7 +5,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; +import frc.robot.constants.Constants; import frc.robot.subsystems.TurdSwerve; public class ResetZeroes extends Command { diff --git a/src/main/java/frc/robot/commands/RevertZeroes.java b/src/main/java/frc/robot/commands/RevertZeroes.java index 06e1392..4ab5d61 100644 --- a/src/main/java/frc/robot/commands/RevertZeroes.java +++ b/src/main/java/frc/robot/commands/RevertZeroes.java @@ -5,7 +5,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; +import frc.robot.constants.Constants; import frc.robot.subsystems.TurdSwerve; public class RevertZeroes extends Command { diff --git a/src/main/java/frc/robot/commands/TurdDrive.java b/src/main/java/frc/robot/commands/TurdDrive.java index 338d493..47a28e4 100644 --- a/src/main/java/frc/robot/commands/TurdDrive.java +++ b/src/main/java/frc/robot/commands/TurdDrive.java @@ -11,7 +11,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; +import frc.robot.constants.Constants; import frc.robot.subsystems.TurdSwerve; public class TurdDrive extends Command { diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java new file mode 100644 index 0000000..899ca27 --- /dev/null +++ b/src/main/java/frc/robot/constants/Constants.java @@ -0,0 +1,48 @@ +// 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 frc.robot.constants; + +import java.util.HashMap; +import java.util.Hashtable; + +import com.revrobotics.CANSparkBase.IdleMode; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.util.Units; + +/** Add your docs here. */ +public final class Constants { + public static final int driverPort = 0; + + public static final double gyroP = 2; + public static final double gyroI = 0.0; + public static final double gyroD = 0.00; + + public static final double robotMaxSpeed = 0.25; + public static final double podMaxSpeed = 1; + + + // Azimuth Settings + public static final IdleMode azimuthMode = IdleMode.kBrake; + + public static final int azimuthAmpLimit = 35; + public static final double azimuthMaxOutput = 0.25; + + public static final double azimuthkP = 0.4; + public static final double azimuthkI = 0.0; + public static final double azimuthkD = 0.003; + public static final double azimuthkIz = 0; + public static final double azimuthDriveSpeedMultiplier = 0;//0.5; + + // Drive Settings + public static final IdleMode driveMode = IdleMode.kCoast; + + public static final int driveAmpLimit = 25; + public static final int driveTopAmpLimit = 90; + public static final double driveSpeedToPower = 1.0; + public static final double driveMotorRampRate = 0.2; +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/constants/RobotMap.java similarity index 59% rename from src/main/java/frc/robot/Constants.java rename to src/main/java/frc/robot/constants/RobotMap.java index f128cdb..7579a7b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/constants/RobotMap.java @@ -2,7 +2,7 @@ // 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 frc.robot; +package frc.robot.constants; import java.util.HashMap; import java.util.Hashtable; @@ -14,8 +14,8 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.util.Units; -/** Add your docs here. */ -public final class Constants { +/** CAN ID, Invert, Pod Positions, Offsets, Conversion Rates */ +public final class RobotMap { public static final int leftAzimuthID = 2; public static final int rightAzimuthID = 3; @@ -27,46 +27,24 @@ public final class Constants { public static final int pigeonID = 6; - public static final double leftAbsoluteEncoderOffset = 3.45;//absolute encoder reading at position - public static final double rightAbsoluteEncoderOffset = 5.420739537358221 - 4.705;// gears facing inwards: fwd/bck TODO: less janky alignment - public static final boolean leftAzimuthInvert = false; public static final boolean rightAzimuthInvert = false; public static final boolean leftDriveInvert = false; public static final boolean rightDriveInvert = true; - public static final double azimuthRadiansPerRotation = 2*Math.PI*15/33; - public static final double driveMetersPerRotation = Units.inchesToMeters(2) * Math.PI * 33 / 45 / 2; - public static final double absoluteEncoderRadiansPerRotation = 2*Math.PI; - - public static final int azimuthAmpLimit = 35; - public static final int driveAmpLimit = 25; - public static final int driveTopAmpLimit = 90; - - public static final double robotMaxSpeed = 0.25; - public static final double podMaxSpeed = 1; - public static final double azimuthMaxOutput = 0.25; - public static double driveSpeedToPower = 1.0; - - public static final double azimuthkP = 0.4; - public static final double azimuthkI = 0.0; - public static final double azimuthkD = 0.003; - public static final double azimuthkIz = 0; - public static final double azimuthDriveSpeedMultiplier = 0;//0.5; - - public static final IdleMode azimuthMode = IdleMode.kBrake; - public static final IdleMode driveMode = IdleMode.kCoast; - - public static final double gyroP = 2; - public static final double gyroI = 0.0; - public static final double gyroD = 0.00; - public static final Translation2d robotCenter = new Translation2d(0, 0); // serves as "center of robot for calculations; robot will turn about this point public static final Translation2d leftPodPosition = new Translation2d(-Units.inchesToMeters(5.5), -Units.inchesToMeters(5.5)); // units in meters public static final Translation2d rightPodPosition = new Translation2d(Units.inchesToMeters(5.5), Units.inchesToMeters(5.5)); public static final SwerveDriveKinematics drivetrainKinematics = new SwerveDriveKinematics(robotCenter.minus(leftPodPosition), robotCenter.minus(rightPodPosition)); - public static final int driverPort = 0; - public static final double driveMotorRampRate = 0.2; + public static final double leftAbsoluteEncoderOffset = 3.45;//absolute encoder reading at position + public static final double rightAbsoluteEncoderOffset = 5.420739537358221 - 4.705;// gears facing inwards: fwd/bck TODO: less janky alignment + + public static final double azimuthRadiansPerMotorRotation = 2*Math.PI*15/33; + public static final double driveMetersPerMotorRotation = Units.inchesToMeters(2) * Math.PI * 33 / 45 / 2; + public static final double absoluteRadiansPerEncoderRotation = 2*Math.PI; + + + } diff --git a/src/main/java/frc/robot/subsystems/TurdPod.java b/src/main/java/frc/robot/subsystems/TurdPod.java index 924f3f4..fa0fa2e 100644 --- a/src/main/java/frc/robot/subsystems/TurdPod.java +++ b/src/main/java/frc/robot/subsystems/TurdPod.java @@ -16,7 +16,9 @@ import edu.wpi.first.wpilibj.AnalogEncoder; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.constants.Constants; +import frc.robot.constants.RobotMap; public class TurdPod extends SubsystemBase { @@ -45,9 +47,9 @@ public TurdPod(int azimuthID, int driveID, int absoluteEncoderID, boolean azimut azimuth.setInverted(azimuthInvert); drive.setInverted(driveInvert); - driveEncoder.setPositionConversionFactor(Constants.driveMetersPerRotation); - azimuthEncoder.setPositionConversionFactor(Constants.azimuthRadiansPerRotation); - absoluteEncoder.setDistancePerRotation(Constants.absoluteEncoderRadiansPerRotation); + driveEncoder.setPositionConversionFactor(RobotMap.driveMetersPerMotorRotation); + azimuthEncoder.setPositionConversionFactor(RobotMap.azimuthRadiansPerMotorRotation); + absoluteEncoder.setDistancePerRotation(RobotMap.absoluteRadiansPerEncoderRotation); // absoluteEncoder.setPositionOffset(absoluteEncoderOffset); this.absoluteEncoderOffset = absoluteEncoderOffset; @@ -79,7 +81,7 @@ public void resetPod() { } public String getPod() { - return azimuth.getDeviceId() == Constants.leftAzimuthID ? "Left" : "Right"; + return azimuth.getDeviceId() == RobotMap.leftAzimuthID ? "Left" : "Right"; } public void resetZero() { @@ -89,7 +91,7 @@ public void resetZero() { } public void revertZero() { - absoluteEncoderOffset = azimuth.getDeviceId() == Constants.leftAzimuthID ? Constants.leftAbsoluteEncoderOffset : Constants.rightAbsoluteEncoderOffset; + absoluteEncoderOffset = azimuth.getDeviceId() == RobotMap.leftAzimuthID ? RobotMap.leftAbsoluteEncoderOffset : RobotMap.rightAbsoluteEncoderOffset; resetPod(); } diff --git a/src/main/java/frc/robot/subsystems/TurdSwerve.java b/src/main/java/frc/robot/subsystems/TurdSwerve.java index cf9dda3..836e8d5 100644 --- a/src/main/java/frc/robot/subsystems/TurdSwerve.java +++ b/src/main/java/frc/robot/subsystems/TurdSwerve.java @@ -24,15 +24,16 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; +import frc.robot.constants.Constants; +import frc.robot.constants.RobotMap; public class TurdSwerve extends SubsystemBase { private final SlewRateLimiter xLimiter = new SlewRateLimiter(0.75); private final SlewRateLimiter yLimiter = new SlewRateLimiter(0.75); - private final Pigeon2 gyro = new Pigeon2(Constants.pigeonID); - private final TurdPod leftPod = new TurdPod(Constants.leftAzimuthID, Constants.leftDriveID, Constants.leftAbsoluteEncoderID, Constants.leftAzimuthInvert, Constants.leftDriveInvert, Constants.leftAbsoluteEncoderOffset); - private final TurdPod rightPod = new TurdPod(Constants.rightAzimuthID, Constants.rightDriveID, Constants.rightAbsoluteEncoderID, Constants.rightAzimuthInvert, Constants.rightDriveInvert, Constants.rightAbsoluteEncoderOffset); - private final SwerveDriveOdometry odometer = new SwerveDriveOdometry(Constants.drivetrainKinematics, + private final Pigeon2 gyro = new Pigeon2(RobotMap.pigeonID); + private final TurdPod leftPod = new TurdPod(RobotMap.leftAzimuthID, RobotMap.leftDriveID, RobotMap.leftAbsoluteEncoderID, RobotMap.leftAzimuthInvert, RobotMap.leftDriveInvert, RobotMap.leftAbsoluteEncoderOffset); + private final TurdPod rightPod = new TurdPod(RobotMap.rightAzimuthID, RobotMap.rightDriveID, RobotMap.rightAbsoluteEncoderID, RobotMap.rightAzimuthInvert, RobotMap.rightDriveInvert, RobotMap.rightAbsoluteEncoderOffset); + private final SwerveDriveOdometry odometer = new SwerveDriveOdometry(RobotMap.drivetrainKinematics, new Rotation2d(0), new SwerveModulePosition[] { leftPod.getPodPosition(), rightPod.getPodPosition() @@ -115,7 +116,7 @@ public void setRobotSpeeds(ChassisSpeeds chassisSpeeds) { boolean manualTurn = true;//Math.abs(chassisSpeeds.omegaRadiansPerSecond) > 0.1; // chassisSpeeds = chassisSpeeds.times(robotMaxSpeed); chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(xLimiter.calculate(chassisSpeeds.vxMetersPerSecond), yLimiter.calculate(chassisSpeeds.vyMetersPerSecond), manualTurn ? chassisSpeeds.omegaRadiansPerSecond * 3.0 : GyroPID.calculate(getGyro().getRadians(), targetAngle), getGyro()); - SwerveModuleState[] states = Constants.drivetrainKinematics.toSwerveModuleStates(chassisSpeeds); + SwerveModuleState[] states = RobotMap.drivetrainKinematics.toSwerveModuleStates(chassisSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(states, Constants.podMaxSpeed); if (manualTurn) { targetAngle = getGyro().getRadians() + (chassisSpeeds.omegaRadiansPerSecond / 2.0); From 6cb256f44efe4efd4e890dead6600b004fa0919b Mon Sep 17 00:00:00 2001 From: "maxwell.lee.372007" Date: Mon, 20 May 2024 22:39:56 -0400 Subject: [PATCH 19/23] rearrange constants --- .../java/frc/robot/commands/ResetZeroes.java | 1 - .../java/frc/robot/commands/RevertZeroes.java | 1 - .../java/frc/robot/constants/Constants.java | 17 +++++++---------- src/main/java/frc/robot/constants/RobotMap.java | 6 ------ src/main/java/frc/robot/subsystems/TurdPod.java | 1 - 5 files changed, 7 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/commands/ResetZeroes.java b/src/main/java/frc/robot/commands/ResetZeroes.java index 2c6ebc8..d585a81 100644 --- a/src/main/java/frc/robot/commands/ResetZeroes.java +++ b/src/main/java/frc/robot/commands/ResetZeroes.java @@ -5,7 +5,6 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.Constants; import frc.robot.subsystems.TurdSwerve; public class ResetZeroes extends Command { diff --git a/src/main/java/frc/robot/commands/RevertZeroes.java b/src/main/java/frc/robot/commands/RevertZeroes.java index 4ab5d61..b4b724e 100644 --- a/src/main/java/frc/robot/commands/RevertZeroes.java +++ b/src/main/java/frc/robot/commands/RevertZeroes.java @@ -5,7 +5,6 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.Constants; import frc.robot.subsystems.TurdSwerve; public class RevertZeroes extends Command { diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index 899ca27..16656f1 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -4,27 +4,19 @@ package frc.robot.constants; -import java.util.HashMap; -import java.util.Hashtable; - import com.revrobotics.CANSparkBase.IdleMode; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.util.Units; /** Add your docs here. */ public final class Constants { public static final int driverPort = 0; + public static final double robotMaxSpeed = 0.25; + public static final double gyroP = 2; public static final double gyroI = 0.0; public static final double gyroD = 0.00; - public static final double robotMaxSpeed = 0.25; - public static final double podMaxSpeed = 1; - // Azimuth Settings public static final IdleMode azimuthMode = IdleMode.kBrake; @@ -36,8 +28,10 @@ public final class Constants { public static final double azimuthkI = 0.0; public static final double azimuthkD = 0.003; public static final double azimuthkIz = 0; + public static final double azimuthDriveSpeedMultiplier = 0;//0.5; + // Drive Settings public static final IdleMode driveMode = IdleMode.kCoast; @@ -45,4 +39,7 @@ public final class Constants { public static final int driveTopAmpLimit = 90; public static final double driveSpeedToPower = 1.0; public static final double driveMotorRampRate = 0.2; + + public static final double podMaxSpeed = 1; + } diff --git a/src/main/java/frc/robot/constants/RobotMap.java b/src/main/java/frc/robot/constants/RobotMap.java index 7579a7b..d659bca 100644 --- a/src/main/java/frc/robot/constants/RobotMap.java +++ b/src/main/java/frc/robot/constants/RobotMap.java @@ -4,12 +4,6 @@ package frc.robot.constants; -import java.util.HashMap; -import java.util.Hashtable; - -import com.revrobotics.CANSparkBase.IdleMode; - -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.util.Units; diff --git a/src/main/java/frc/robot/subsystems/TurdPod.java b/src/main/java/frc/robot/subsystems/TurdPod.java index fa0fa2e..d6ebf37 100644 --- a/src/main/java/frc/robot/subsystems/TurdPod.java +++ b/src/main/java/frc/robot/subsystems/TurdPod.java @@ -16,7 +16,6 @@ import edu.wpi.first.wpilibj.AnalogEncoder; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Robot; import frc.robot.constants.Constants; import frc.robot.constants.RobotMap; From e26cd86d195812e8f36df932f85140d61747fe01 Mon Sep 17 00:00:00 2001 From: "maxwell.lee.372007" Date: Sat, 1 Jun 2024 10:13:16 -0400 Subject: [PATCH 20/23] faster gears --- src/main/java/frc/robot/constants/RobotMap.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/constants/RobotMap.java b/src/main/java/frc/robot/constants/RobotMap.java index d659bca..7609a35 100644 --- a/src/main/java/frc/robot/constants/RobotMap.java +++ b/src/main/java/frc/robot/constants/RobotMap.java @@ -31,11 +31,11 @@ public final class RobotMap { public static final Translation2d rightPodPosition = new Translation2d(Units.inchesToMeters(5.5), Units.inchesToMeters(5.5)); public static final SwerveDriveKinematics drivetrainKinematics = new SwerveDriveKinematics(robotCenter.minus(leftPodPosition), robotCenter.minus(rightPodPosition)); - public static final double leftAbsoluteEncoderOffset = 3.45;//absolute encoder reading at position - public static final double rightAbsoluteEncoderOffset = 5.420739537358221 - 4.705;// gears facing inwards: fwd/bck TODO: less janky alignment + public static final double leftAbsoluteEncoderOffset = 4.731349179724511;//absolute encoder reading at position + public static final double rightAbsoluteEncoderOffset = 0.73436864196419;// gears facing inwards: fwd/bck TODO: less janky alignment public static final double azimuthRadiansPerMotorRotation = 2*Math.PI*15/33; - public static final double driveMetersPerMotorRotation = Units.inchesToMeters(2) * Math.PI * 33 / 45 / 2; + public static final double driveMetersPerMotorRotation = Units.inchesToMeters(2) * Math.PI * 33 / 45 / 15/13; public static final double absoluteRadiansPerEncoderRotation = 2*Math.PI; From 2e6e4e31cd40bbe53543afeab3aeaeceffc0ddaf Mon Sep 17 00:00:00 2001 From: "maxwell.lee.372007" Date: Sat, 1 Jun 2024 14:29:21 -0400 Subject: [PATCH 21/23] fixed boost --- src/main/java/frc/robot/RobotContainer.java | 7 +++---- src/main/java/frc/robot/commands/TurdDrive.java | 16 ++++++++++------ 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4337535..d4b9633 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -33,14 +33,13 @@ public RobotContainer() { Supplier driverRightJoystick = () -> new Translation2d(driverRaw.getRightX(), driverRaw.getRightY()); Supplier driverLeftJoystick = () -> new Translation2d(driverRaw.getLeftX(), driverRaw.getLeftY()); Supplier DPAD = () -> driverRaw.getPOV(); - Supplier robotMaxSpeed = (driverRaw.getLeftBumper() ? () -> 1.0 : () -> Constants.robotMaxSpeed); - swerve.setDefaultCommand(new TurdDrive(swerve, driverLeftJoystick, driverRightJoystick, DPAD, robotMaxSpeed)); + swerve.setDefaultCommand(new TurdDrive(swerve, driverLeftJoystick, driverRightJoystick, DPAD, driverRaw::getLeftBumper)); swerve.addDashboardWidgets(Odometry); } private void configureBindings() { - driverCommand.rightBumper().and(() -> driverRaw.getYButton()).onTrue(new ResetZeroes(swerve)); - driverCommand.rightBumper().and(() -> driverRaw.getXButton()).whileTrue(new RevertZeroes(swerve)); + driverCommand.rightBumper().and(driverRaw::getYButton).onTrue(new ResetZeroes(swerve)); + driverCommand.rightBumper().and(driverRaw::getXButton).whileTrue(new RevertZeroes(swerve)); driverCommand.start().whileTrue(new InstantCommand(swerve::resetPods, swerve)); } diff --git a/src/main/java/frc/robot/commands/TurdDrive.java b/src/main/java/frc/robot/commands/TurdDrive.java index 47a28e4..cbc544b 100644 --- a/src/main/java/frc/robot/commands/TurdDrive.java +++ b/src/main/java/frc/robot/commands/TurdDrive.java @@ -12,6 +12,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.Constants; +import frc.robot.constants.RobotMap; import frc.robot.subsystems.TurdSwerve; public class TurdDrive extends Command { @@ -19,15 +20,16 @@ public class TurdDrive extends Command { TurdSwerve swerve; Supplier joystickRight, joystickLeft; Supplier DPAD; - Supplier robotMaxSpeed; + Supplier boost; Rotation2d rotation = new Rotation2d(); + double maxSpeed = Constants.robotMaxSpeed; - public TurdDrive(TurdSwerve swerve, Supplier joystickLeft, Supplier joystickRight, Supplier DPAD, Supplier robotMaxSpeed) { + public TurdDrive(TurdSwerve swerve, Supplier joystickLeft, Supplier joystickRight, Supplier DPAD, Supplier boost) { this.swerve = swerve; this.joystickRight = joystickRight; this.joystickLeft = joystickLeft; this.DPAD = DPAD; - this.robotMaxSpeed = robotMaxSpeed; + this.boost = boost; addRequirements(swerve); } @@ -45,14 +47,16 @@ public void execute() { swerve.targetAngle = -Units.degreesToRadians(DPAD.get()); } - if (robotMaxSpeed.get() > 0.95) { + if (boost.get()) { swerve.setAmpLimit(Constants.driveTopAmpLimit); + maxSpeed = 1; } else { swerve.setAmpLimit(Constants.driveAmpLimit); + maxSpeed = Constants.robotMaxSpeed; } boolean deadband = Math.abs(joystickRight.get().getX()) + Math.abs(joystickRight.get().getY()) < 0.05; - double speedX = deadband ? 0 : -joystickRight.get().getX() * robotMaxSpeed.get(); - double speedY = deadband ? 0 : joystickRight.get().getY() * robotMaxSpeed.get(); + double speedX = deadband ? 0 : -joystickRight.get().getX() * maxSpeed; + double speedY = deadband ? 0 : joystickRight.get().getY() * maxSpeed; // double speedX = deadband ? 0 : 3.0 * Math.abs(joystickRight.get().getX()) * -joystickRight.get().getX(); // double speedY = deadband ? 0 : 3.0 * Math.abs(joystickRight.get().getY()) * joystickRight.get().getY(); double speedOmega = Math.abs(joystickLeft.get().getX()) > 0.07 ? -joystickLeft.get().getX() * Math.abs(joystickLeft.get().getX()) : 0; From 46e1cafc98725ab0cc9a18bf9520a794d79fc58b Mon Sep 17 00:00:00 2001 From: "maxwell.lee.372007" Date: Sat, 8 Jun 2024 11:46:39 -0400 Subject: [PATCH 22/23] current output dashboard, azimuth amp limit raise --- src/main/java/frc/robot/Robot.java | 16 ++++++++++++++++ src/main/java/frc/robot/commands/TurdDrive.java | 2 +- src/main/java/frc/robot/constants/Constants.java | 4 ++-- src/main/java/frc/robot/subsystems/TurdPod.java | 9 ++++++++- .../java/frc/robot/subsystems/TurdSwerve.java | 4 ++++ 5 files changed, 31 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 63bd2bd..66d39d9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,11 @@ package frc.robot; +import edu.wpi.first.util.datalog.BooleanLogEntry; +import edu.wpi.first.util.datalog.DataLog; +import edu.wpi.first.util.datalog.DoubleLogEntry; +import edu.wpi.first.util.datalog.StringLogEntry; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -13,14 +18,25 @@ public class Robot extends TimedRobot { private RobotContainer m_robotContainer; + DoubleLogEntry rightAmps; + DoubleLogEntry leftAmps; + @Override public void robotInit() { m_robotContainer = new RobotContainer(); + + DataLogManager.start(); + DataLog log = DataLogManager.getLog(); + rightAmps = new DoubleLogEntry(log, "left drive amps"); + leftAmps = new DoubleLogEntry(log, "right drive amps"); } @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); + leftAmps.append(RobotContainer.swerve.getDriveAmps()[0]); + rightAmps.append(RobotContainer.swerve.getDriveAmps()[1]); + } @Override diff --git a/src/main/java/frc/robot/commands/TurdDrive.java b/src/main/java/frc/robot/commands/TurdDrive.java index cbc544b..2f8c42a 100644 --- a/src/main/java/frc/robot/commands/TurdDrive.java +++ b/src/main/java/frc/robot/commands/TurdDrive.java @@ -59,7 +59,7 @@ public void execute() { double speedY = deadband ? 0 : joystickRight.get().getY() * maxSpeed; // double speedX = deadband ? 0 : 3.0 * Math.abs(joystickRight.get().getX()) * -joystickRight.get().getX(); // double speedY = deadband ? 0 : 3.0 * Math.abs(joystickRight.get().getY()) * joystickRight.get().getY(); - double speedOmega = Math.abs(joystickLeft.get().getX()) > 0.07 ? -joystickLeft.get().getX() * Math.abs(joystickLeft.get().getX()) : 0; + double speedOmega = Math.abs(joystickLeft.get().getX()) > 0.07 ? -joystickLeft.get().getX() * Math.abs(joystickLeft.get().getX())*0.3 : 0; ChassisSpeeds speeds = new ChassisSpeeds(speedX, speedY, speedOmega); swerve.setRobotSpeeds(speeds); diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index 16656f1..23e542a 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -21,7 +21,7 @@ public final class Constants { // Azimuth Settings public static final IdleMode azimuthMode = IdleMode.kBrake; - public static final int azimuthAmpLimit = 35; + public static final int azimuthAmpLimit = 55; public static final double azimuthMaxOutput = 0.25; public static final double azimuthkP = 0.4; @@ -36,7 +36,7 @@ public final class Constants { public static final IdleMode driveMode = IdleMode.kCoast; public static final int driveAmpLimit = 25; - public static final int driveTopAmpLimit = 90; + public static final int driveTopAmpLimit = 50; public static final double driveSpeedToPower = 1.0; public static final double driveMotorRampRate = 0.2; diff --git a/src/main/java/frc/robot/subsystems/TurdPod.java b/src/main/java/frc/robot/subsystems/TurdPod.java index d6ebf37..778f29b 100644 --- a/src/main/java/frc/robot/subsystems/TurdPod.java +++ b/src/main/java/frc/robot/subsystems/TurdPod.java @@ -91,6 +91,7 @@ public void resetZero() { public void revertZero() { absoluteEncoderOffset = azimuth.getDeviceId() == RobotMap.leftAzimuthID ? RobotMap.leftAbsoluteEncoderOffset : RobotMap.rightAbsoluteEncoderOffset; + SmartDashboard.putNumber((getPod() + " Encoder Offset"), absoluteEncoderOffset); resetPod(); } @@ -134,12 +135,18 @@ public double getAbsoluteEncoder() { return (absoluteEncoder.getAbsolutePosition() * 2*Math.PI) - absoluteEncoderOffset; } + public double getDriveAmp() { + return drive.getOutputCurrent(); + } + @Override public void periodic() { SmartDashboard.putNumber("getabsoluteEncoder() " + absoluteEncoder.getChannel(), getAbsoluteEncoder()); drive.set(speed + (azimuth.getAppliedOutput() * azimuthDriveSpeedMultiplier)); SmartDashboard.putNumber("azimuthEncoder.getPosition() " + azimuth.getDeviceId(), azimuthEncoder.getPosition()); SmartDashboard.putNumber("drive pos " + drive.getDeviceId(), driveEncoder.getPosition()); - SmartDashboard.putNumber("azimuth.getAppliedOutput()" + azimuth.getDeviceId(), azimuth.getAppliedOutput()); //getAppliedOutput()); + SmartDashboard.putNumber("azimuth.getoutputcurrent()" + azimuth.getDeviceId(), azimuth.getOutputCurrent()); + SmartDashboard.putNumber("drive.getoutputcurrent()" + drive.getDeviceId(), drive.getOutputCurrent()); } + } diff --git a/src/main/java/frc/robot/subsystems/TurdSwerve.java b/src/main/java/frc/robot/subsystems/TurdSwerve.java index 836e8d5..d9f0da0 100644 --- a/src/main/java/frc/robot/subsystems/TurdSwerve.java +++ b/src/main/java/frc/robot/subsystems/TurdSwerve.java @@ -144,4 +144,8 @@ public void addDashboardWidgets(ShuffleboardTab tab) { tab.add("Field", field2d).withPosition(0, 0).withSize(6, 4); tab.addString("Pose", this::getFomattedPose).withPosition(6, 2).withSize(2, 1); } + + public double[] getDriveAmps() { + return new double[] {leftPod.getDriveAmp(), rightPod.getDriveAmp()}; + } } From 4695220bb6a7c0a6d522ba35e8c7ecc9cd96d052 Mon Sep 17 00:00:00 2001 From: "maxwell.lee.372007" Date: Wed, 12 Jun 2024 13:24:19 -0400 Subject: [PATCH 23/23] adjust CAN IDs to match competition robot --- src/main/java/frc/robot/constants/RobotMap.java | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/constants/RobotMap.java b/src/main/java/frc/robot/constants/RobotMap.java index 7609a35..81fb6a1 100644 --- a/src/main/java/frc/robot/constants/RobotMap.java +++ b/src/main/java/frc/robot/constants/RobotMap.java @@ -10,16 +10,16 @@ /** CAN ID, Invert, Pod Positions, Offsets, Conversion Rates */ public final class RobotMap { - public static final int leftAzimuthID = 2; - public static final int rightAzimuthID = 3; + public static final int leftAzimuthID = 18;//BL + public static final int rightAzimuthID = 14;//FR - public static final int leftDriveID = 4; - public static final int rightDriveID = 5; + public static final int leftDriveID = 17;//BL + public static final int rightDriveID = 13;//FR public static final int leftAbsoluteEncoderID = 3; public static final int rightAbsoluteEncoderID = 0; - public static final int pigeonID = 6; + public static final int pigeonID = 25; public static final boolean leftAzimuthInvert = false; public static final boolean rightAzimuthInvert = false; @@ -31,8 +31,8 @@ public final class RobotMap { public static final Translation2d rightPodPosition = new Translation2d(Units.inchesToMeters(5.5), Units.inchesToMeters(5.5)); public static final SwerveDriveKinematics drivetrainKinematics = new SwerveDriveKinematics(robotCenter.minus(leftPodPosition), robotCenter.minus(rightPodPosition)); - public static final double leftAbsoluteEncoderOffset = 4.731349179724511;//absolute encoder reading at position - public static final double rightAbsoluteEncoderOffset = 0.73436864196419;// gears facing inwards: fwd/bck TODO: less janky alignment + public static final double leftAbsoluteEncoderOffset = 1.006118384272561;//absolute encoder reading at position + public static final double rightAbsoluteEncoderOffset = 1.056659935134538;// gears facing inwards: fwd/bck TODO: less janky alignment public static final double azimuthRadiansPerMotorRotation = 2*Math.PI*15/33; public static final double driveMetersPerMotorRotation = Units.inchesToMeters(2) * Math.PI * 33 / 45 / 15/13;