Runs in sim now, button binding temporary

Team-4795 · Oct 10, 2024 · 32feff9 · 32feff9
1 parent 034d44f
commit 32feff9
Show file tree

Hide file tree

Showing 8 changed files with 21 additions and 16 deletions.
diff --git a/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat b/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat
diff --git a/logs/Log_24-10-10_16-16-33.wpilog b/logs/Log_24-10-10_16-16-33.wpilog
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -22,7 +22,7 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
-  public static final Mode currentMode = Mode.REAL;
+  public static final Mode currentMode = Mode.SIM;
 
   public static enum Mode {
     /** Running on a real robot. */

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -14,7 +14,6 @@
 package frc.robot;
 
 import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.auto.NamedCommands;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.GenericHID;
@@ -103,8 +102,7 @@ public RobotContainer() {
         break;
     }
 
-
-    //This can go away
+    // This can go away
 
     // Set up auto routines
     // NamedCommands.registerCommand(
@@ -154,7 +152,9 @@ private void configureButtonBindings() {
                     drive)
                 .ignoringDisable(true));
 
-    controller.y().onTrue(Commands.runEnd(() -> pivot.setGoal(45), () -> pivot.setGoal(0)));
+    controller
+        .y()
+        .whileTrue(Commands.runEnd(() -> pivot.setGoal(Math.PI / 4), () -> pivot.setGoal(0), pivot));
   }
 
   /**

diff --git a/src/main/java/frc/robot/subsystems/pivot/Pivot.java b/src/main/java/frc/robot/subsystems/pivot/Pivot.java
@@ -39,7 +39,7 @@ public static Pivot getInstance() {
   }
 
   public static Pivot initialize(PivotIO io) {
-    if (Pivot.pivotInstance == null) {
+    if (Pivot.pivotInstance == null) { 
       pivotInstance = new Pivot(io);
     }
     return pivotInstance;
@@ -48,6 +48,7 @@ public static Pivot initialize(PivotIO io) {
   @Override
   public void periodic() {
     io.updateInputs(inputs);
+    io.setVoltage(inputs.appliedVolts);
     Logger.processInputs("Pivot", inputs);
   }
 
@@ -56,7 +57,7 @@ public void runVoltage(double volts) {
   }
 
   public void setGoal(double goal) {
-    goalRad = goal;
+    io.setAngle(goal);
   }
 
   public void stop() {

diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIO.java b/src/main/java/frc/robot/subsystems/pivot/PivotIO.java
@@ -8,7 +8,7 @@ public interface PivotIO {
   public static class PivotIOInputs {
     public double positionRad = 0.0;
     public double appliedVolts = 0.0;
-    public double velocityRevolutionsPerSec = 0.0;
+    public double velocityRadPerSec = 0.0;
     public double[] currentAmps = new double[] {};
   }
 

diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java b/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java
@@ -40,7 +40,7 @@ public PivotIOReal() {
   @Override
   public void updateInputs(PivotIOInputs inputs) {
     inputs.positionRad = pivotEncoderRight.getPosition() * 2 * Math.PI;
-    inputs.velocityRevolutionsPerSec = pivotEncoderRight.getVelocity();
+    inputs.velocityRadPerSec = pivotEncoderRight.getVelocity() * 2 * Math.PI / 60;
     inputs.appliedVolts = appliedVolts;
 
     if (closedLoop) {

diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java b/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java
@@ -7,19 +7,23 @@
 
 public class PivotIOSim implements PivotIO {
   private SingleJointedArmSim sim =
-      new SingleJointedArmSim(DCMotor.getNEO(1), 1.5, 0.004, 0, 0, 0, false, 0);
-  private PIDController pid = new PIDController(1.0, 0.0, 0.0);
+      new SingleJointedArmSim(DCMotor.getNEO(1), 1.5, 0.004, 0.2, 0, Math.PI, false, 0);
+  private PIDController pid = new PIDController(0.2, 0.0, 0.0);
 
-  private boolean closedLoop = false;
+  private boolean closedLoop = true;
   private double ffVolts = 0.0;
   private double appliedVolts = 0.0;
 
   @Override
   public void updateInputs(PivotIOInputs inputs) {
-    if (closedLoop) {
-      appliedVolts = MathUtil.clamp(pid.calculate(sim.getAngleRads()) + ffVolts, -12.0, 12.0);
-      sim.setInputVoltage(appliedVolts);
-    }
+    sim.update(0.02);
+
+    appliedVolts = MathUtil.clamp(pid.calculate(sim.getAngleRads()) + ffVolts, -12.0, 12.0);
+    sim.setInputVoltage(appliedVolts);
+
+    inputs.positionRad = sim.getAngleRads();
+    inputs.appliedVolts = appliedVolts;
+    inputs.velocityRadPerSec = sim.getVelocityRadPerSec();
   }
 
   @Override