Skip to content

Commit 9d647cf

Browse files
codeNinja-1codeNinja-1
committed
Fix barge throw
1 parent 83cf0c7 commit 9d647cf

5 files changed

Lines changed: 76 additions & 42 deletions

File tree

‎src/main/java/frc/robot/commands/PieceCombos.java‎

Lines changed: 16 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,9 @@
22

33
import static edu.wpi.first.units.Units.Inches;
44

5+
import com.team6962.lib.telemetry.Logger;
56
import com.team6962.lib.utils.CommandUtils;
7+
68
import edu.wpi.first.wpilibj2.command.Command;
79
import edu.wpi.first.wpilibj2.command.Commands;
810
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
@@ -105,27 +107,23 @@ public Command algaeL3() {
105107
public Command algaeBargeSetup() {
106108
return safeSubsystems
107109
.safeMoveCommand(
108-
elevator.algaeBarge(), manipulator.pivot.algaeBargeSetup(), ELEVATOR.ALGAE.BARGE_HEIGHT)
109-
.withName("BARGE SETUP");
110+
elevator.algaeBarge(),
111+
manipulator.pivot.algaeBargeSetup(),
112+
ELEVATOR.ALGAE.BARGE_HEIGHT
113+
)
114+
.withName("BARGE SETUP");
110115
}
111116

112117
public Command algaeBargeShoot() {
113-
return Commands.either(
114-
manipulator
115-
.pivot
116-
.algaeBargeSetup()
117-
.andThen(
118-
manipulator.pivot.algaeBargeSetup(),
119-
manipulator
120-
.pivot
121-
.algaeBargeShoot()
122-
.deadlineFor(
123-
Commands.sequence(
124-
// Commands.waitUntil(() ->
125-
// manipulator.pivot.inRange(MANIPULATOR_PIVOT.ALGAE.BARGE.RELEASE_ANGLE)),
126-
manipulator.grabber.dropAlgae()))),
127-
Commands.print("not at barge height"),
128-
() -> elevator.inRange(ELEVATOR.ALGAE.BARGE_HEIGHT));
118+
return Commands.sequence(
119+
manipulator.pivot.algaeBargeSetup(),
120+
manipulator.pivot.algaeBargeShoot()
121+
.deadlineFor(
122+
Commands.sequence(
123+
// Commands.waitUntil(() ->
124+
// manipulator.pivot.inRange(MANIPULATOR_PIVOT.ALGAE.BARGE.RELEASE_ANGLE)),
125+
manipulator.grabber.dropAlgae()))
126+
).onlyIf(() -> elevator.inRange(ELEVATOR.ALGAE.BARGE_HEIGHT));
129127
}
130128

131129
public Command algaeProcessor() {

‎src/main/java/frc/robot/subsystems/manipulator/pivot/DisabledManipulatorPivot.java‎

Lines changed: 48 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -3,84 +3,112 @@
33
import com.team6962.lib.utils.CommandUtils;
44
import edu.wpi.first.units.measure.Angle;
55
import edu.wpi.first.wpilibj2.command.Command;
6+
7+
import static edu.wpi.first.units.Units.Radians;
8+
69
import java.util.function.Supplier;
710

811
public class DisabledManipulatorPivot implements ManipulatorPivot {
9-
public Command pivotTo(Supplier<Angle> angleSupplier, Angle tolerance) {
12+
@Override
13+
public Angle getAngle() {
14+
return Radians.of(0);
15+
}
16+
17+
@Override
18+
public Command pivotTo(Supplier<Angle> angleSupplier, Angle tolerance) {
1019
return CommandUtils.noneWithRequirements(this);
1120
}
1221

13-
public Command pivotTo(Supplier<Angle> angleSupplier) {
22+
@Override
23+
public Command pivotTo(Supplier<Angle> angleSupplier) {
1424
return CommandUtils.noneWithRequirements(this);
1525
}
1626

17-
public Command hold() {
27+
@Override
28+
public Command hold() {
1829
return CommandUtils.noneWithRequirements(this);
1930
}
2031

21-
public Command coralIntake() {
32+
@Override
33+
public Command coralIntake() {
2234
return CommandUtils.noneWithRequirements(this);
2335
}
2436

25-
public Command coralL1() {
37+
@Override
38+
public Command coralL1() {
2639
return CommandUtils.noneWithRequirements(this);
2740
}
2841

29-
public Command coralL23() {
42+
@Override
43+
public Command coralL23() {
3044
return CommandUtils.noneWithRequirements(this);
3145
}
3246

33-
public Command coralL4() {
47+
@Override
48+
public Command coralL4() {
3449
return CommandUtils.noneWithRequirements(this);
3550
}
3651

37-
public Command algaeReef() {
52+
@Override
53+
public Command algaeReef() {
3854
return CommandUtils.noneWithRequirements(this);
3955
}
4056

41-
public Command algaeBargeSetup() {
57+
@Override
58+
public Command algaeBargeSetup() {
4259
return CommandUtils.noneWithRequirements(this);
4360
}
4461

45-
public Command algaeBargeShoot() {
62+
@Override
63+
public Command algaeBargeShoot() {
4664
return CommandUtils.noneWithRequirements(this);
4765
}
4866

49-
public Command algaeProcessor() {
67+
@Override
68+
public Command algaeProcessor() {
5069
return CommandUtils.noneWithRequirements(this);
5170
}
5271

53-
public Command algaeGround() {
72+
@Override
73+
public Command algaeGround() {
5474
return CommandUtils.noneWithRequirements(this);
5575
}
5676

57-
public Command stow() {
77+
@Override
78+
public Command stow() {
5879
return CommandUtils.noneWithRequirements(this);
5980
}
6081

61-
public Command safe(Angle tolerance) {
82+
@Override
83+
public Command safe(Angle tolerance) {
6284
return CommandUtils.noneWithRequirements(this);
6385
}
6486

65-
public Command safe() {
87+
@Override
88+
public Command safe() {
6689
return CommandUtils.noneWithRequirements(this);
6790
}
6891

69-
public Command stop() {
92+
@Override
93+
public Command stop() {
7094
return CommandUtils.noneWithRequirements(this);
7195
}
7296

73-
public Command up() {
97+
@Override
98+
public Command up() {
7499
return CommandUtils.noneWithRequirements(this);
75100
}
76101

77-
public Command down() {
102+
@Override
103+
public Command down() {
78104
return CommandUtils.noneWithRequirements(this);
79105
}
80106

81-
public boolean inRange(Angle angle) {
107+
@Override
108+
public boolean inRange(Angle angle) {
82109
return true;
83110
}
84111

85-
public void setMinMaxAngle(Angle min, Angle max) {}
112+
@Override
113+
public void setMinMaxAngle(Angle min, Angle max) {}
86114
}

‎src/main/java/frc/robot/subsystems/manipulator/pivot/ManipulatorPivot.java‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,8 @@
77
import java.util.function.Supplier;
88

99
public interface ManipulatorPivot extends Subsystem {
10+
public Angle getAngle();
11+
1012
public Command pivotTo(Supplier<Angle> angleSupplier, Angle tolerance);
1113

1214
public Command pivotTo(Supplier<Angle> angleSupplier);

‎src/main/java/frc/robot/subsystems/manipulator/pivot/RealManipulatorPivot.java‎

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
package frc.robot.subsystems.manipulator.pivot;
22

3+
import static edu.wpi.first.units.Units.Degrees;
34
import static edu.wpi.first.units.Units.Rotations;
45
import static edu.wpi.first.units.Units.RotationsPerSecond;
56
import static edu.wpi.first.units.Units.Second;
@@ -45,6 +46,11 @@ public RealManipulatorPivot() {
4546
setDefaultCommand(hold());
4647
}
4748

49+
@Override
50+
public Angle getAngle() {
51+
return getAbsolutePosition();
52+
}
53+
4854
@Override
4955
public void periodic() {
5056
if (!ENABLED_SYSTEMS.isManipulatorEnabled()) return;
@@ -109,7 +115,8 @@ public Command algaeReef() {
109115

110116
@Override
111117
public Command algaeBargeSetup() {
112-
return pivotTo(() -> MANIPULATOR_PIVOT.ALGAE.BARGE.AIM_ANGLE);
118+
return pivotTo(() -> MANIPULATOR_PIVOT.ALGAE.BARGE.AIM_ANGLE)
119+
.until(() -> getAngle().gt(MANIPULATOR_PIVOT.MAX_ANGLE.minus(Degrees.of(1))));
113120
}
114121

115122
@Override

‎src/main/java/frc/robot/util/hardware/motion/DualLinearActuator.java‎

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
import com.team6962.lib.telemetry.StatusChecks;
1616
import edu.wpi.first.math.MathUtil;
1717
import edu.wpi.first.math.filter.Debouncer;
18+
import edu.wpi.first.math.filter.Debouncer.DebounceType;
1819
import edu.wpi.first.units.measure.Distance;
1920
import edu.wpi.first.wpilibj.DigitalInput;
2021
import edu.wpi.first.wpilibj2.command.Command;
@@ -37,8 +38,6 @@ public class DualLinearActuator extends SubsystemBase {
3738
private Distance baseHeight, minHeight, maxHeight, tolerance;
3839
private boolean moving = false;
3940

40-
private Debouncer debouncer = new Debouncer(0.1);
41-
4241
private Distance spoolHeight;
4342

4443
private boolean zeroed = false;
@@ -209,7 +208,7 @@ public boolean unsafeMoveDown() {
209208

210209
public boolean inRange(Distance height) {
211210
if (height == null) return true;
212-
return debouncer.calculate(getAverageHeight().minus(height).abs(Meters) < tolerance.in(Meters));
211+
return getAverageHeight().minus(height).abs(Meters) < tolerance.in(Meters);
213212
}
214213

215214
public boolean doneMoving() {

0 commit comments

Comments
 (0)