Skip to content
This repository was archived by the owner on Jun 20, 2025. It is now read-only.

Commit b2b1c7e

Browse files
rutmanzrutmanz
authored
Merge pull request #19 from flamingchickens1540/intake
Intake and indexer
2 parents 552691f + ee8c739 commit b2b1c7e

File tree

7 files changed

+297
-1
lines changed

7 files changed

+297
-1
lines changed

‎src/main/java/org/team1540/robot2024/Constants.java

Lines changed: 18 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@ public final class Constants {
1818
// Whether to pull PID constants from SmartDashboard
1919
public static final boolean tuningMode = true; // TODO: DO NOT SET TO TRUE FOR COMP
2020
private static final Mode simMode = Mode.SIM; // Can also be Mode.REPLAY
21-
21+
2222
public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode;
2323

2424
public enum Mode {
@@ -63,6 +63,23 @@ public static class Drivetrain {
6363
public static final double DRIVE_BASE_RADIUS = Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0);
6464
public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;
6565
}
66+
public static class Indexer {
67+
// TODO: fix these constants
68+
public static final int INTAKE_ID = 11;
69+
public static final int FEEDER_ID = 12;
70+
public static final double FEEDER_KP = 0.5;
71+
public static final double FEEDER_KI = 0.1;
72+
public static final double FEEDER_KD = 0.001;
73+
public static final double FEEDER_KS = 0.0;
74+
public static final double FEEDER_KV = 0.0;
75+
public static final double FEEDER_GEAR_RATIO = 1.0;
76+
public static final double INTAKE_GEAR_RATIO = 1.0;
77+
public static final double INTAKE_MOI = 0.025;
78+
public static final double FEEDER_MOI = 0.025;
79+
public static final int BEAM_BREAK_ID = 0;
80+
81+
82+
}
6683

6784

6885
public static class Vision {

‎src/main/java/org/team1540/robot2024/RobotContainer.java

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@
1212
import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
1313
import org.team1540.robot2024.commands.FeedForwardCharacterization;
1414
import org.team1540.robot2024.commands.SwerveDriveCommand;
15+
import org.team1540.robot2024.commands.indexer.IntakeCommand;
1516
import org.team1540.robot2024.subsystems.drive.*;
1617
import org.team1540.robot2024.subsystems.tramp.Tramp;
1718
import org.team1540.robot2024.subsystems.tramp.TrampIO;
@@ -23,6 +24,10 @@
2324
import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOLimelight;
2425
import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOSim;
2526
import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher;
27+
import org.team1540.robot2024.subsystems.indexer.Indexer;
28+
import org.team1540.robot2024.subsystems.indexer.IndexerIO;
29+
import org.team1540.robot2024.subsystems.indexer.IndexerIOSim;
30+
import org.team1540.robot2024.subsystems.indexer.IndexerIOSparkMax;
2631
import org.team1540.robot2024.util.swerve.SwerveFactory;
2732
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
2833
import org.team1540.robot2024.util.vision.VisionPoseAcceptor;
@@ -40,12 +45,14 @@ public class RobotContainer {
4045
public final Drivetrain drivetrain;
4146
public final Tramp tramp;
4247
public final Shooter shooter;
48+
public final Indexer indexer;
4349
public final AprilTagVision aprilTagVision;
4450

4551
// Controller
4652
public final CommandXboxController driver = new CommandXboxController(0);
4753
public final CommandXboxController copilot = new CommandXboxController(1);
4854

55+
4956
// Dashboard inputs
5057
public final LoggedDashboardChooser<Command> autoChooser;
5158

@@ -71,6 +78,10 @@ public RobotContainer() {
7178
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT), odometrySignalRefresher));
7279
tramp = new Tramp(new TrampIOSparkMax());
7380
shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX());
81+
indexer =
82+
new Indexer(
83+
new IndexerIOSparkMax()
84+
);
7485
aprilTagVision = new AprilTagVision(
7586
new AprilTagVisionIOLimelight(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE),
7687
new AprilTagVisionIOLimelight(Constants.Vision.REAR_CAMERA_NAME, Constants.Vision.REAR_CAMERA_POSE),
@@ -96,6 +107,10 @@ public RobotContainer() {
96107
ignored -> {},
97108
() -> 0.0, // TODO: ACTUALLY GET ELEVATOR HEIGHT HERE
98109
new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)); // TODO: ACTUALLY GET ELEVATOR VELOCITY HERE
110+
indexer =
111+
new Indexer(
112+
new IndexerIOSim()
113+
);
99114
break;
100115
default:
101116
// Replayed robot, disable IO implementations
@@ -114,7 +129,14 @@ public RobotContainer() {
114129
(ignored) -> {},
115130
() -> 0.0,
116131
new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0));
132+
133+
indexer =
134+
new Indexer(
135+
new IndexerIO() {}
136+
);
137+
117138
tramp = new Tramp(new TrampIO() {});
139+
118140
break;
119141
}
120142

@@ -154,6 +176,8 @@ private void configureButtonBindings() {
154176

155177
copilot.a().onTrue(shooter.spinUpCommand(leftFlywheelSetpoint.get(), rightFlywheelSetpoint.get()))
156178
.onFalse(Commands.runOnce(shooter::stopFlywheels, shooter));
179+
180+
driver.a().onTrue(new IntakeCommand(indexer));
157181
}
158182

159183
/**

‎src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
package org.team1540.robot2024.commands.indexer;
2+
3+
import edu.wpi.first.wpilibj2.command.Command;
4+
import org.team1540.robot2024.subsystems.indexer.Indexer;
5+
6+
public class IntakeCommand extends Command {
7+
8+
private final Indexer indexer;
9+
10+
public IntakeCommand(Indexer indexer) {
11+
this.indexer = indexer;
12+
addRequirements(indexer);
13+
}
14+
15+
@Override
16+
public void initialize() {
17+
indexer.setIntakePercent(0.4);
18+
}
19+
20+
21+
@Override
22+
public boolean isFinished() {
23+
return indexer.isNotePresent();
24+
}
25+
26+
@Override
27+
public void end(boolean interrupted) {
28+
indexer.setIntakePercent(0);
29+
}
30+
}

‎src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java

Lines changed: 51 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,51 @@
1+
package org.team1540.robot2024.subsystems.indexer;
2+
3+
import edu.wpi.first.wpilibj2.command.Command;
4+
import edu.wpi.first.wpilibj2.command.Commands;
5+
import edu.wpi.first.wpilibj2.command.SubsystemBase;
6+
import org.littletonrobotics.junction.Logger;
7+
import org.team1540.robot2024.util.LoggedTunableNumber;
8+
import static org.team1540.robot2024.Constants.Indexer.*;
9+
import static org.team1540.robot2024.Constants.tuningMode;
10+
11+
12+
public class Indexer extends SubsystemBase {
13+
private final IndexerIO indexerIO;
14+
private final IndexerIOInputsAutoLogged indexerInputs = new IndexerIOInputsAutoLogged();
15+
private final LoggedTunableNumber kP = new LoggedTunableNumber("Indexer/kP", FEEDER_KP);
16+
private final LoggedTunableNumber kI = new LoggedTunableNumber("Indexer/kI", FEEDER_KI);
17+
private final LoggedTunableNumber kD = new LoggedTunableNumber("Indexer/kD", FEEDER_KD);
18+
19+
20+
public Indexer(IndexerIO indexerIO) {
21+
this.indexerIO = indexerIO;
22+
}
23+
24+
public void periodic() {
25+
indexerIO.updateInputs(indexerInputs);
26+
Logger.processInputs("Indexer", indexerInputs);
27+
if (tuningMode) {
28+
if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode()) || kD.hasChanged(hashCode())) {
29+
indexerIO.configureFeederPID(kP.get(), kI.get(), kD.get());
30+
}
31+
}
32+
}
33+
34+
public void setIntakePercent(double percent) {
35+
indexerIO.setIntakeVoltage(percent * 12.0);
36+
}
37+
38+
public boolean isNotePresent() {
39+
return indexerInputs.noteInIntake;
40+
}
41+
42+
public Command feedToAmp() {
43+
return Commands.runOnce(() -> indexerIO.setFeederVelocity(-600), this);
44+
}
45+
46+
public Command feedToShooter() {
47+
return Commands.runOnce(() -> indexerIO.setFeederVelocity(1200), this);
48+
}
49+
50+
51+
}

‎src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
package org.team1540.robot2024.subsystems.indexer;
2+
3+
import org.littletonrobotics.junction.AutoLog;
4+
5+
public interface IndexerIO {
6+
7+
@AutoLog
8+
class IndexerIOInputs {
9+
public double intakeVoltage;
10+
public double intakeCurrentAmps;
11+
public double intakeVelocityRPM;
12+
public double feederVoltage;
13+
public double feederCurrentAmps;
14+
public double feederVelocityRPM;
15+
public boolean noteInIntake = false;
16+
public double setpointRPM;
17+
public double feederVelocityError;
18+
}
19+
20+
/**
21+
* Updates the set of loggable inputs.
22+
*/
23+
default void updateInputs(IndexerIOInputs inputs) {}
24+
25+
default void setIntakeVoltage(double volts) {}
26+
27+
default void setFeederVoltage(double volts) {}
28+
29+
default void setFeederVelocity(double velocity) {}
30+
31+
default void configureFeederPID(double p, double i, double d) {}
32+
33+
}

‎src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
package org.team1540.robot2024.subsystems.indexer;
2+
3+
import edu.wpi.first.math.MathUtil;
4+
import edu.wpi.first.math.controller.PIDController;
5+
import edu.wpi.first.math.system.plant.DCMotor;
6+
import edu.wpi.first.wpilibj.simulation.DCMotorSim;
7+
import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
8+
import org.team1540.robot2024.Constants;
9+
10+
import static org.team1540.robot2024.Constants.Indexer.*;
11+
12+
public class IndexerIOSim implements IndexerIO {
13+
14+
15+
private final DCMotorSim intakeSim = new DCMotorSim(DCMotor.getNEO(1), INTAKE_GEAR_RATIO, INTAKE_MOI);
16+
private final DCMotorSim feederSim = new DCMotorSim(DCMotor.getNEO(1), FEEDER_GEAR_RATIO, FEEDER_MOI);
17+
// private final SimDeviceSim beamBreakSim = new SimDeviceSim("Indexer Beam Break");
18+
private final PIDController feederSimPID = new PIDController(FEEDER_KP, FEEDER_KI,FEEDER_KD);
19+
private boolean isClosedLoop = true;
20+
private double intakeVoltage = 0.0;
21+
private double feederVoltage = 0.0;
22+
private double feederSetpointRPS = 0.0;
23+
24+
@Override
25+
public void updateInputs(IndexerIOInputs inputs) {
26+
if (isClosedLoop) {
27+
feederVoltage = MathUtil.clamp(feederSimPID.calculate(feederSim.getAngularVelocityRPM() / 60, feederSetpointRPS), -12.0, 12.0);
28+
}
29+
intakeSim.setInputVoltage(intakeVoltage);
30+
feederSim.setInputVoltage(feederVoltage);
31+
intakeSim.update(Constants.LOOP_PERIOD_SECS);
32+
feederSim.update(Constants.LOOP_PERIOD_SECS);
33+
inputs.intakeCurrentAmps = intakeSim.getCurrentDrawAmps();
34+
inputs.intakeVoltage = intakeVoltage;
35+
inputs.intakeVelocityRPM = intakeSim.getAngularVelocityRPM();
36+
inputs.feederCurrentAmps = feederSim.getCurrentDrawAmps();
37+
inputs.feederVoltage = feederVoltage;
38+
inputs.feederVelocityRPM = feederSim.getAngularVelocityRPM();
39+
// inputs.noteInIntake = beamBreakSim.getBoolean("Indexer Beam Break").get();
40+
inputs.setpointRPM = feederSimPID.getSetpoint() * 60;
41+
inputs.feederVelocityError = feederSimPID.getPositionError();
42+
}
43+
44+
@Override
45+
public void setIntakeVoltage(double volts) {
46+
intakeVoltage = MathUtil.clamp(volts, -12.0, 12.0);
47+
}
48+
49+
@Override
50+
public void configureFeederPID(double p, double i, double d) {
51+
feederSimPID.setPID(p, i, d);
52+
}
53+
54+
public void setFeederVoltage(double volts) {
55+
isClosedLoop = false;
56+
feederVoltage = MathUtil.clamp(volts, -12.0, 12.0);
57+
}
58+
59+
60+
public void setFeederVelocity(double velocity) {
61+
isClosedLoop = true;
62+
feederSimPID.reset();
63+
feederSetpointRPS = velocity / 60;
64+
}
65+
66+
}

‎src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java

Lines changed: 75 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,75 @@
1+
package org.team1540.robot2024.subsystems.indexer;
2+
3+
import com.revrobotics.*;
4+
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
5+
import edu.wpi.first.wpilibj.DigitalInput;
6+
7+
import static org.team1540.robot2024.Constants.Indexer.*;
8+
9+
10+
public class IndexerIOSparkMax implements IndexerIO {
11+
private final CANSparkMax intakeMotor = new CANSparkMax(INTAKE_ID, CANSparkLowLevel.MotorType.kBrushless);
12+
private final CANSparkMax feederMotor = new CANSparkMax(FEEDER_ID, CANSparkLowLevel.MotorType.kBrushless);
13+
private final DigitalInput indexerBeamBreak = new DigitalInput(BEAM_BREAK_ID);
14+
private final SparkPIDController feederPID;
15+
private final SimpleMotorFeedforward feederFF = new SimpleMotorFeedforward(FEEDER_KS, FEEDER_KV);
16+
private double setpointRPM;
17+
18+
19+
public IndexerIOSparkMax() {
20+
intakeMotor.setIdleMode(CANSparkBase.IdleMode.kBrake);
21+
intakeMotor.enableVoltageCompensation(12.0);
22+
intakeMotor.setSmartCurrentLimit(30);
23+
24+
feederMotor.setIdleMode(CANSparkBase.IdleMode.kBrake);
25+
feederMotor.enableVoltageCompensation(12.0);
26+
feederMotor.setSmartCurrentLimit(30);
27+
28+
feederPID = feederMotor.getPIDController();
29+
feederPID.setP(FEEDER_KP, 0);
30+
feederPID.setI(FEEDER_KI, 0);
31+
feederPID.setD(FEEDER_KD, 0);
32+
}
33+
34+
@Override
35+
public void updateInputs(IndexerIOInputs inputs) {
36+
inputs.intakeCurrentAmps = intakeMotor.getOutputCurrent();
37+
inputs.intakeVoltage = intakeMotor.getBusVoltage() * intakeMotor.getAppliedOutput();
38+
inputs.intakeVelocityRPM = intakeMotor.getEncoder().getVelocity();
39+
inputs.feederCurrentAmps = feederMotor.getOutputCurrent();
40+
inputs.feederVoltage = feederMotor.getBusVoltage() * feederMotor.getAppliedOutput();
41+
inputs.feederVelocityRPM = feederMotor.getEncoder().getVelocity();
42+
inputs.noteInIntake = indexerBeamBreak.get();
43+
inputs.setpointRPM = setpointRPM;
44+
inputs.feederVelocityError = setpointRPM - feederMotor.getEncoder().getVelocity();
45+
}
46+
47+
@Override
48+
public void setIntakeVoltage(double volts) {
49+
intakeMotor.setVoltage(volts);
50+
}
51+
52+
@Override
53+
public void setFeederVoltage(double volts) {
54+
feederMotor.setVoltage(volts);
55+
}
56+
57+
@Override
58+
public void setFeederVelocity(double velocity) {
59+
setpointRPM = velocity;
60+
feederPID.setReference(
61+
velocity / FEEDER_GEAR_RATIO, // Should this be multiplied?
62+
CANSparkBase.ControlType.kVelocity,
63+
0,
64+
feederFF.calculate(velocity),
65+
SparkPIDController.ArbFFUnits.kVoltage
66+
);
67+
}
68+
69+
@Override
70+
public void configureFeederPID(double p, double i, double d) {
71+
feederPID.setP(p);
72+
feederPID.setI(i);
73+
feederPID.setD(d);
74+
}
75+
}

0 commit comments

Comments
 (0)