Devbot

Mechanical-Advantage · Feb 7, 2024 · 7559b70 · 7559b70
1 parent 03414d3
commit 7559b70
Show file tree

Hide file tree

Showing 3 changed files with 67 additions and 38 deletions.
diff --git a/src/main/java/frc/robot/subsystems/superstructure/Arm/Arm.java b/src/main/java/frc/robot/subsystems/superstructure/Arm/Arm.java
@@ -1,9 +1,7 @@
 package frc.robot.subsystems.superstructure.Arm;
 
-import static edu.wpi.first.units.Units.*;
 import static frc.robot.subsystems.superstructure.SuperstructureConstants.ArmConstants.*;
 
-import com.ctre.phoenix6.SignalLogger;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
@@ -14,7 +12,6 @@
 import edu.wpi.first.wpilibj.util.Color8Bit;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.subsystems.drive.DriveConstants;
 import frc.robot.util.EqualsUtil;
 import frc.robot.util.LoggedTunableNumber;
@@ -58,11 +55,8 @@ public class Arm extends SubsystemBase {
   private final Timer homingTimer = new Timer();
 
   private boolean homed = false;
-
   private double setpoint = 0.0;
 
-  private SysIdRoutine routine;
-
   //  private final MechanismLigament2d armSetpoint;
 
   public Arm(ArmIO io) {
@@ -93,23 +87,6 @@ public Arm(ArmIO io) {
     //            10.0,
     //            new Color8Bit(Color.kGreen));
     //    armRoot.append(armSetpoint);
-
-    routine =
-        new SysIdRoutine(
-            new SysIdRoutine.Config(
-                Volts.of(0.05).per(Seconds.of(1.0)),
-                Volts.of(4.0),
-                Seconds.of(30.0),
-                state -> {
-                  SignalLogger.writeString("SysIdState", state.toString());
-                }),
-            new SysIdRoutine.Mechanism(
-                voltageMeasure -> armIO.setVoltage(voltageMeasure.in(Volts)),
-                sysIdRoutineLog -> {
-                  Logger.recordOutput("Arm/SysIdVoltage", inputs.armAppliedVolts[0]);
-                  Logger.recordOutput("Arm/SysIdCurrent", inputs.armTorqueCurrentAmps[0]);
-                },
-                this));
   }
 
   @Override
@@ -129,7 +106,6 @@ public void periodic() {
         armVelocity,
         armAcceleration);
 
-    setpoint = Units.degreesToRadians(armDesiredSetpoint.get());
     // Home if not already homed
     if (!homed && DriverStation.isEnabled()) {
       armIO.setVoltage(-1.0);
@@ -145,7 +121,7 @@ public void periodic() {
         homed = true;
       }
     } else {
-      setSetpoint(setpoint);
+      armIO.setSetpoint(setpoint);
     }
 
     if (DriverStation.isDisabled()) {
@@ -166,7 +142,6 @@ public void setVoltage(double volts) {
 
   public void setSetpoint(double setpointRads) {
     if (disableSupplier.getAsBoolean() || !homed) return;
-    armIO.setSetpoint(setpointRads);
   }
 
   public void setBrakeMode(boolean enabled) {
@@ -182,15 +157,6 @@ public void stop() {
     armIO.stop();
   }
 
-  // SysId command factories
-  public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
-    return routine.quasistatic(direction).beforeStarting(SignalLogger::start);
-  }
-
-  public Command sysIdDynamic(SysIdRoutine.Direction direction) {
-    return routine.dynamic(direction).finallyDo(SignalLogger::stop);
-  }
-
   public Command getStaticCurrent() {
     Timer timer = new Timer();
     return run(() -> armIO.setCurrent(0.5 * timer.get()))
@@ -204,4 +170,9 @@ public Command getStaticCurrent() {
   public boolean homed() {
     return homed;
   }
+
+  @AutoLogOutput(key = "Arm/AtSetpoint")
+  public boolean atSetpoint() {
+    return Math.abs(inputs.armAnglePositionRads - setpoint) <= armTolerance.get();
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/superstructure/Arm/ArmIOKrakenFOC.java b/src/main/java/frc/robot/subsystems/superstructure/Arm/ArmIOKrakenFOC.java
@@ -113,11 +113,17 @@ public void updateInputs(ArmIOInputs inputs) {
         Units.rotationsToRadians(armTrajectorySetpointPositionRotations.getValue());
     inputs.armVelocityRadsPerSec = Units.rotationsToRadians(armVelocityRPS.getValue());
     inputs.armAppliedVolts =
-        new double[] {armAppliedVoltage[0].getValueAsDouble(), armAppliedVoltage[1].getValueAsDouble()};
+        new double[] {
+          armAppliedVoltage[0].getValueAsDouble(), armAppliedVoltage[1].getValueAsDouble()
+        };
     inputs.armCurrentAmps =
-        new double[] {armOutputCurrent[0].getValueAsDouble(), armOutputCurrent[1].getValueAsDouble()};
+        new double[] {
+          armOutputCurrent[0].getValueAsDouble(), armOutputCurrent[1].getValueAsDouble()
+        };
     inputs.armTorqueCurrentAmps =
-        new double[] {armTorqueCurrent[0].getValueAsDouble(), armTorqueCurrent[1].getValueAsDouble()};
+        new double[] {
+          armTorqueCurrent[0].getValueAsDouble(), armTorqueCurrent[1].getValueAsDouble()
+        };
     inputs.armTempCelcius =
         new double[] {armTempCelsius[0].getValueAsDouble(), armTempCelsius[1].getValueAsDouble()};
   }

diff --git a/src/main/java/frc/robot/subsystems/superstructure/DevBotSuperstructure.java b/src/main/java/frc/robot/subsystems/superstructure/DevBotSuperstructure.java
@@ -0,0 +1,52 @@
+package frc.robot.subsystems.superstructure;
+
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.superstructure.Arm.Arm;
+import frc.robot.subsystems.superstructure.shooter.Shooter;
+import lombok.Getter;
+import org.littletonrobotics.junction.Logger;
+
+public class DevBotSuperstructure extends SubsystemBase {
+  public enum State {
+    SHOOTING,
+    INTAKING,
+    IDLE
+  }
+
+  @Getter private State currentState = State.IDLE;
+  private final Arm arm;
+  private final Shooter shooter;
+
+  public DevBotSuperstructure(Arm arm, Shooter shooter) {
+    this.arm = arm;
+    this.shooter = shooter;
+  }
+
+  @Override
+  public void periodic() {
+    switch (currentState) {
+      case IDLE -> {
+        arm.setSetpoint(Units.degreesToRadians(5.0));
+        shooter.stop();
+      }
+      case INTAKING -> {
+        arm.setSetpoint(Units.degreesToRadians(50.0));
+        if (arm.atSetpoint()) {
+          shooter.setIntaking();
+        }
+      }
+      case SHOOTING -> {
+        if (arm.atSetpoint()) {
+          shooter.setShooting();
+        }
+      }
+    }
+
+    Logger.recordOutput("DevBotSuperstructure/currentState", currentState.toString());
+  }
+
+  public void setDesiredState(State desiredState) {
+    currentState = desiredState;
+  }
+}