Skip to content

Commit 604c6e2

Browse files
committed
fix: fix Arm and IntakeWheel subsys
1 parent 7c8102b commit 604c6e2

File tree

6 files changed

+8
-4
lines changed

6 files changed

+8
-4
lines changed

src/main/java/frc/team5115/RobotContainer.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -231,7 +231,7 @@ public void robotPeriodic() {
231231
}
232232
faultPrintTimeout -= 1;
233233
Logger.recordOutput("HasFaults", hasFaults);
234-
Logger.recordOutput("ClearForMatch",!hasFaults);
234+
Logger.recordOutput("ClearForMatch", !hasFaults);
235235
}
236236
}
237237

src/main/java/frc/team5115/subsystems/arm/Arm.java

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@ public class Arm extends SubsystemBase {
1717
private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged();
1818
private final ArmFeedforward feedforward;
1919
private final PIDController pid;
20+
private final double ks;
2021

2122
public Arm(ArmIO io) {
2223
this.io = io;
@@ -26,14 +27,17 @@ public Arm(ArmIO io) {
2627
case REPLAY:
2728
feedforward = new ArmFeedforward(0.3, 0.35, 0.13509, 0.048686);
2829
pid = new PIDController(0.405, 0.0, 0.0);
30+
ks = 0.3;
2931
break;
3032
case SIM:
3133
feedforward = new ArmFeedforward(0.0, 0.35, 0.1351, 0.0);
3234
pid = new PIDController(0.5, 0.0, 0.0);
35+
ks = 0.3;
3336
break;
3437
default:
3538
feedforward = new ArmFeedforward(0.0, 0.0, 0, 0.0);
3639
pid = new PIDController(0.0, 0.0, 0.0);
40+
ks = 0.3;
3741
break;
3842
}
3943

@@ -57,7 +61,7 @@ public void periodic() {
5761
double voltage = feedforward.calculate(inputs.armAngle.getRadians(), speed);
5862
voltage = MathUtil.clamp(voltage, -10, +10);
5963

60-
if (Math.abs(voltage) < 2 * feedforward.ks) {
64+
if (Math.abs(voltage) < 2 * ks) {
6165
voltage = 0;
6266
}
6367

src/main/java/frc/team5115/subsystems/intake-wheel/IntakeWheel.java renamed to src/main/java/frc/team5115/subsystems/intakewheel/IntakeWheel.java

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -11,9 +11,9 @@
1111
public class IntakeWheel extends SubsystemBase {
1212
private static final double INTAKE_SPEED = 0.15;
1313
private final IntakeWheelIO io;
14-
private final IntakeWheelIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged();
14+
private final IntakeWheelIOInputsAutoLogged inputs = new IntakeWheelIOInputsAutoLogged();
1515

16-
public IntakeWheel(IntakeIO io) {
16+
public IntakeWheel(IntakeWheelIO io) {
1717
this.io = io;
1818
}
1919

0 commit comments

Comments
 (0)