From 33b1517a86cc7e45dabf03c7f525f15adc6b25f6 Mon Sep 17 00:00:00 2001 From: Papplefarm20 <146298520+Papplefarm20@users.noreply.github.com> Date: Sat, 27 Jan 2024 21:45:11 -0800 Subject: [PATCH] feat: Indexer Simulation --- .../org/team1540/robot2024/Constants.java | 3 +- .../team1540/robot2024/RobotContainer.java | 23 +++++++++ .../subsystems/indexer/IndexerIO.java | 2 +- .../subsystems/indexer/IndexerIOSim.java | 51 +++++++++++++++++++ .../subsystems/indexer/IndexerIOSparkMax.java | 16 ++---- 5 files changed, 82 insertions(+), 13 deletions(-) create mode 100644 src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index a9194659..2d6fe9ff 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -63,7 +63,8 @@ public static class Indexer { public static final double FEEDER_KD = 0.0; public static final double FEEDER_KS = 0.0; public static final double FEEDER_KV = 0.0; - public static final double GEAR_RATIO = 0.0; + public static final double FEEDER_GEAR_RATIO = 0.0; + public static final double INTAKE_GEAR_RATIO = 0.0; } } diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index d6e67634..558a8c57 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -12,7 +12,12 @@ import org.team1540.robot2024.commands.FeedForwardCharacterization; import org.team1540.robot2024.commands.SwerveDriveCommand; +import org.team1540.robot2024.commands.indexer.IntakeCommand; import org.team1540.robot2024.subsystems.drive.*; +import org.team1540.robot2024.subsystems.indexer.Indexer; +import org.team1540.robot2024.subsystems.indexer.IndexerIO; +import org.team1540.robot2024.subsystems.indexer.IndexerIOSim; +import org.team1540.robot2024.subsystems.indexer.IndexerIOSparkMax; import org.team1540.robot2024.util.swerve.SwerveFactory; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -31,6 +36,7 @@ public class RobotContainer { // Controller public final CommandXboxController driver = new CommandXboxController(0); public final CommandXboxController copilot = new CommandXboxController(1); + public final Indexer indexer; // Dashboard inputs public final LoggedDashboardChooser autoChooser; @@ -49,6 +55,11 @@ public RobotContainer() { new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT)), new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT)), new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT))); + + indexer = + new Indexer( + new IndexerIOSparkMax() + ); break; case SIM: @@ -60,6 +71,10 @@ public RobotContainer() { new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim()); + indexer = + new Indexer( + new IndexerIOSim() + ); break; default: @@ -76,6 +91,10 @@ public RobotContainer() { }, new ModuleIO() { }); + indexer = + new Indexer( + new IndexerIO() {} + ); break; } @@ -108,6 +127,10 @@ private void configureButtonBindings() { drivetrain ).ignoringDisable(true) ); + + copilot.x().onTrue(indexer.feedToAmp()); + copilot.y().onTrue(indexer.feedToShooter()); + driver.a().onTrue(new IntakeCommand(indexer)); } /** diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java index a65aeab8..5ed351bc 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java @@ -7,7 +7,7 @@ public interface IndexerIO { @AutoLog class IndexerIOInputs { public double intakeVoltage; - public double intakeCurrent; + public double intakeCurrentAmps; public double intakeVelocityRPM; public double feederVoltage; public double feederCurrentAmps; diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java new file mode 100644 index 00000000..347e371c --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java @@ -0,0 +1,51 @@ +package org.team1540.robot2024.subsystems.indexer; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import edu.wpi.first.wpilibj.simulation.SimDeviceSim; +import org.team1540.robot2024.Constants; + +import static org.team1540.robot2024.Constants.Indexer.*; + +public class IndexerIOSim implements IndexerIO { + + + private final DCMotorSim intakeSim = new DCMotorSim(DCMotor.getNEO(1), INTAKE_GEAR_RATIO,0.025); + private final DCMotorSim feederSim = new DCMotorSim(DCMotor.getNEO(1), FEEDER_GEAR_RATIO,0.025); + private final SimDeviceSim beamBreakSim = new SimDeviceSim("Indexer Beam Break"); + private final PIDController feederSimPID = new PIDController(FEEDER_KP, FEEDER_KI,FEEDER_KD); + + @Override + public void updateInputs(IndexerIOInputs inputs) { + intakeSim.update(Constants.LOOP_PERIOD_SECS); + feederSim.update(Constants.LOOP_PERIOD_SECS); + inputs.intakeCurrentAmps = intakeSim.getCurrentDrawAmps(); +// inputs.intakeVoltage = intakeSim.getBusVoltage() * intakeSim.getAppliedOutput(); + inputs.intakeVelocityRPM = intakeSim.getAngularVelocityRPM(); + inputs.feederCurrentAmps = feederSim.getCurrentDrawAmps(); +// inputs.feederVoltage = feederSim.getBusVoltage() * feederSim.getAppliedOutput(); + inputs.feederVelocityRPM = feederSim.getAngularVelocityRPM(); + inputs.noteInIntake = beamBreakSim.getBoolean("Indexer Beam Break").get(); + + // this is a very funny line of code, and absolutely does not belong here, but I don't know how to do this otherwise + feederSim.setState(feederSim.getAngularPositionRad(), feederSimPID.calculate(feederSim.getAngularVelocityRadPerSec())); + + } + + @Override + public void setIntakeVoltage(double volts) { + intakeSim.setInputVoltage(MathUtil.clamp(volts, -12, 12)); + } + + @Override + public void setFeederVelocity(double velocity) { + feederSimPID.setSetpoint(velocity); + } + + @Override + public void setFeederVoltage(double volts) { + feederSim.setInputVoltage(MathUtil.clamp(volts, -12, 12)); + } +} diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java index 7e30bcd0..2cf45ddb 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java @@ -11,7 +11,7 @@ public class IndexerIOSparkMax implements IndexerIO { private final CANSparkMax intakeMotor = new CANSparkMax(INTAKE_ID, CANSparkLowLevel.MotorType.kBrushless); private final CANSparkMax feederMotor = new CANSparkMax(FEEDER_ID, CANSparkLowLevel.MotorType.kBrushless); - DigitalInput indexerBeamBreak = new DigitalInput(0); + private final DigitalInput indexerBeamBreak = new DigitalInput(0); private final SparkPIDController feederPID; private final SimpleMotorFeedforward feederFF = new SimpleMotorFeedforward(FEEDER_KS, FEEDER_KV); @@ -33,19 +33,14 @@ public IndexerIOSparkMax() { } - // 2 jobs, - // 1. be able to route to tramp - // 2. feeding the shooter (by default, be still, spin up fast 4 for shooter. - // make cmd factory spin - // slower in other direction for tramp @Override public void updateInputs(IndexerIOInputs inputs) { - inputs.intakeCurrent = intakeMotor.getOutputCurrent(); + inputs.intakeCurrentAmps = intakeMotor.getOutputCurrent(); inputs.intakeVoltage = intakeMotor.getBusVoltage() * intakeMotor.getAppliedOutput(); inputs.intakeVelocityRPM = intakeMotor.getEncoder().getVelocity(); - inputs.feederVoltage = feederMotor.getOutputCurrent(); - inputs.feederCurrentAmps = feederMotor.getBusVoltage() * feederMotor.getAppliedOutput(); + inputs.feederCurrentAmps = feederMotor.getOutputCurrent(); + inputs.feederVoltage = feederMotor.getBusVoltage() * feederMotor.getAppliedOutput(); inputs.feederVelocityRPM = feederMotor.getEncoder().getVelocity(); inputs.noteInIntake = indexerBeamBreak.get(); } @@ -63,7 +58,7 @@ public void setFeederVoltage(double volts) { @Override public void setFeederVelocity(double velocity) { feederPID.setReference( - Units.radiansPerSecondToRotationsPerMinute(velocity) * GEAR_RATIO, + Units.radiansPerSecondToRotationsPerMinute(velocity) * FEEDER_GEAR_RATIO, CANSparkBase.ControlType.kVelocity, 0, feederFF.calculate(velocity), @@ -71,4 +66,3 @@ public void setFeederVelocity(double velocity) { ); } } -// for elevator sim, app. voltage \ No newline at end of file