Skip to content

Commit 4bedda0

Browse files
nlaverdurenlaverdure
committed
make arm actuators independent
1 parent bdfbdc4 commit 4bedda0

5 files changed

Lines changed: 105 additions & 114 deletions

File tree

‎src/main/java/frc/robot/Robot.java‎

Lines changed: 8 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -68,6 +68,7 @@
6868
import frc.robot.subsystems.intake.IntakeArmIOSimSpark;
6969
import frc.robot.subsystems.intake.IntakeArmIOSpark;
7070
import frc.robot.subsystems.intake.IntakeConstants;
71+
import frc.robot.subsystems.intake.IntakeConstants.ArmConstants;
7172
import frc.robot.subsystems.intake.IntakeConstants.RollerConstants;
7273
import frc.robot.subsystems.intake.RollerIO;
7374
import frc.robot.subsystems.intake.RollerIOSimSpark;
@@ -196,7 +197,8 @@ public Robot() {
196197
new Intake(
197198
new RollerIOSpark(RollerConstants.UPPER_ROLLER_CONFIG),
198199
new RollerIOSpark(RollerConstants.LOWER_ROLLER_CONFIG),
199-
new IntakeArmIOSpark());
200+
new IntakeArmIOSpark(ArmConstants.LEFT_ARM_CONFIG),
201+
new IntakeArmIOSpark(ArmConstants.RIGHT_ARM_CONFIG));
200202
feeder = new Feeder(new SpindexerIOSpark(), new KickerIOSpark());
201203

202204
// Start kernel log monitoring (singleton, starts automatically on first call)
@@ -237,7 +239,8 @@ public Robot() {
237239
new Intake(
238240
new RollerIOSimSpark(RollerConstants.UPPER_ROLLER_CONFIG),
239241
new RollerIOSimSpark(RollerConstants.LOWER_ROLLER_CONFIG),
240-
new IntakeArmIOSimSpark());
242+
new IntakeArmIOSimSpark(ArmConstants.LEFT_ARM_CONFIG),
243+
new IntakeArmIOSimSpark(ArmConstants.RIGHT_ARM_CONFIG));
241244
break;
242245

243246
case REPLAY: // Replaying a log
@@ -272,7 +275,9 @@ public Robot() {
272275
new FlywheelIO() {},
273276
new HoodIO() {});
274277
if (FeatureFlags.HOPPER_ENABLED) hopper = new Hopper(new HopperIO() {});
275-
intake = new Intake(new RollerIO() {}, new RollerIO() {}, new IntakeArmIO() {});
278+
intake =
279+
new Intake(
280+
new RollerIO() {}, new RollerIO() {}, new IntakeArmIO() {}, new IntakeArmIO() {});
276281
feeder = new Feeder(new SpindexerIO() {}, new KickerIO() {});
277282
break;
278283
}

‎src/main/java/frc/robot/subsystems/intake/Intake.java‎

Lines changed: 29 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -18,50 +18,63 @@
1818
public class Intake extends SubsystemBase {
1919
private final RollerIO upperRollerIO;
2020
private final RollerIO lowerRollerIO;
21-
private final IntakeArmIO intakeArmIO;
21+
private final IntakeArmIO leftArmIO;
22+
private final IntakeArmIO rightArmIO;
2223

2324
private final RollerIOInputsAutoLogged upperRollerInputs = new RollerIOInputsAutoLogged();
2425
private final RollerIOInputsAutoLogged lowerRollerInputs = new RollerIOInputsAutoLogged();
25-
private final IntakeArmIOInputsAutoLogged intakeArmInputs = new IntakeArmIOInputsAutoLogged();
26+
private final IntakeArmIOInputsAutoLogged leftArmInputs = new IntakeArmIOInputsAutoLogged();
27+
private final IntakeArmIOInputsAutoLogged rightArmInputs = new IntakeArmIOInputsAutoLogged();
2628

2729
private final Alert upperRollerDisconnectedAlert;
2830
private final Alert lowerRollerDisconnectedAlert;
29-
private final Alert intakeArmDisconnectedAlert;
31+
private final Alert leftArmDisconnectedAlert;
32+
private final Alert rightArmDisconnectedAlert;
3033

3134
// Injected after both subsystems are created to avoid a circular dependency.
3235
// When set, getDeployCommand() and getReverseCommand() will deploy the hopper first if needed.
3336
private BooleanSupplier hopperIsDeployed;
3437
private Supplier<Command> hopperDeployCommand;
3538

36-
public Intake(RollerIO upperRollerIO, RollerIO lowerRollerIO, IntakeArmIO intakeArmIO) {
39+
public Intake(
40+
RollerIO upperRollerIO,
41+
RollerIO lowerRollerIO,
42+
IntakeArmIO leftArmIO,
43+
IntakeArmIO rightArmIO) {
3744
this.upperRollerIO = upperRollerIO;
3845
this.lowerRollerIO = lowerRollerIO;
39-
this.intakeArmIO = intakeArmIO;
46+
this.leftArmIO = leftArmIO;
47+
this.rightArmIO = rightArmIO;
4048

4149
upperRollerDisconnectedAlert = new Alert("Disconnected upper intake roller", AlertType.kError);
4250
lowerRollerDisconnectedAlert = new Alert("Disconnected lower intake roller", AlertType.kError);
43-
intakeArmDisconnectedAlert = new Alert("Disconnected intake arm", AlertType.kError);
51+
leftArmDisconnectedAlert = new Alert("Disconnected intake arm", AlertType.kError);
52+
rightArmDisconnectedAlert = new Alert("Disconnected intake arm", AlertType.kError);
4453
}
4554

4655
@Override
4756
public void periodic() {
4857
long t0 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0;
4958
upperRollerIO.updateInputs(upperRollerInputs);
5059
lowerRollerIO.updateInputs(lowerRollerInputs);
51-
intakeArmIO.updateInputs(intakeArmInputs);
60+
leftArmIO.updateInputs(leftArmInputs);
61+
rightArmIO.updateInputs(rightArmInputs);
5262
long t1 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0;
5363

5464
Logger.processInputs("UpperRoller", upperRollerInputs);
5565
Logger.processInputs("LowerRoller", lowerRollerInputs);
56-
Logger.processInputs("IntakeArm", intakeArmInputs);
66+
Logger.processInputs("LeftArm", leftArmInputs);
67+
Logger.processInputs("RightArm", rightArmInputs);
5768
long t2 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0;
5869

5970
upperRollerDisconnectedAlert.set(!upperRollerInputs.connected);
6071
lowerRollerDisconnectedAlert.set(!lowerRollerInputs.connected);
61-
intakeArmDisconnectedAlert.set(!intakeArmInputs.connected);
72+
leftArmDisconnectedAlert.set(!leftArmInputs.connected);
73+
rightArmDisconnectedAlert.set(!rightArmInputs.connected);
6274
Logger.recordOutput("Faults/Intake/UpperRollerDisconnected", !upperRollerInputs.connected);
6375
Logger.recordOutput("Faults/Intake/LowerRollerDisconnected", !lowerRollerInputs.connected);
64-
Logger.recordOutput("Faults/Intake/IntakeArmDisconnected", !intakeArmInputs.connected);
76+
Logger.recordOutput("Faults/Intake/LeftArmDisconnected", !leftArmInputs.connected);
77+
Logger.recordOutput("Faults/Intake/RightArmDisconnected", !rightArmInputs.connected);
6578

6679
// Profiling output
6780
if (Constants.FeatureFlags.PROFILING_ENABLED) {
@@ -82,15 +95,18 @@ public void periodic() {
8295
public void stop() {
8396
upperRollerIO.setOpenLoop(Volts.of(0.0));
8497
lowerRollerIO.setOpenLoop(Volts.of(0.0));
85-
intakeArmIO.setPosition(minPos, RadiansPerSecond.of(0.0));
98+
leftArmIO.setPosition(minPos, RadiansPerSecond.of(0.0));
99+
rightArmIO.setPosition(minPos, RadiansPerSecond.of(0.0));
86100
}
87101

88102
public void deployArm() {
89-
intakeArmIO.setPosition(maxPos, RadiansPerSecond.of(0.0));
103+
leftArmIO.setPosition(maxPos, RadiansPerSecond.of(0.0));
104+
rightArmIO.setPosition(maxPos, RadiansPerSecond.of(0.0));
90105
}
91106

92107
public void retractArm() {
93-
intakeArmIO.setPosition(minPos, RadiansPerSecond.of(0.0));
108+
leftArmIO.setPosition(minPos, RadiansPerSecond.of(0.0));
109+
rightArmIO.setPosition(minPos, RadiansPerSecond.of(0.0));
94110
}
95111

96112
public Boolean isDeployed() {

‎src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java‎

Lines changed: 27 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,6 @@
2020
import edu.wpi.first.units.measure.Voltage;
2121
import edu.wpi.first.wpilibj.simulation.DCMotorSim;
2222
import edu.wpi.first.wpilibj.simulation.RoboRioSim;
23-
import frc.robot.Constants.CANBusPorts.CAN2;
2423
import frc.robot.Constants.MotorConstants.NEOConstants;
2524
import frc.robot.Constants.RobotConstants;
2625
import frc.robot.Robot;
@@ -31,84 +30,74 @@ public class IntakeArmIOSimSpark implements IntakeArmIO {
3130

3231
private final DCMotorSim armSim;
3332

34-
private final SparkMax maxRight;
35-
private final SparkMax maxLeft;
33+
private final SparkMax motor;
3634
private final SparkClosedLoopController controller;
37-
private final SparkMaxSim maxSim;
35+
private final SparkMaxSim motorSim;
3836

39-
private final SparkMaxConfig armConfig;
40-
private final SparkMaxConfig followerConfig;
37+
private final SparkMaxConfig motorConfig;
4138

42-
public IntakeArmIOSimSpark() {
43-
maxRight = new SparkMax(CAN2.INTAKE_ARM_RIGHT, MotorType.kBrushless);
44-
maxLeft = new SparkMax(CAN2.INTAKE_ARM_LEFT, MotorType.kBrushless);
39+
public IntakeArmIOSimSpark(ArmConfig armConfig) {
40+
motor = new SparkMax(armConfig.port(), MotorType.kBrushless);
4541

46-
controller = maxRight.getClosedLoopController();
42+
controller = motor.getClosedLoopController();
4743

48-
armConfig = new SparkMaxConfig();
44+
motorConfig = new SparkMaxConfig();
4945

50-
armConfig
51-
.inverted(false)
46+
motorConfig
47+
.inverted(armConfig.inverted())
5248
.idleMode(IdleMode.kBrake)
5349
.smartCurrentLimit(NEOConstants.DEFAULT_SUPPLY_CURRENT_LIMIT)
5450
.voltageCompensation(RobotConstants.NOMINAL_VOLTAGE);
5551

56-
armConfig
52+
motorConfig
5753
.encoder
5854
.positionConversionFactor(encoderPositionFactor)
5955
.velocityConversionFactor(encoderVelocityFactor);
6056

61-
armConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kP, 0.0, kD);
57+
motorConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kP, 0.0, kD);
6258

63-
armConfig
59+
motorConfig
6460
.softLimit
6561
.forwardSoftLimit(maxPosRad)
6662
.forwardSoftLimitEnabled(true)
6763
.reverseSoftLimit(minPosRad)
6864
.reverseSoftLimitEnabled(true);
6965

70-
followerConfig = new SparkMaxConfig();
71-
72-
followerConfig.apply(armConfig).follow(CAN2.INTAKE_ARM_RIGHT);
73-
74-
maxRight.configure(armConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
75-
maxSim = new SparkMaxSim(maxRight, gearbox);
66+
motor.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
67+
motorSim = new SparkMaxSim(motor, gearbox);
7668

7769
armSim =
7870
new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox);
7971

8072
armSim.setState(0.0, 0.0);
81-
maxSim.setPosition(0.0);
82-
83-
maxLeft.configure(
84-
followerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
73+
motorSim.setPosition(0.0);
8574
}
8675

8776
@Override
8877
public void updateInputs(IntakeArmIOInputs inputs) {
8978
// Update simulation state
9079
double busVoltage = RoboRioSim.getVInVoltage();
91-
armSim.setInput(maxSim.getAppliedOutput() * busVoltage);
80+
armSim.setInput(motorSim.getAppliedOutput() * busVoltage);
9281
armSim.update(Robot.defaultPeriodSecs);
9382

94-
if (maxSim.getPosition() > maxPosRad) {
83+
if (motorSim.getPosition() > maxPosRad) {
9584
armSim.setState(maxPosRad, 0.0);
96-
maxSim.setPosition(maxPosRad);
85+
motorSim.setPosition(maxPosRad);
9786
}
9887

99-
maxSim.iterate(armSim.getAngularVelocityRadPerSec(), busVoltage, Robot.defaultPeriodSecs);
88+
motorSim.iterate(armSim.getAngularVelocityRadPerSec(), busVoltage, Robot.defaultPeriodSecs);
10089

10190
// Update inputs
10291
inputs.connected = true;
103-
inputs.position = maxSim.getPosition();
104-
inputs.velocityMetersPerSec = maxSim.getVelocity();
105-
inputs.appliedVolts = maxSim.getAppliedOutput() * maxSim.getBusVoltage();
106-
inputs.currentAmps = Math.abs(maxSim.getMotorCurrent());
92+
inputs.position = motorSim.getPosition();
93+
inputs.velocityMetersPerSec = motorSim.getVelocity();
94+
inputs.appliedVolts = motorSim.getAppliedOutput() * motorSim.getBusVoltage();
95+
inputs.currentAmps = Math.abs(motorSim.getMotorCurrent());
10796
}
10897

10998
@Override
11099
public void setOpenLoop(Voltage volts) {
111-
maxSim.setAppliedOutput(volts.in(Volts) / RobotConstants.NOMINAL_VOLTAGE);
100+
motorSim.setAppliedOutput(volts.in(Volts) / RobotConstants.NOMINAL_VOLTAGE);
112101
}
113102

114103
@Override
@@ -123,12 +112,12 @@ public void setPosition(Angle rotation, AngularVelocity velocity) {
123112

124113
@Override
125114
public void configureSoftLimits(boolean enable) {
126-
armConfig.softLimit.forwardSoftLimitEnabled(enable);
127-
armConfig.softLimit.reverseSoftLimitEnabled(enable);
115+
motorConfig.softLimit.forwardSoftLimitEnabled(enable);
116+
motorConfig.softLimit.reverseSoftLimitEnabled(enable);
128117
}
129118

130119
@Override
131120
public void resetEncoder() {
132-
maxSim.setPosition(maxPosRad);
121+
motorSim.setPosition(maxPosRad);
133122
}
134123
}

0 commit comments

Comments
 (0)