From 0633ff8fb4db72688603eca20c002bee66f45e75 Mon Sep 17 00:00:00 2001 From: apoorva <97716935+carokhan@users.noreply.github.com> Date: Wed, 23 Oct 2024 19:32:07 -0400 Subject: [PATCH] grrr --- simgui.json | 1 + src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/Constants.java | 20 +++++++++---------- src/main/java/frc/robot/Robot.java | 4 ---- .../robot/subsystems/drive/ModuleIOReal.java | 6 +++--- .../frc/robot/subsystems/pivot2/Pivot.java | 17 +++++++++++++--- 6 files changed, 33 insertions(+), 25 deletions(-) diff --git a/simgui.json b/simgui.json index c799be1..2be0cc5 100644 --- a/simgui.json +++ b/simgui.json @@ -10,6 +10,7 @@ "types": { "/AdvantageKit/RealOutputs/Climb/Mechanism2d": "Mechanism2d", "/AdvantageKit/RealOutputs/Visualizer/FullRobot": "Mechanism2d", + "/AdvantageKit/RealOutputs/Visualizer/M_main": "Mechanism2d", "/FMSInfo": "FMSInfo", "/SmartDashboard/Climb Down": "Command", "/SmartDashboard/Climb Up": "Command", diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 31d1756..7f69548 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Forte-2-5"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 86; - public static final String GIT_SHA = "d3a4a8707328591d90a430f102dc2b82e8cbbb9b"; - public static final String GIT_DATE = "2024-10-18 06:22:47 EDT"; + public static final int GIT_REVISION = 89; + public static final String GIT_SHA = "5eb048c58fec65b3f00463e715a48f1dd720a332"; + public static final String GIT_DATE = "2024-10-23 06:52:00 EDT"; public static final String GIT_BRANCH = "main"; - public static final String BUILD_DATE = "2024-10-22 21:05:57 EDT"; - public static final long BUILD_UNIX_TIME = 1729645557657L; + public static final String BUILD_DATE = "2024-10-23 19:29:37 EDT"; + public static final long BUILD_UNIX_TIME = 1729726177390L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 85d28c5..6dfbf38 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 double loopPeriodSecs = 0.02; - public static final Mode currentMode = Mode.REAL; + public static final Mode currentMode = Mode.SIM; public static enum Mode { /** Running on a real robot. */ @@ -121,7 +121,7 @@ public static class DriveConstants { public static final int driveSupplyCurrent = 50; // 70 public static final int driveStatorCurrent = 90; // 120 - public static final int turnCurrent = 30; // 30 + public static final int turnCurrent = 40; // 30 public static final double odometeryFrequency = 250; public static final double updateFrequency = 100; @@ -130,7 +130,7 @@ public static class DriveConstants { // public static final double maxLinearVelocity = Units.feetToMeters(1.4); public static final double maxLinearAccel = 8.0; - public static final double maxAngularVelocity = 10; + public static final double maxAngularVelocity = 20; public static final double maxAngularAccel = 10; public static double kPDriveReal = 2.0; @@ -139,7 +139,7 @@ public static class DriveConstants { public static double kVDriveReal = 1.93; public static double kADriveReal = 0.25; - public static double kPTurnReal = 3; // 1.5? + public static double kPTurnReal = 2.5; // 1.5? public static double kDTurnReal = 0.0; public static double kPDriveSim = 0.3; @@ -216,7 +216,7 @@ public static class IntakeConstants { public static final double simOffset = 1.27838411; public static final int pivotCurrentLimit = 30; - public static final int rollerCurrentLimit = 70; + public static final int rollerCurrentLimit = 60; public static final double kGPivot = 0.5; public static final double kVPivot = 1.06; @@ -265,7 +265,7 @@ public static class ShooterConstants { // public static final double pivotMOI = .0001; public static final double maxPivotVelocity = 20; - public static final double maxPivotAccel = 5; + public static final double maxPivotAccel = 10; public static final double pivotAbsConversion = Math.PI * 2.0; public static final double pivotEncConversion = 2.0 * Math.PI / pivotRatio; @@ -279,10 +279,10 @@ public static class ShooterConstants { public static final double shooterMOI = 0.00920287973; - public static final int pivotCurrentLimit = 25; + public static final int pivotCurrentLimit = 15; - public static final double kGPivot = 0.381640625; - public static final double kVPivot = 0.875; + public static final double kGPivot = 0.38; + public static final double kVPivot = 0.8; public static final double kAPivot = 0.00; public static final double kVShooter = 0.0055; // COMP: 0.0055, DEMO: 0.0041875 @@ -292,7 +292,7 @@ public static class ShooterConstants { public static final double kIShooterReal = 0.00000; public static final double kSShooterReal = 0; - public static final double kPPivot = 5.0; + public static final double kPPivot = 3.0; public static final double kPShooterSim = 0.5; public static final double kIShooterSim = 0.0; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5eb48eb..b08906b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -15,12 +15,10 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; - import java.io.File; import java.io.IOException; import java.nio.file.Files; import java.util.Arrays; - import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -104,8 +102,6 @@ public void robotInit() { robotContainer = new RobotContainer(); } - - /** This function is called periodically during all modes. */ @Override public void robotPeriodic() { diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOReal.java index 3b65625..8b930ac 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOReal.java @@ -188,6 +188,8 @@ public ModuleIOReal(int index) { PeriodicFrame.kStatus2, (int) (1000.0 / DriveConstants.odometeryFrequency)); } + turnRelativeEncoder.setPosition(getAbsoluteEncoder()); + turnPID.setPositionPIDWrappingEnabled(true); turnPID.setPositionPIDWrappingMinInput(-Math.PI); turnPID.setPositionPIDWrappingMaxInput(Math.PI); @@ -195,8 +197,6 @@ public ModuleIOReal(int index) { turnSparkMax.burnFlash(); turnSparkMax.setCANTimeout(0); - turnRelativeEncoder.setPosition(getAbsoluteEncoder()); - timestampQueue = HybridOdometryThread.getInstance().makeTimestampQueue(); drivePosition = driveTalon.getPosition(); @@ -225,7 +225,7 @@ public ModuleIOReal(int index) { @Override public void processInputs(ModuleIOInputsAutoLogged inputs) { - if (iLoveRev % 50 == 0) { + if (iLoveRev % 25 == 0) { turnRelativeEncoder.setPosition(getAbsoluteEncoder()); } diff --git a/src/main/java/frc/robot/subsystems/pivot2/Pivot.java b/src/main/java/frc/robot/subsystems/pivot2/Pivot.java index bfb189e..42c7d32 100644 --- a/src/main/java/frc/robot/subsystems/pivot2/Pivot.java +++ b/src/main/java/frc/robot/subsystems/pivot2/Pivot.java @@ -19,6 +19,7 @@ public class Pivot extends SubsystemBase { public PivotIO io; public PivotIOInputsAutoLogged inputs = new PivotIOInputsAutoLogged(); + boolean overshoot; private ArmFeedforward pivotFF = new ArmFeedforward( @@ -45,15 +46,25 @@ public Pivot(PivotIO io) { public void periodic() { io.processInputs(inputs); Logger.processInputs("Shooter/Pivot", inputs); + overshoot = + (inputs.pivotPosition.getRadians() > inputs.pivotTargetPosition.getRadians()) + && (inputs.pivotTargetPosition.getRadians() != 0); + + // if (overshoot) { + // io.setPivotVoltage( + // pivotPID.calculate( + // inputs.pivotPosition.getRadians(), inputs.pivotTargetPosition.getRadians())); + // } + Logger.recordOutput("Test/Overshoot", overshoot); } public Command setPivotTarget(DoubleSupplier radians) { return this.run( () -> { double volts = - pivotPID.calculate(inputs.pivotPosition.getRadians(), radians.getAsDouble()) - + pivotFF.calculate( - pivotPID.getSetpoint().position, pivotPID.getSetpoint().velocity); + pivotPID.calculate(inputs.pivotPosition.getRadians(), radians.getAsDouble()); + // + pivotFF.calculate( + // pivotPID.getSetpoint().position, pivotPID.getSetpoint().velocity); io.setPivotVoltage(volts); inputs.pivotAppliedVolts = volts;