Skip to content

Commit 33b1517

Browse files
autumnwilkesautumnwilkes
committed
feat: Indexer Simulation
1 parent 61e7def commit 33b1517

File tree

5 files changed

+82
-13
lines changed

5 files changed

+82
-13
lines changed

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

+2-1
Original file line numberDiff line numberDiff line change
@@ -63,7 +63,8 @@ public static class Indexer {
6363
public static final double FEEDER_KD = 0.0;
6464
public static final double FEEDER_KS = 0.0;
6565
public static final double FEEDER_KV = 0.0;
66-
public static final double GEAR_RATIO = 0.0;
66+
public static final double FEEDER_GEAR_RATIO = 0.0;
67+
public static final double INTAKE_GEAR_RATIO = 0.0;
6768

6869
}
6970
}

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

+23
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,12 @@
1212

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.*;
17+
import org.team1540.robot2024.subsystems.indexer.Indexer;
18+
import org.team1540.robot2024.subsystems.indexer.IndexerIO;
19+
import org.team1540.robot2024.subsystems.indexer.IndexerIOSim;
20+
import org.team1540.robot2024.subsystems.indexer.IndexerIOSparkMax;
1621
import org.team1540.robot2024.util.swerve.SwerveFactory;
1722
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
1823

@@ -31,6 +36,7 @@ public class RobotContainer {
3136
// Controller
3237
public final CommandXboxController driver = new CommandXboxController(0);
3338
public final CommandXboxController copilot = new CommandXboxController(1);
39+
public final Indexer indexer;
3440

3541
// Dashboard inputs
3642
public final LoggedDashboardChooser<Command> autoChooser;
@@ -49,6 +55,11 @@ public RobotContainer() {
4955
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT)),
5056
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT)),
5157
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT)));
58+
59+
indexer =
60+
new Indexer(
61+
new IndexerIOSparkMax()
62+
);
5263
break;
5364

5465
case SIM:
@@ -60,6 +71,10 @@ public RobotContainer() {
6071
new ModuleIOSim(),
6172
new ModuleIOSim(),
6273
new ModuleIOSim());
74+
indexer =
75+
new Indexer(
76+
new IndexerIOSim()
77+
);
6378
break;
6479

6580
default:
@@ -76,6 +91,10 @@ public RobotContainer() {
7691
},
7792
new ModuleIO() {
7893
});
94+
indexer =
95+
new Indexer(
96+
new IndexerIO() {}
97+
);
7998
break;
8099
}
81100

@@ -108,6 +127,10 @@ private void configureButtonBindings() {
108127
drivetrain
109128
).ignoringDisable(true)
110129
);
130+
131+
copilot.x().onTrue(indexer.feedToAmp());
132+
copilot.y().onTrue(indexer.feedToShooter());
133+
driver.a().onTrue(new IntakeCommand(indexer));
111134
}
112135

113136
/**

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

+1-1
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@ public interface IndexerIO {
77
@AutoLog
88
class IndexerIOInputs {
99
public double intakeVoltage;
10-
public double intakeCurrent;
10+
public double intakeCurrentAmps;
1111
public double intakeVelocityRPM;
1212
public double feederVoltage;
1313
public double feederCurrentAmps;

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

+51
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.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,0.025);
16+
private final DCMotorSim feederSim = new DCMotorSim(DCMotor.getNEO(1), FEEDER_GEAR_RATIO,0.025);
17+
private final SimDeviceSim beamBreakSim = new SimDeviceSim("Indexer Beam Break");
18+
private final PIDController feederSimPID = new PIDController(FEEDER_KP, FEEDER_KI,FEEDER_KD);
19+
20+
@Override
21+
public void updateInputs(IndexerIOInputs inputs) {
22+
intakeSim.update(Constants.LOOP_PERIOD_SECS);
23+
feederSim.update(Constants.LOOP_PERIOD_SECS);
24+
inputs.intakeCurrentAmps = intakeSim.getCurrentDrawAmps();
25+
// inputs.intakeVoltage = intakeSim.getBusVoltage() * intakeSim.getAppliedOutput();
26+
inputs.intakeVelocityRPM = intakeSim.getAngularVelocityRPM();
27+
inputs.feederCurrentAmps = feederSim.getCurrentDrawAmps();
28+
// inputs.feederVoltage = feederSim.getBusVoltage() * feederSim.getAppliedOutput();
29+
inputs.feederVelocityRPM = feederSim.getAngularVelocityRPM();
30+
inputs.noteInIntake = beamBreakSim.getBoolean("Indexer Beam Break").get();
31+
32+
// this is a very funny line of code, and absolutely does not belong here, but I don't know how to do this otherwise
33+
feederSim.setState(feederSim.getAngularPositionRad(), feederSimPID.calculate(feederSim.getAngularVelocityRadPerSec()));
34+
35+
}
36+
37+
@Override
38+
public void setIntakeVoltage(double volts) {
39+
intakeSim.setInputVoltage(MathUtil.clamp(volts, -12, 12));
40+
}
41+
42+
@Override
43+
public void setFeederVelocity(double velocity) {
44+
feederSimPID.setSetpoint(velocity);
45+
}
46+
47+
@Override
48+
public void setFeederVoltage(double volts) {
49+
feederSim.setInputVoltage(MathUtil.clamp(volts, -12, 12));
50+
}
51+
}

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

+5-11
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@
1111
public class IndexerIOSparkMax implements IndexerIO {
1212
private final CANSparkMax intakeMotor = new CANSparkMax(INTAKE_ID, CANSparkLowLevel.MotorType.kBrushless);
1313
private final CANSparkMax feederMotor = new CANSparkMax(FEEDER_ID, CANSparkLowLevel.MotorType.kBrushless);
14-
DigitalInput indexerBeamBreak = new DigitalInput(0);
14+
private final DigitalInput indexerBeamBreak = new DigitalInput(0);
1515
private final SparkPIDController feederPID;
1616
private final SimpleMotorFeedforward feederFF = new SimpleMotorFeedforward(FEEDER_KS, FEEDER_KV);
1717

@@ -33,19 +33,14 @@ public IndexerIOSparkMax() {
3333

3434

3535
}
36-
// 2 jobs,
37-
// 1. be able to route to tramp
38-
// 2. feeding the shooter (by default, be still, spin up fast 4 for shooter.
39-
// make cmd factory spin
40-
// slower in other direction for tramp
4136

4237
@Override
4338
public void updateInputs(IndexerIOInputs inputs) {
44-
inputs.intakeCurrent = intakeMotor.getOutputCurrent();
39+
inputs.intakeCurrentAmps = intakeMotor.getOutputCurrent();
4540
inputs.intakeVoltage = intakeMotor.getBusVoltage() * intakeMotor.getAppliedOutput();
4641
inputs.intakeVelocityRPM = intakeMotor.getEncoder().getVelocity();
47-
inputs.feederVoltage = feederMotor.getOutputCurrent();
48-
inputs.feederCurrentAmps = feederMotor.getBusVoltage() * feederMotor.getAppliedOutput();
42+
inputs.feederCurrentAmps = feederMotor.getOutputCurrent();
43+
inputs.feederVoltage = feederMotor.getBusVoltage() * feederMotor.getAppliedOutput();
4944
inputs.feederVelocityRPM = feederMotor.getEncoder().getVelocity();
5045
inputs.noteInIntake = indexerBeamBreak.get();
5146
}
@@ -63,12 +58,11 @@ public void setFeederVoltage(double volts) {
6358
@Override
6459
public void setFeederVelocity(double velocity) {
6560
feederPID.setReference(
66-
Units.radiansPerSecondToRotationsPerMinute(velocity) * GEAR_RATIO,
61+
Units.radiansPerSecondToRotationsPerMinute(velocity) * FEEDER_GEAR_RATIO,
6762
CANSparkBase.ControlType.kVelocity,
6863
0,
6964
feederFF.calculate(velocity),
7065
SparkPIDController.ArbFFUnits.kVoltage
7166
);
7267
}
7368
}
74-
// for elevator sim, app. voltage

0 commit comments

Comments
 (0)