Added second motor

team-7558 · Jan 22, 2025 · 850aa74 · 850aa74
1 parent b103451
commit 850aa74
Show file tree

Hide file tree

Showing 6 changed files with 108 additions and 37 deletions.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -127,6 +127,8 @@ public void robotPeriodic() {
     } else if (bb) {
       /* scoring */
       hand.queueState(HandStates.SPITTING);
+    } else {
+      hand.queueState(HandStates.IDLE);
     }
     SwerveInput input = new SwerveInput(x_, y_, w_, throttle);
     drive.setInput(input);

diff --git a/src/main/java/frc/robot/subsystems/hand/Hand.java b/src/main/java/frc/robot/subsystems/hand/Hand.java
@@ -48,16 +48,17 @@ public void handleStateMachine() {
       case DISABLED:
         break;
       case IDLE:
-        setVoltage(0.0);
+        intakeSetVoltage(0.0);
+        scoringSetVoltage(0.0);
         break;
       case INTAKING:
-        setVoltage(3.0);
+        intakeSetVoltage(3.0);
         if (inputs.beamBreakActivated) {
           queueState(HandStates.IDLE);
         }
         break;
       case SPITTING:
-        setVoltage(-3.0);
+        scoringSetVoltage(-3.0);
         break;
       default:
         break;
@@ -66,15 +67,20 @@ public void handleStateMachine() {
 
   @Override
   public void outputPeriodic() {
-    mech.setAngle(inputs.VelocityDegPS);
+    mech.setAngle(inputs.intakeVelocityDegPS);
+    mech.score(inputs.scoringVelocityDegPS);
     mech.periodic();
   }
 
-  public void setVelocity(double velocity_DPS) {
-    setVoltage(kV * Units.degreesToRadians(velocity_DPS) + kS);
+  public void scoringSetVoltage(double volts) {
+    io.scoringSetVoltage(volts);
   }
 
-  public void setVoltage(double volts) {
-    io.setVoltage(volts);
+  public void intakeSetVelocity(double velocity_DPS) {
+    intakeSetVoltage(kV * Units.degreesToRadians(velocity_DPS) + kS);
+  }
+
+  public void intakeSetVoltage(double volts) {
+    io.intakeSetVoltage(volts);
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/hand/Hand2d.java b/src/main/java/frc/robot/subsystems/hand/Hand2d.java
@@ -11,19 +11,29 @@ public class Hand2d {
   Mechanism2d mech;
   MechanismRoot2d root;
   public MechanismLigament2d hand;
+  MechanismLigament2d joint;
+  MechanismLigament2d scoring;
+
   public static Hand2d instance;
 
   public Hand2d() {
     mech = new Mechanism2d(4, 4);
     root = mech.getRoot("Root", 2, 0.5);
 
-    hand = root.append(new MechanismLigament2d("elevator", 0.5, 90, 2, new Color8Bit(255, 0, 0)));
+    hand = root.append(new MechanismLigament2d("intake", 0.5, 90, 2, new Color8Bit(255, 0, 0)));
+    joint = root.append(new MechanismLigament2d("joint", 1, 0, 2, new Color8Bit(255, 0, 255)));
+    scoring =
+        joint.append(new MechanismLigament2d("scoring", 0.5, 90, 2, new Color8Bit(255, 0, 0)));
   }
 
   public void setAngle(double angle) {
     hand.setAngle(angle);
   }
 
+  public void score(double angle) {
+    scoring.setAngle(angle);
+  }
+
   public void periodic() {
     SmartDashboard.putData("Hand2d", mech);
     Logger.recordOutput("Hand2d", mech);

diff --git a/src/main/java/frc/robot/subsystems/hand/HandIO.java b/src/main/java/frc/robot/subsystems/hand/HandIO.java
@@ -5,17 +5,24 @@
 public interface HandIO {
   @AutoLog
   public static class HandIOInputs {
-    public double VelocityDegPS = 0.0;
-    public double AppliedVolts = 0.0;
-    public double[] current_Amps = new double[] {};
+    public double intakeVelocityDegPS = 0.0;
+    public double intakeAppliedVolts = 0.0;
+    public double[] intake_current_Amps = new double[] {};
+    public double scoringVelocityDegPS = 0.0;
+    public double scoringAppliedVolts = 0.0;
+    public double[] scoring_current_Amps = new double[] {};
     public boolean beamBreakActivated = false;
   }
 
   public default void updateInputs(HandIOInputs inputs) {}
 
-  public default void setVoltage(double volts) {}
+  public default void intakeSetVoltage(double volts) {}
 
-  public default void stop() {}
+  public default void intakeStop() {}
+
+  public default void scoringSetVoltage(double volts) {}
+
+  public default void scoringStop() {}
 
   public default void toggleBrake() {}
 }
diff --git a/src/main/java/frc/robot/subsystems/hand/HandIOSim.java b/src/main/java/frc/robot/subsystems/hand/HandIOSim.java
@@ -5,29 +5,49 @@
 import edu.wpi.first.wpilibj.simulation.FlywheelSim;
 
 public class HandIOSim implements HandIO {
-  public FlywheelSim hand = new FlywheelSim(DCMotor.getKrakenX60Foc(1), 1, 1);
-  private double applied_volts = 0.0;
+  public FlywheelSim intake = new FlywheelSim(DCMotor.getKrakenX60Foc(1), 1, 1);
+  public FlywheelSim scoring = new FlywheelSim(DCMotor.getKrakenX60Foc(1), 1, 1);
+  private double intake_applied_volts = 0.0;
+  private double scoring_applied_volts = 0.0;
 
   @Override
   public void updateInputs(HandIOInputs inputs) {
     // Update elevator simulation
-    hand.update(0.02); // 20 ms update
-    inputs.VelocityDegPS = Units.radiansToDegrees(hand.getAngularVelocityRadPerSec());
-    inputs.AppliedVolts = applied_volts;
-    inputs.current_Amps =
+    intake.update(0.02); // 20 ms update
+    scoring.update(0.02); // 20 ms update
+    inputs.intakeVelocityDegPS = Units.radiansToDegrees(intake.getAngularVelocityRadPerSec());
+    inputs.intakeAppliedVolts = intake_applied_volts;
+    inputs.intake_current_Amps =
         new double[] {
-          hand.getCurrentDrawAmps(), hand.getCurrentDrawAmps()
+          intake.getCurrentDrawAmps(), intake.getCurrentDrawAmps()
         }; // Simulate multiple motor left_currentsFF
+    inputs.scoringVelocityDegPS = Units.radiansToDegrees(scoring.getAngularVelocityRadPerSec());
+    inputs.scoringAppliedVolts = scoring_applied_volts;
+    inputs.scoring_current_Amps =
+        new double[] {
+          scoring.getCurrentDrawAmps(), scoring.getCurrentDrawAmps()
+        }; // Simulate multiple motor left_currentsFF
+  }
+
+  @Override
+  public void intakeSetVoltage(double volts) {
+    intake_applied_volts = volts;
+    intake.setInputVoltage(volts);
+  }
+
+  @Override
+  public void scoringSetVoltage(double volts) {
+    scoring_applied_volts = volts;
+    scoring.setInputVoltage(volts);
   }
 
   @Override
-  public void setVoltage(double volts) {
-    applied_volts = volts;
-    hand.setInputVoltage(volts);
+  public void intakeStop() {
+    intakeSetVoltage(0.0);
   }
 
   @Override
-  public void stop() {
-    setVoltage(0.0);
+  public void scoringStop() {
+    intakeSetVoltage(0.0);
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/hand/HandIOTalonFX.java b/src/main/java/frc/robot/subsystems/hand/HandIOTalonFX.java
@@ -10,12 +10,14 @@
 
 public class HandIOTalonFX implements HandIO {
 
-  private final TalonFX motor;
+  private final TalonFX intakeMotor;
+  private final TalonFX scoringMotor;
   private final VoltageOut voltage_out;
   private final DigitalInput beambreak;
 
   public HandIOTalonFX() {
-    motor = new TalonFX(13);
+    intakeMotor = new TalonFX(13);
+    scoringMotor = new TalonFX(14);
     var motorConfig = new TalonFXConfiguration();
     beambreak = new DigitalInput(0);
 
@@ -24,7 +26,8 @@ public HandIOTalonFX() {
     motorConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = 0.5;
     motorConfig.Feedback.SensorToMechanismRatio = 5;
     motorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
-    motor.getConfigurator().apply(motorConfig);
+    intakeMotor.getConfigurator().apply(motorConfig);
+    scoringMotor.getConfigurator().apply(motorConfig);
 
     // Initialize voltage control
     voltage_out = new VoltageOut(0.0);
@@ -35,22 +38,45 @@ public void updateInputs(HandIOInputs inputs) {
     BaseStatusSignal.refreshAll();
 
     // Read motor data and update inputs
-    inputs.current_Amps = new double[] {motor.getStatorCurrent().getValueAsDouble()};
-    inputs.AppliedVolts = motor.getMotorVoltage().getValueAsDouble();
-    inputs.VelocityDegPS =
-        motor.getVelocity().getValueAsDouble(); // Assuming getVelocity() is properly configured
+    inputs.intake_current_Amps = new double[] {intakeMotor.getStatorCurrent().getValueAsDouble()};
+    inputs.intakeAppliedVolts = intakeMotor.getMotorVoltage().getValueAsDouble();
+    inputs.intakeVelocityDegPS =
+        intakeMotor
+            .getVelocity()
+            .getValueAsDouble(); // Assuming getVelocity() is properly configured
+    inputs.scoring_current_Amps = new double[] {intakeMotor.getStatorCurrent().getValueAsDouble()};
+    inputs.scoringAppliedVolts = intakeMotor.getMotorVoltage().getValueAsDouble();
+    inputs.scoringVelocityDegPS =
+        scoringMotor
+            .getVelocity()
+            .getValueAsDouble(); // Assuming getVelocity() is properly configured
   }
 
-  public void setVoltage(double volts) {
+  @Override
+  public void intakeSetVoltage(double volts) {
+    // Clamp voltage to valid range (-12V to 12V)
+    volts = MathUtil.clamp(volts, -12.0, 12.0);
+
+    // Set motor voltage
+    intakeMotor.setControl(voltage_out.withOutput(volts));
+  }
+
+  @Override
+  public void intakeStop() {
+    intakeSetVoltage(0.0);
+  }
+
+  @Override
+  public void scoringSetVoltage(double volts) {
     // Clamp voltage to valid range (-12V to 12V)
     volts = MathUtil.clamp(volts, -12.0, 12.0);
 
     // Set motor voltage
-    motor.setControl(voltage_out.withOutput(volts));
+    scoringMotor.setControl(voltage_out.withOutput(volts));
   }
 
   @Override
-  public void stop() {
-    setVoltage(0.0);
+  public void scoringStop() {
+    scoringSetVoltage(0.0);
   }
 }