From a491418d7599f67a245821e608ecb3db2308c18b Mon Sep 17 00:00:00 2001 From: Papplefarm20 <146298520+Papplefarm20@users.noreply.github.com> Date: Thu, 25 Jan 2024 22:23:39 -0800 Subject: [PATCH 01/11] Intake motor subsystem code --- .../org/team1540/robot2024/Constants.java | 4 +++ .../robot2024/subsystems/indexer/Indexer.java | 30 ++++++++++++++++ .../subsystems/indexer/IndexerIO.java | 25 ++++++++++++++ .../subsystems/indexer/IndexerIOReal.java | 34 +++++++++++++++++++ 4 files changed, 93 insertions(+) create mode 100644 src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java create mode 100644 src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java create mode 100644 src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOReal.java diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index a81310f2..3703ba85 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -55,4 +55,8 @@ public static class Drivetrain { public static final double DRIVE_BASE_RADIUS = Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; } + public static class Indexer { + public static final int INTAKE_ID = 0; + + } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java new file mode 100644 index 00000000..f3facc50 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java @@ -0,0 +1,30 @@ +package org.team1540.robot2024.subsystems.indexer; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + + +public class Indexer extends SubsystemBase { + private final IndexerIO indexerIO; + private final IndexerIOInputsAutoLogged indexerInputs = new IndexerIOInputsAutoLogged(); + + public Indexer(IndexerIO indexerIO) { + this.indexerIO = indexerIO; + } + + public void periodic() { + indexerIO.updateInputs(indexerInputs); + Logger.processInputs("Indexer", indexerInputs); + } + + public void setIntakeSpeed(double speed) { + indexerIO.setIntakeSpeed(speed); + } + + public void setIntakeVoltage(double volts) { + indexerIO.setIntakeVoltage(volts); + } + + + +} diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java new file mode 100644 index 00000000..23266270 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java @@ -0,0 +1,25 @@ +package org.team1540.robot2024.subsystems.indexer; + +import org.littletonrobotics.junction.AutoLog; + +public interface IndexerIO { + + @AutoLog + class IndexerIOInputs { + public double intakeVoltage = 0.0; + public double intakeCurrent = 0.0; + public double intakeVelocityRPM = 0.0; + } + + /** + * Updates the set of loggable inputs. + */ + default void updateInputs(IndexerIOInputs inputs) {} + + default void setIntakeVoltage(double volts) {} + + default void setIntakeSpeed(double speed) {} + + + +} diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOReal.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOReal.java new file mode 100644 index 00000000..849aa338 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOReal.java @@ -0,0 +1,34 @@ +package org.team1540.robot2024.subsystems.indexer; + +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkLowLevel; +import com.revrobotics.CANSparkMax; + +import static org.team1540.robot2024.Constants.Indexer.*; + +public class IndexerIOReal implements IndexerIO { + CANSparkMax intakeMotor = new CANSparkMax(INTAKE_ID, CANSparkLowLevel.MotorType.kBrushless); + + + public IndexerIOReal() { + intakeMotor.setIdleMode(CANSparkBase.IdleMode.kBrake); + intakeMotor.enableVoltageCompensation(12.0); + intakeMotor.setSmartCurrentLimit(30); + } + + @Override + public void updateInputs(IndexerIOInputs inputs) { + inputs.intakeCurrent = intakeMotor.getOutputCurrent(); + inputs.intakeVoltage = intakeMotor.getBusVoltage() * intakeMotor.getAppliedOutput(); + } + + @Override + public void setIntakeVoltage(double volts) { + intakeMotor.setVoltage(volts); + } + + @Override + public void setIntakeSpeed(double speed) { + intakeMotor.set(speed); + } +} From 61e7def26fae8241818a7ea7d591e8db3ce37695 Mon Sep 17 00:00:00 2001 From: Papplefarm20 <146298520+Papplefarm20@users.noreply.github.com> Date: Fri, 26 Jan 2024 18:57:15 -0800 Subject: [PATCH 02/11] feat: Intake & Indexer IO and subsystem --- .../org/team1540/robot2024/Constants.java | 7 ++ .../commands/indexer/IntakeCommand.java | 30 ++++++++ .../robot2024/subsystems/indexer/Indexer.java | 26 ++++++- .../subsystems/indexer/IndexerIO.java | 14 ++-- .../subsystems/indexer/IndexerIOReal.java | 34 --------- .../subsystems/indexer/IndexerIOSparkMax.java | 74 +++++++++++++++++++ 6 files changed, 142 insertions(+), 43 deletions(-) create mode 100644 src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java delete mode 100644 src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOReal.java create mode 100644 src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 3703ba85..a9194659 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -57,6 +57,13 @@ public static class Drivetrain { } public static class Indexer { public static final int INTAKE_ID = 0; + public static final int FEEDER_ID = 0; + public static final double FEEDER_KP = 0.0; + public static final double FEEDER_KI = 0.0; + 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; } } diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java b/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java new file mode 100644 index 00000000..68af9d5e --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java @@ -0,0 +1,30 @@ +package org.team1540.robot2024.commands.indexer; + +import edu.wpi.first.wpilibj2.command.Command; +import org.team1540.robot2024.subsystems.indexer.Indexer; + +public class IntakeCommand extends Command { + + private final Indexer indexer; + + public IntakeCommand(Indexer indexer) { + this.indexer = indexer; + addRequirements(indexer); + } + + @Override + public void initialize() { + indexer.setIntakePercent(0.4); + } + + + @Override + public boolean isFinished() { + return indexer.noteInIndexer(); + } + + @Override + public void end(boolean interrupted) { + indexer.setIntakePercent(0); + } +} diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java index f3facc50..3688812b 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java @@ -1,5 +1,8 @@ package org.team1540.robot2024.subsystems.indexer; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; @@ -17,14 +20,29 @@ public void periodic() { Logger.processInputs("Indexer", indexerInputs); } - public void setIntakeSpeed(double speed) { - indexerIO.setIntakeSpeed(speed); + public void setIntakePercent(double percent) { + indexerIO.setIntakeVoltage(percent * 12.0); } - public void setIntakeVoltage(double volts) { - indexerIO.setIntakeVoltage(volts); + public boolean noteInIndexer() { + return indexerInputs.noteInIntake; } + public Command feedToAmp() { + return Commands.sequence( + Commands.runOnce(() -> indexerIO.setFeederVelocity(-600), this), + Commands.waitSeconds(5), + Commands.runOnce(() -> indexerIO.setFeederVelocity(0), this) + ); + } + + public Command feedToShooter() { + return Commands.sequence( + Commands.runOnce(() -> indexerIO.setFeederVelocity(1200), this), + Commands.waitSeconds(1), + Commands.runOnce(() -> indexerIO.setFeederVelocity(0), this) + ); + } } 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 23266270..a65aeab8 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java @@ -6,9 +6,13 @@ public interface IndexerIO { @AutoLog class IndexerIOInputs { - public double intakeVoltage = 0.0; - public double intakeCurrent = 0.0; - public double intakeVelocityRPM = 0.0; + public double intakeVoltage; + public double intakeCurrent; + public double intakeVelocityRPM; + public double feederVoltage; + public double feederCurrentAmps; + public double feederVelocityRPM; + public boolean noteInIntake; } /** @@ -18,8 +22,8 @@ default void updateInputs(IndexerIOInputs inputs) {} default void setIntakeVoltage(double volts) {} - default void setIntakeSpeed(double speed) {} - + default void setFeederVoltage(double volts) {} + default void setFeederVelocity(double velocity) {} } diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOReal.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOReal.java deleted file mode 100644 index 849aa338..00000000 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOReal.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.team1540.robot2024.subsystems.indexer; - -import com.revrobotics.CANSparkBase; -import com.revrobotics.CANSparkLowLevel; -import com.revrobotics.CANSparkMax; - -import static org.team1540.robot2024.Constants.Indexer.*; - -public class IndexerIOReal implements IndexerIO { - CANSparkMax intakeMotor = new CANSparkMax(INTAKE_ID, CANSparkLowLevel.MotorType.kBrushless); - - - public IndexerIOReal() { - intakeMotor.setIdleMode(CANSparkBase.IdleMode.kBrake); - intakeMotor.enableVoltageCompensation(12.0); - intakeMotor.setSmartCurrentLimit(30); - } - - @Override - public void updateInputs(IndexerIOInputs inputs) { - inputs.intakeCurrent = intakeMotor.getOutputCurrent(); - inputs.intakeVoltage = intakeMotor.getBusVoltage() * intakeMotor.getAppliedOutput(); - } - - @Override - public void setIntakeVoltage(double volts) { - intakeMotor.setVoltage(volts); - } - - @Override - public void setIntakeSpeed(double speed) { - intakeMotor.set(speed); - } -} diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java new file mode 100644 index 00000000..7e30bcd0 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java @@ -0,0 +1,74 @@ +package org.team1540.robot2024.subsystems.indexer; + +import com.revrobotics.*; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DigitalInput; + +import static org.team1540.robot2024.Constants.Indexer.*; + + +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 SparkPIDController feederPID; + private final SimpleMotorFeedforward feederFF = new SimpleMotorFeedforward(FEEDER_KS, FEEDER_KV); + + + public IndexerIOSparkMax() { + intakeMotor.setIdleMode(CANSparkBase.IdleMode.kBrake); + intakeMotor.enableVoltageCompensation(12.0); + intakeMotor.setSmartCurrentLimit(30); + + feederMotor.setIdleMode(CANSparkBase.IdleMode.kBrake); + feederMotor.enableVoltageCompensation(12.0); + feederMotor.setSmartCurrentLimit(30); + + feederPID = feederMotor.getPIDController(); + feederPID.setP(FEEDER_KP, 0); + feederPID.setI(FEEDER_KI, 0); + feederPID.setD(FEEDER_KD, 0); + + + + } + // 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.intakeVoltage = intakeMotor.getBusVoltage() * intakeMotor.getAppliedOutput(); + inputs.intakeVelocityRPM = intakeMotor.getEncoder().getVelocity(); + inputs.feederVoltage = feederMotor.getOutputCurrent(); + inputs.feederCurrentAmps = feederMotor.getBusVoltage() * feederMotor.getAppliedOutput(); + inputs.feederVelocityRPM = feederMotor.getEncoder().getVelocity(); + inputs.noteInIntake = indexerBeamBreak.get(); + } + + @Override + public void setIntakeVoltage(double volts) { + intakeMotor.setVoltage(volts); + } + + @Override + public void setFeederVoltage(double volts) { + feederMotor.setVoltage(volts); + } + + @Override + public void setFeederVelocity(double velocity) { + feederPID.setReference( + Units.radiansPerSecondToRotationsPerMinute(velocity) * GEAR_RATIO, + CANSparkBase.ControlType.kVelocity, + 0, + feederFF.calculate(velocity), + SparkPIDController.ArbFFUnits.kVoltage + ); + } +} +// for elevator sim, app. voltage \ No newline at end of file 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 03/11] 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 From 3f0b2e58412df83d7b497208f5d7809064eeaf3b Mon Sep 17 00:00:00 2001 From: Papplefarm20 <146298520+Papplefarm20@users.noreply.github.com> Date: Sat, 27 Jan 2024 22:02:06 -0800 Subject: [PATCH 04/11] fix: made gear ratios non-zero --- src/main/java/org/team1540/robot2024/Constants.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 2d6fe9ff..67d642d5 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -56,6 +56,7 @@ public static class Drivetrain { public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; } public static class Indexer { + // TODO: fix these constants public static final int INTAKE_ID = 0; public static final int FEEDER_ID = 0; public static final double FEEDER_KP = 0.0; @@ -63,8 +64,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 FEEDER_GEAR_RATIO = 0.0; - public static final double INTAKE_GEAR_RATIO = 0.0; + public static final double FEEDER_GEAR_RATIO = 1.0; + public static final double INTAKE_GEAR_RATIO = 1.0; } } From 671b55a363d58e3019754ced79967f014df1ccc8 Mon Sep 17 00:00:00 2001 From: Papplefarm20 <146298520+Papplefarm20@users.noreply.github.com> Date: Sat, 27 Jan 2024 22:02:06 -0800 Subject: [PATCH 05/11] fix: made gear ratios non-zero --- src/main/java/org/team1540/robot2024/Constants.java | 5 +++-- .../team1540/robot2024/subsystems/indexer/IndexerIO.java | 2 +- .../team1540/robot2024/subsystems/indexer/IndexerIOSim.java | 6 +++--- 3 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 2d6fe9ff..67d642d5 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -56,6 +56,7 @@ public static class Drivetrain { public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; } public static class Indexer { + // TODO: fix these constants public static final int INTAKE_ID = 0; public static final int FEEDER_ID = 0; public static final double FEEDER_KP = 0.0; @@ -63,8 +64,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 FEEDER_GEAR_RATIO = 0.0; - public static final double INTAKE_GEAR_RATIO = 0.0; + public static final double FEEDER_GEAR_RATIO = 1.0; + public static final double INTAKE_GEAR_RATIO = 1.0; } } 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 5ed351bc..93060ac9 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java @@ -12,7 +12,7 @@ class IndexerIOInputs { public double feederVoltage; public double feederCurrentAmps; public double feederVelocityRPM; - public boolean noteInIntake; + public boolean noteInIntake = false; } /** diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java index 347e371c..1252d170 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java @@ -14,7 +14,7 @@ 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 SimDeviceSim beamBreakSim = new SimDeviceSim("Indexer Beam Break"); private final PIDController feederSimPID = new PIDController(FEEDER_KP, FEEDER_KI,FEEDER_KD); @Override @@ -27,10 +27,10 @@ public void updateInputs(IndexerIOInputs inputs) { inputs.feederCurrentAmps = feederSim.getCurrentDrawAmps(); // inputs.feederVoltage = feederSim.getBusVoltage() * feederSim.getAppliedOutput(); inputs.feederVelocityRPM = feederSim.getAngularVelocityRPM(); - inputs.noteInIntake = beamBreakSim.getBoolean("Indexer Beam Break").get(); +// 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())); + feederSim.setState(feederSim.getAngularPositionRad(), feederSimPID.calculate(feederSim.getAngularVelocityRadPerSec())); } From b906548e721fba0239853e2520e4dd9aebfb2602 Mon Sep 17 00:00:00 2001 From: Papplefarm20 <146298520+Papplefarm20@users.noreply.github.com> Date: Mon, 29 Jan 2024 02:34:34 -0800 Subject: [PATCH 06/11] fix: made simulation somewhat working --- .../org/team1540/robot2024/Constants.java | 10 ++++----- .../robot2024/subsystems/drive/Module.java | 2 +- .../robot2024/subsystems/indexer/Indexer.java | 12 +++++++++++ .../subsystems/indexer/IndexerIO.java | 5 +++++ .../subsystems/indexer/IndexerIOSim.java | 10 ++++++++- .../robot2024/util/LoggedTunableNumber.java | 21 +++++++++++++++++++ 6 files changed, 53 insertions(+), 7 deletions(-) create mode 100644 src/main/java/org/team1540/robot2024/util/LoggedTunableNumber.java diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 67d642d5..3b2bece4 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -57,11 +57,11 @@ public static class Drivetrain { } public static class Indexer { // TODO: fix these constants - public static final int INTAKE_ID = 0; - public static final int FEEDER_ID = 0; - public static final double FEEDER_KP = 0.0; - public static final double FEEDER_KI = 0.0; - public static final double FEEDER_KD = 0.0; + public static final int INTAKE_ID = 11; + public static final int FEEDER_ID = 12; + public static final double FEEDER_KP = 0.5; + public static final double FEEDER_KI = 0.1; + public static final double FEEDER_KD = 0.001; public static final double FEEDER_KS = 0.0; public static final double FEEDER_KV = 0.0; public static final double FEEDER_GEAR_RATIO = 1.0; diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java index b63b8b38..92b82226 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java @@ -55,7 +55,7 @@ public Module(ModuleIO io, int index) { public void periodic() { io.updateInputs(inputs); - Logger.processInputs("Drive/Module" + index, inputs); + Logger.processInputs("Drivetrain/Module" + index, inputs); // On first cycle, reset relative turn encoder // Wait until absolute angle is nonzero in case it wasn't initialized yet diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java index 3688812b..503c6a30 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java @@ -5,11 +5,18 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; +import org.team1540.robot2024.util.LoggedTunableNumber; +import static org.team1540.robot2024.Constants.Indexer.*; public class Indexer extends SubsystemBase { private final IndexerIO indexerIO; private final IndexerIOInputsAutoLogged indexerInputs = new IndexerIOInputsAutoLogged(); + private final boolean TUNING = true; + private final LoggedTunableNumber kP = new LoggedTunableNumber("Indexer/kP", FEEDER_KP); + private final LoggedTunableNumber kI = new LoggedTunableNumber("Indexer/kI", FEEDER_KI); + private final LoggedTunableNumber kD = new LoggedTunableNumber("Indexer/kD", FEEDER_KD); + public Indexer(IndexerIO indexerIO) { this.indexerIO = indexerIO; @@ -18,6 +25,11 @@ public Indexer(IndexerIO indexerIO) { public void periodic() { indexerIO.updateInputs(indexerInputs); Logger.processInputs("Indexer", indexerInputs); + if (TUNING) { + if (kP.hasChanged() || kI.hasChanged() || kD.hasChanged()) { + indexerIO.configurePID(kP.get(), kI.get(), kD.get()); + } + } } public void setIntakePercent(double percent) { 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 93060ac9..baff5462 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java @@ -13,6 +13,9 @@ class IndexerIOInputs { public double feederCurrentAmps; public double feederVelocityRPM; public boolean noteInIntake = false; + public double setpoint; + public double feederVelocityRadPerSec; + public double feederPositionError; } /** @@ -26,4 +29,6 @@ default void setFeederVoltage(double volts) {} default void setFeederVelocity(double velocity) {} + default void configurePID(double p, double i, double d) {} + } diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java index 1252d170..cf5a209f 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java @@ -28,9 +28,12 @@ public void updateInputs(IndexerIOInputs inputs) { // inputs.feederVoltage = feederSim.getBusVoltage() * feederSim.getAppliedOutput(); inputs.feederVelocityRPM = feederSim.getAngularVelocityRPM(); // inputs.noteInIntake = beamBreakSim.getBoolean("Indexer Beam Break").get(); + inputs.setpoint = feederSimPID.getSetpoint(); + inputs.feederVelocityRadPerSec = feederSim.getAngularVelocityRadPerSec(); + inputs.feederPositionError = feederSimPID.getPositionError(); // 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())); + feederSim.setState(feederSim.getAngularPositionRad(), feederSim.getAngularVelocityRadPerSec() + feederSimPID.calculate(feederSim.getAngularVelocityRadPerSec())); } @@ -44,6 +47,11 @@ public void setFeederVelocity(double velocity) { feederSimPID.setSetpoint(velocity); } + @Override + public void configurePID(double p, double i, double d) { + feederSimPID.setPID(p, i, d); + } + @Override public void setFeederVoltage(double volts) { feederSim.setInputVoltage(MathUtil.clamp(volts, -12, 12)); diff --git a/src/main/java/org/team1540/robot2024/util/LoggedTunableNumber.java b/src/main/java/org/team1540/robot2024/util/LoggedTunableNumber.java new file mode 100644 index 00000000..80750bda --- /dev/null +++ b/src/main/java/org/team1540/robot2024/util/LoggedTunableNumber.java @@ -0,0 +1,21 @@ +package org.team1540.robot2024.util; + +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; + +public class LoggedTunableNumber extends LoggedDashboardNumber { + private double lastHasChangedValue; + + public LoggedTunableNumber(String key, double defaultValue) { + super(key, defaultValue); + lastHasChangedValue = defaultValue; + } + + public boolean hasChanged() { + double currentValue = get(); + if (currentValue != lastHasChangedValue) { + lastHasChangedValue = currentValue; + return true; + } + return false; + } +} \ No newline at end of file From 94dcca9d46d866d36fc2dcf30c07a51f810826af Mon Sep 17 00:00:00 2001 From: Papplefarm20 <146298520+Papplefarm20@users.noreply.github.com> Date: Wed, 31 Jan 2024 12:25:38 -0800 Subject: [PATCH 07/11] fix: made simulations work correctly(?) --- .../robot2024/subsystems/indexer/Indexer.java | 2 +- .../subsystems/indexer/IndexerIO.java | 3 +- .../subsystems/indexer/IndexerIOSim.java | 39 +++++++++++-------- .../subsystems/indexer/IndexerIOSparkMax.java | 10 ++--- 4 files changed, 30 insertions(+), 24 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java index 503c6a30..ae2c4f0b 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java @@ -26,7 +26,7 @@ public void periodic() { indexerIO.updateInputs(indexerInputs); Logger.processInputs("Indexer", indexerInputs); if (TUNING) { - if (kP.hasChanged() || kI.hasChanged() || kD.hasChanged()) { + if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode()) || kD.hasChanged(hashCode())) { indexerIO.configurePID(kP.get(), kI.get(), kD.get()); } } 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 baff5462..66a0171c 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java @@ -13,8 +13,7 @@ class IndexerIOInputs { public double feederCurrentAmps; public double feederVelocityRPM; public boolean noteInIntake = false; - public double setpoint; - public double feederVelocityRadPerSec; + public double setpointRPM; public double feederPositionError; } diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java index cf5a209f..ab8ef4d7 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java @@ -16,35 +16,34 @@ public class IndexerIOSim implements IndexerIO { 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); + private boolean isClosedLoop = true; + private double intakeVoltage = 0.0; + private double feederVoltage = 0.0; + private double feederVelocityRPS = 0.0; @Override public void updateInputs(IndexerIOInputs inputs) { + if (isClosedLoop) { + feederVoltage = MathUtil.clamp(feederSimPID.calculate(feederSim.getAngularVelocityRPM() / 60, feederVelocityRPS), -12.0, 12.0); + } + intakeSim.setInputVoltage(intakeVoltage); + feederSim.setInputVoltage(feederVoltage); intakeSim.update(Constants.LOOP_PERIOD_SECS); feederSim.update(Constants.LOOP_PERIOD_SECS); inputs.intakeCurrentAmps = intakeSim.getCurrentDrawAmps(); -// inputs.intakeVoltage = intakeSim.getBusVoltage() * intakeSim.getAppliedOutput(); + inputs.intakeVoltage = intakeVoltage; inputs.intakeVelocityRPM = intakeSim.getAngularVelocityRPM(); inputs.feederCurrentAmps = feederSim.getCurrentDrawAmps(); -// inputs.feederVoltage = feederSim.getBusVoltage() * feederSim.getAppliedOutput(); + inputs.feederVoltage = feederVoltage; inputs.feederVelocityRPM = feederSim.getAngularVelocityRPM(); // inputs.noteInIntake = beamBreakSim.getBoolean("Indexer Beam Break").get(); - inputs.setpoint = feederSimPID.getSetpoint(); - inputs.feederVelocityRadPerSec = feederSim.getAngularVelocityRadPerSec(); + inputs.setpointRPM = feederSimPID.getSetpoint() * 60; inputs.feederPositionError = feederSimPID.getPositionError(); - - // 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(), feederSim.getAngularVelocityRadPerSec() + 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); + intakeVoltage = MathUtil.clamp(volts, -12.0, 12.0); } @Override @@ -52,8 +51,16 @@ public void configurePID(double p, double i, double d) { feederSimPID.setPID(p, i, d); } - @Override public void setFeederVoltage(double volts) { - feederSim.setInputVoltage(MathUtil.clamp(volts, -12, 12)); + isClosedLoop = false; + feederVoltage = MathUtil.clamp(volts, -12.0, 12.0); + } + + + public void setFeederVelocity(double velocity) { + isClosedLoop = true; + feederSimPID.reset(); + feederVelocityRPS = velocity / 60; } + } 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 2cf45ddb..110f8192 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java @@ -2,7 +2,6 @@ import com.revrobotics.*; import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DigitalInput; import static org.team1540.robot2024.Constants.Indexer.*; @@ -14,6 +13,7 @@ public class IndexerIOSparkMax implements IndexerIO { private final DigitalInput indexerBeamBreak = new DigitalInput(0); private final SparkPIDController feederPID; private final SimpleMotorFeedforward feederFF = new SimpleMotorFeedforward(FEEDER_KS, FEEDER_KV); + private double setpointRPM; public IndexerIOSparkMax() { @@ -29,9 +29,6 @@ public IndexerIOSparkMax() { feederPID.setP(FEEDER_KP, 0); feederPID.setI(FEEDER_KI, 0); feederPID.setD(FEEDER_KD, 0); - - - } @Override @@ -43,6 +40,8 @@ public void updateInputs(IndexerIOInputs inputs) { inputs.feederVoltage = feederMotor.getBusVoltage() * feederMotor.getAppliedOutput(); inputs.feederVelocityRPM = feederMotor.getEncoder().getVelocity(); inputs.noteInIntake = indexerBeamBreak.get(); + inputs.setpointRPM = setpointRPM; + inputs.feederPositionError = setpointRPM - feederMotor.getEncoder().getVelocity(); } @Override @@ -57,8 +56,9 @@ public void setFeederVoltage(double volts) { @Override public void setFeederVelocity(double velocity) { + setpointRPM = velocity; feederPID.setReference( - Units.radiansPerSecondToRotationsPerMinute(velocity) * FEEDER_GEAR_RATIO, + velocity / FEEDER_GEAR_RATIO, // Should this be multiplied? CANSparkBase.ControlType.kVelocity, 0, feederFF.calculate(velocity), From 952ed9b6802cf6e540efb8a14fc4f5b52cf507aa Mon Sep 17 00:00:00 2001 From: Papplefarm20 <146298520+Papplefarm20@users.noreply.github.com> Date: Fri, 2 Feb 2024 09:09:33 -0800 Subject: [PATCH 08/11] fix: made values correctly be in constants, and made field and method names clearer --- .../java/org/team1540/robot2024/Constants.java | 4 ++++ .../robot2024/commands/indexer/IntakeCommand.java | 2 +- .../robot2024/subsystems/indexer/Indexer.java | 9 ++++----- .../robot2024/subsystems/indexer/IndexerIO.java | 4 ++-- .../robot2024/subsystems/indexer/IndexerIOSim.java | 14 +++++++------- .../subsystems/indexer/IndexerIOSparkMax.java | 11 +++++++++-- 6 files changed, 27 insertions(+), 17 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 33f3e5aa..553db50c 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -69,6 +69,10 @@ public static class Indexer { public static final double FEEDER_KV = 0.0; public static final double FEEDER_GEAR_RATIO = 1.0; public static final double INTAKE_GEAR_RATIO = 1.0; + public static final double INTAKE_MOI = 0.025; + public static final double FEEDER_MOI = 0.025; + public static final int BEAM_BREAK_ID = 0; + } diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java b/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java index 68af9d5e..72df4da5 100644 --- a/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java @@ -20,7 +20,7 @@ public void initialize() { @Override public boolean isFinished() { - return indexer.noteInIndexer(); + return indexer.isNotePresent(); } @Override diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java index ae2c4f0b..d832a173 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java @@ -1,18 +1,17 @@ package org.team1540.robot2024.subsystems.indexer; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; import org.team1540.robot2024.util.LoggedTunableNumber; import static org.team1540.robot2024.Constants.Indexer.*; +import static org.team1540.robot2024.Constants.tuningMode; public class Indexer extends SubsystemBase { private final IndexerIO indexerIO; private final IndexerIOInputsAutoLogged indexerInputs = new IndexerIOInputsAutoLogged(); - private final boolean TUNING = true; private final LoggedTunableNumber kP = new LoggedTunableNumber("Indexer/kP", FEEDER_KP); private final LoggedTunableNumber kI = new LoggedTunableNumber("Indexer/kI", FEEDER_KI); private final LoggedTunableNumber kD = new LoggedTunableNumber("Indexer/kD", FEEDER_KD); @@ -25,9 +24,9 @@ public Indexer(IndexerIO indexerIO) { public void periodic() { indexerIO.updateInputs(indexerInputs); Logger.processInputs("Indexer", indexerInputs); - if (TUNING) { + if (tuningMode) { if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode()) || kD.hasChanged(hashCode())) { - indexerIO.configurePID(kP.get(), kI.get(), kD.get()); + indexerIO.configureFeederPID(kP.get(), kI.get(), kD.get()); } } } @@ -36,7 +35,7 @@ public void setIntakePercent(double percent) { indexerIO.setIntakeVoltage(percent * 12.0); } - public boolean noteInIndexer() { + public boolean isNotePresent() { return indexerInputs.noteInIntake; } 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 66a0171c..f08a1858 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java @@ -14,7 +14,7 @@ class IndexerIOInputs { public double feederVelocityRPM; public boolean noteInIntake = false; public double setpointRPM; - public double feederPositionError; + public double feederVelocityError; } /** @@ -28,6 +28,6 @@ default void setFeederVoltage(double volts) {} default void setFeederVelocity(double velocity) {} - default void configurePID(double p, double i, double d) {} + default void configureFeederPID(double p, double i, double d) {} } diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java index ab8ef4d7..ea11766f 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java @@ -12,19 +12,19 @@ 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 DCMotorSim intakeSim = new DCMotorSim(DCMotor.getNEO(1), INTAKE_GEAR_RATIO, INTAKE_MOI); + private final DCMotorSim feederSim = new DCMotorSim(DCMotor.getNEO(1), FEEDER_GEAR_RATIO, FEEDER_MOI); // private final SimDeviceSim beamBreakSim = new SimDeviceSim("Indexer Beam Break"); private final PIDController feederSimPID = new PIDController(FEEDER_KP, FEEDER_KI,FEEDER_KD); private boolean isClosedLoop = true; private double intakeVoltage = 0.0; private double feederVoltage = 0.0; - private double feederVelocityRPS = 0.0; + private double feederSetpointRPS = 0.0; @Override public void updateInputs(IndexerIOInputs inputs) { if (isClosedLoop) { - feederVoltage = MathUtil.clamp(feederSimPID.calculate(feederSim.getAngularVelocityRPM() / 60, feederVelocityRPS), -12.0, 12.0); + feederVoltage = MathUtil.clamp(feederSimPID.calculate(feederSim.getAngularVelocityRPM() / 60, feederSetpointRPS), -12.0, 12.0); } intakeSim.setInputVoltage(intakeVoltage); feederSim.setInputVoltage(feederVoltage); @@ -38,7 +38,7 @@ public void updateInputs(IndexerIOInputs inputs) { inputs.feederVelocityRPM = feederSim.getAngularVelocityRPM(); // inputs.noteInIntake = beamBreakSim.getBoolean("Indexer Beam Break").get(); inputs.setpointRPM = feederSimPID.getSetpoint() * 60; - inputs.feederPositionError = feederSimPID.getPositionError(); + inputs.feederVelocityError = feederSimPID.getPositionError(); } @Override @@ -47,7 +47,7 @@ public void setIntakeVoltage(double volts) { } @Override - public void configurePID(double p, double i, double d) { + public void configureFeederPID(double p, double i, double d) { feederSimPID.setPID(p, i, d); } @@ -60,7 +60,7 @@ public void setFeederVoltage(double volts) { public void setFeederVelocity(double velocity) { isClosedLoop = true; feederSimPID.reset(); - feederVelocityRPS = velocity / 60; + feederSetpointRPS = velocity / 60; } } 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 110f8192..18c9686b 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java @@ -10,7 +10,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); - private final DigitalInput indexerBeamBreak = new DigitalInput(0); + private final DigitalInput indexerBeamBreak = new DigitalInput(BEAM_BREAK_ID); private final SparkPIDController feederPID; private final SimpleMotorFeedforward feederFF = new SimpleMotorFeedforward(FEEDER_KS, FEEDER_KV); private double setpointRPM; @@ -41,7 +41,7 @@ public void updateInputs(IndexerIOInputs inputs) { inputs.feederVelocityRPM = feederMotor.getEncoder().getVelocity(); inputs.noteInIntake = indexerBeamBreak.get(); inputs.setpointRPM = setpointRPM; - inputs.feederPositionError = setpointRPM - feederMotor.getEncoder().getVelocity(); + inputs.feederVelocityError = setpointRPM - feederMotor.getEncoder().getVelocity(); } @Override @@ -65,4 +65,11 @@ public void setFeederVelocity(double velocity) { SparkPIDController.ArbFFUnits.kVoltage ); } + + @Override + public void configureFeederPID(double p, double i, double d) { + feederPID.setP(p); + feederPID.setI(i); + feederPID.setD(d); + } } From c554380513b6699c61fd691483c1e8681015efec Mon Sep 17 00:00:00 2001 From: Papplefarm20 <146298520+Papplefarm20@users.noreply.github.com> Date: Fri, 2 Feb 2024 11:38:59 -0800 Subject: [PATCH 09/11] fix: made feeder factory commands not return a command sequence --- .../java/org/team1540/robot2024/RobotContainer.java | 1 + .../robot2024/subsystems/indexer/Indexer.java | 12 ++---------- 2 files changed, 3 insertions(+), 10 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 1f5f0645..8931b30b 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -139,6 +139,7 @@ private void configureButtonBindings() { copilot.a().onTrue(shooter.spinUpCommand(leftFlywheelSetpoint.get(), rightFlywheelSetpoint.get())) .onFalse(Commands.runOnce(shooter::stopFlywheels, shooter)); + // these are just for testing the feeder 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/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java index d832a173..37afbc62 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java @@ -40,19 +40,11 @@ public boolean isNotePresent() { } public Command feedToAmp() { - return Commands.sequence( - Commands.runOnce(() -> indexerIO.setFeederVelocity(-600), this), - Commands.waitSeconds(5), - Commands.runOnce(() -> indexerIO.setFeederVelocity(0), this) - ); + return Commands.runOnce(() -> indexerIO.setFeederVelocity(-600), this); } public Command feedToShooter() { - return Commands.sequence( - Commands.runOnce(() -> indexerIO.setFeederVelocity(1200), this), - Commands.waitSeconds(1), - Commands.runOnce(() -> indexerIO.setFeederVelocity(0), this) - ); + return Commands.runOnce(() -> indexerIO.setFeederVelocity(1200), this); } From 08023a354b363bb7b812c857e77a8c89602a49aa Mon Sep 17 00:00:00 2001 From: Papplefarm20 <146298520+Papplefarm20@users.noreply.github.com> Date: Fri, 2 Feb 2024 15:04:09 -0800 Subject: [PATCH 10/11] fix: removed not good controller bindings --- src/main/java/org/team1540/robot2024/RobotContainer.java | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 8931b30b..a7463712 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -139,9 +139,6 @@ private void configureButtonBindings() { copilot.a().onTrue(shooter.spinUpCommand(leftFlywheelSetpoint.get(), rightFlywheelSetpoint.get())) .onFalse(Commands.runOnce(shooter::stopFlywheels, shooter)); - // these are just for testing the feeder - copilot.x().onTrue(indexer.feedToAmp()); - copilot.y().onTrue(indexer.feedToShooter()); driver.a().onTrue(new IntakeCommand(indexer)); } From 3195a97908fdb3525871c3d77cd3d05634fd06be Mon Sep 17 00:00:00 2001 From: Papplefarm20 <146298520+Papplefarm20@users.noreply.github.com> Date: Fri, 2 Feb 2024 15:14:38 -0800 Subject: [PATCH 11/11] fix: made indexer not listed as a controller --- src/main/java/org/team1540/robot2024/RobotContainer.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index a7463712..0d65987d 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -34,11 +34,12 @@ public class RobotContainer { // Subsystems public final Drivetrain drivetrain; public final Shooter shooter; + public final Indexer indexer; // 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;