Skip to content

Commit 1690222

Browse files
committed
Maybe fixed eject outtakeside + including more stuff in NEO commands
1 parent 7a2edd9 commit 1690222

File tree

4 files changed

+8
-10
lines changed

4 files changed

+8
-10
lines changed

src/main/java/org/carlmontrobotics/commands/EjectOuttakeSide.java

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,7 @@ public EjectOuttakeSide(IntakeShooter intakeShooter) {
1717
@Override
1818
public void initialize() {
1919
intakeShooter.setMaxIntake(1);
20-
intakeShooter.setMaxOutakeOverload(1);
20+
intakeShooter.setMaxOutake(1);
2121
timer.reset();
2222
timer.start();
2323
}
@@ -36,6 +36,6 @@ public void end(boolean interrupted) {
3636
}
3737
@Override
3838
public boolean isFinished() {
39-
return timer.get() >= SMART_CURRENT_LIMIT_TIMEOUT;
39+
return timer.get() >= 4 || (!intakeShooter.intakeDetectsNote() && !intakeShooter.outakeDetectsNote());
4040
}
4141
}

src/main/java/org/carlmontrobotics/commands/IntakeNEO.java

Lines changed: 0 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -11,9 +11,6 @@ public class IntakeNEO extends Command {
1111
// intake until sees game peice or 4sec has passed
1212
private Timer timer = new Timer();
1313
private final IntakeShooter intake;
14-
15-
private double endAt = 0;
16-
private final double keepIntakingFor = 0.2;
1714
int increaseAmount = 750;
1815
int index = 0;
1916

src/main/java/org/carlmontrobotics/commands/PassToOuttake.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,7 @@ public void execute() {
2828
public void end(boolean interrupted) {
2929
timer.stop();
3030
timer.reset();
31-
intakeShooter.setCurrentLimit(20);
31+
intakeShooter.resetCurrentLimit();
3232
}
3333

3434
@Override

src/main/java/org/carlmontrobotics/commands/SwitchRPMShootNEO.java

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@
1212
public class SwitchRPMShootNEO extends Command {
1313
private IntakeShooter intakeShooter;
1414
private double rpmAmount;
15+
private Timer timer = new Timer();
1516

1617

1718
public SwitchRPMShootNEO(IntakeShooter intakeShooter) {
@@ -21,26 +22,26 @@ public SwitchRPMShootNEO(IntakeShooter intakeShooter) {
2122
@Override
2223
public void initialize() {
2324
intakeShooter.setMaxOutake(1);
24-
25+
timer.reset();
2526
}
2627
@Override
2728
public void execute() {
2829
rpmAmount = RPM_SELECTOR[Arm.getSelector()];
2930
if(intakeShooter.getOutakeRPM() >= rpmAmount) {
3031
intakeShooter.setMaxIntake(1);
31-
32+
timer.start();
3233
}
3334
}
3435
@Override
3536
public void end(boolean interrupted) {
3637
intakeShooter.stopIntake();
3738
intakeShooter.stopOutake();
3839
intakeShooter.resetCurrentLimit();
39-
40+
timer.stop();
4041

4142
}
4243
@Override
4344
public boolean isFinished() {
44-
return (!intakeShooter.intakeDetectsNote() && !intakeShooter.outakeDetectsNote());
45+
return (!intakeShooter.intakeDetectsNote() && !intakeShooter.outakeDetectsNote()) || timer.get() > 10;
4546
}
4647
}

0 commit comments

Comments
 (0)