Skip to content

Commit 2c8b0dc

Browse files
prankX02prankX02
committed
Worked on algae scoop cmd. Removes algae without collecting. Tested and running
1 parent 70bd5de commit 2c8b0dc

4 files changed

Lines changed: 181 additions & 4 deletions

File tree

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -104,7 +104,7 @@ public static class ReefConstants {
104104
/**
105105
* How far to back up from the ALGAE scoring pose.
106106
*/
107-
public static final Distance backupDistanceAlgae = Inches.of(20);
107+
public static final Distance backupDistanceAlgae = Inches.of(40);
108108

109109
/**
110110
* The distance from the center of the ROBOT to the ARM.

‎src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefScoopCmd.java‎

Lines changed: 175 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,175 @@
1+
package frc.robot.commands.robot.collectalgaereef;
2+
3+
import static edu.wpi.first.units.Units.MetersPerSecond;
4+
import static edu.wpi.first.units.Units.MetersPerSecondPerSecond;
5+
import static edu.wpi.first.units.Units.Milliseconds;
6+
import static edu.wpi.first.units.Units.Volts;
7+
import static edu.wpi.first.units.Units.Degrees;
8+
import static edu.wpi.first.units.Units.Meters;
9+
10+
import java.util.Optional;
11+
12+
import org.xerosw.math.XeroMath;
13+
import org.xerosw.util.XeroSequenceCmd;
14+
15+
import edu.wpi.first.math.geometry.Pose2d;
16+
import edu.wpi.first.math.geometry.Rotation2d;
17+
import edu.wpi.first.units.measure.Angle;
18+
import edu.wpi.first.units.measure.Distance;
19+
import edu.wpi.first.wpilibj.DriverStation;
20+
import edu.wpi.first.wpilibj.DriverStation.Alliance;
21+
import edu.wpi.first.wpilibj2.command.Command;
22+
import edu.wpi.first.wpilibj2.command.Commands;
23+
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
24+
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
25+
import frc.robot.RobotContainer;
26+
import frc.robot.Constants.ReefLevel;
27+
import frc.robot.commands.drive.DriveCommands;
28+
import frc.robot.commands.robot.CommandConstants;
29+
30+
import frc.robot.subsystems.brain.BrainSubsystem;
31+
import frc.robot.subsystems.brain.GamePiece;
32+
import frc.robot.subsystems.brain.SetHoldingCmd;
33+
import frc.robot.subsystems.drive.Drive;
34+
import frc.robot.subsystems.grabber.GrabberSubsystem;
35+
import frc.robot.subsystems.grabber.commands.RunGrabberVoltsCmd;
36+
import frc.robot.subsystems.manipulator.ManipulatorConstants;
37+
import frc.robot.subsystems.manipulator.ManipulatorSubsystem;
38+
import frc.robot.subsystems.manipulator.commands.GoToCmd;
39+
import frc.robot.subsystems.manipulator.commands.GoToCmdDirect;
40+
import frc.robot.util.ReefFaceInfo;
41+
import frc.robot.util.ReefUtil;
42+
43+
44+
public class CollectAlgaeReefScoopCmd extends XeroSequenceCmd {
45+
private BrainSubsystem brain_ ;
46+
private ManipulatorSubsystem manipulator_;
47+
private GrabberSubsystem grabber_;
48+
private ReefLevel height_ ;
49+
private Drive db_ ;
50+
private boolean eject_ ;
51+
private boolean quick_ ;
52+
53+
public CollectAlgaeReefScoopCmd(BrainSubsystem brain, Drive db, ManipulatorSubsystem manipulator, GrabberSubsystem grabber, ReefLevel height, boolean eject, boolean quick) {
54+
super("CollectAlgaeReefScoopCmd") ;
55+
brain_ = brain ;
56+
db_ = db ;
57+
manipulator_ = manipulator;
58+
grabber_ = grabber;
59+
height_ = height ;
60+
eject_ = eject ;
61+
quick_ = quick ;
62+
}
63+
64+
@Override
65+
public void initSequence(SequentialCommandGroup seq) {
66+
ReefLevel level = height_ ;
67+
68+
Angle angle ;
69+
Distance height ;
70+
71+
if (height_ == ReefLevel.AskBrain) {
72+
level = brain_.algaeLevel() ;
73+
}
74+
75+
if (level == ReefLevel.L2 || level == ReefLevel.L1) {
76+
angle = ManipulatorConstants.Arm.Positions.kAlgaeReefCollectNewL2 ;
77+
height = ManipulatorConstants.Elevator.Positions.kAlgaeReefCollectNewL2 ;
78+
} else if (level == ReefLevel.L3 || level == ReefLevel.L4) {
79+
angle = ManipulatorConstants.Arm.Positions.kAlgaeReefCollectNewL3 ;
80+
height = ManipulatorConstants.Elevator.Positions.kAlgaeReefCollectNewL3 ;
81+
}
82+
else {
83+
//
84+
// Bad height value, so we just return and have an empty sequence which
85+
// does nothing.
86+
//
87+
return ;
88+
}
89+
90+
Optional<ReefFaceInfo> reefFace = ReefUtil.getTargetedReefFace(db_.getPose());
91+
if (reefFace.isEmpty())
92+
return ;
93+
94+
Alliance a = DriverStation.getAlliance().get() ;
95+
96+
//
97+
// This is about the actual collect operation
98+
//
99+
Pose2d bup = reefFace.get().getAlgaeBackupPose() ;
100+
Pose2d buprot ;
101+
102+
if (eject_) {
103+
Rotation2d rot ;
104+
105+
if (a == Alliance.Red) {
106+
if (XeroMath.normalizeAngleDegrees(bup.getRotation().getDegrees() - 180.0) < 5.0) {
107+
rot = Rotation2d.fromDegrees(90.0) ;
108+
}
109+
else {
110+
rot = Rotation2d.fromDegrees(180.0) ;
111+
}
112+
}
113+
else {
114+
if (XeroMath.normalizeAngleDegrees(bup.getRotation().getDegrees()) < 5.0) {
115+
rot = Rotation2d.fromDegrees(90.0) ;
116+
}
117+
else {
118+
rot = Rotation2d.fromDegrees(0.0) ;
119+
}
120+
}
121+
buprot = new Pose2d(bup.getTranslation(), rot) ;
122+
}
123+
else {
124+
buprot = bup ;
125+
}
126+
127+
seq.addCommands(
128+
new GoToCmdDirect(manipulator_, height, angle),
129+
RobotContainer.getInstance().gamepad().setLockCommand(true),
130+
//grabber_.setVoltageCommand(Volts.of(-6.0)),
131+
DriveCommands.simplePathCommand(db_, reefFace.get().getAlgaeCollectPose(), bup,
132+
MetersPerSecond.of(2.0),
133+
CommandConstants.ReefDrive.kMaxDriveAcceleration),
134+
new GoToCmdDirect(manipulator_, height.plus(Meters.of(0.05)), angle.minus(Degrees.of(40))),
135+
DriveCommands.simplePathCommand(db_, buprot, bup,
136+
MetersPerSecond.of(3.0),
137+
MetersPerSecondPerSecond.of(3.0)));
138+
139+
140+
Command postseq ;
141+
142+
if (!quick_) {
143+
postseq = Commands.sequence(
144+
RobotContainer.getInstance().gamepad().setLockCommand(false),
145+
new SetHoldingCmd(brain_, GamePiece.ALGAE_HIGH),
146+
new GoToCmdDirect(manipulator_, manipulator_.getElevatorPosition(),
147+
ManipulatorConstants.Arm.Positions.kAlgaeReefHold),
148+
new GentleLowerElevator(manipulator_, ManipulatorConstants.Elevator.Positions.kAlgaeReefHold)) ;
149+
}
150+
else {
151+
postseq = Commands.sequence(
152+
RobotContainer.getInstance().gamepad().setLockCommand(false)) ;
153+
}
154+
155+
if (eject_) {
156+
seq.addCommands(new RunGrabberVoltsCmd(grabber_, Milliseconds.of(500))) ;
157+
}
158+
else {
159+
seq.addCommands(
160+
new ConditionalCommand(
161+
postseq,
162+
Commands.sequence(
163+
RobotContainer.getInstance().gamepad().setLockCommand(false),
164+
grabber_.setVoltageCommand(Volts.zero()),
165+
new GoToCmdDirect(manipulator_, height, angle)
166+
),
167+
this::hasAlgae)) ;
168+
}
169+
seq.addCommands(RobotContainer.getInstance().gamepad().setLockCommand(false)) ;
170+
}
171+
172+
private boolean hasAlgae() {
173+
return grabber_.hasAlgae() ;
174+
}
175+
}

‎src/main/java/frc/robot/subsystems/brain/BrainSubsystem.java‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,7 @@
2727
import frc.robot.util.ReefUtil;
2828
import frc.robot.commands.robot.NullCmd ;
2929
import frc.robot.commands.robot.collectalgaereef.CollectAlgaeReefCmd;
30+
import frc.robot.commands.robot.collectalgaereef.CollectAlgaeReefScoopCmd;
3031
import frc.robot.commands.robot.collectalgaereef.CollectAlgaeReefGotoCmd;
3132
import frc.robot.commands.robot.collectcoral.CollectCoralCmd;
3233
import frc.robot.commands.robot.placecoral.PlaceCoralCmd;
@@ -556,7 +557,8 @@ private RobotActionCommandList getRobotActionCommand(RobotAction action, ReefLev
556557
list.add(new CollectAlgaeReefGotoCmd(this, m_, ReefLevel.AskBrain)) ;
557558
conds.add(null) ;
558559

559-
list.add(new CollectAlgaeReefCmd(this, db_, m_, g_, ReefLevel.AskBrain, true, false)) ;
560+
//list.add(new CollectAlgaeReefCmd(this, db_, m_, g_, ReefLevel.AskBrain, true, false)) ;
561+
list.add(new CollectAlgaeReefScoopCmd(this, db_, m_, g_, ReefLevel.AskBrain, false, false)) ;
560562
conds.add(() -> { return ReefUtil.getTargetedReefFace(db_.getPose()).isPresent() ; }) ;
561563
break ;
562564

‎src/main/java/frc/robot/subsystems/manipulator/ManipulatorConstants.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -218,8 +218,8 @@ public class Positions {
218218
public static final Distance kAlgaeReefCollectL3 = Centimeters.of(78.0) ;
219219
public static final Distance kAlgaeReefCollectL2 = Centimeters.of(40.0) ;
220220

221-
public static final Distance kAlgaeReefCollectNewL3 = Centimeters.of(72.0) ;
222-
public static final Distance kAlgaeReefCollectNewL2 = Centimeters.of(32.0) ;
221+
public static final Distance kAlgaeReefCollectNewL3 = Centimeters.of(70.0) ;
222+
public static final Distance kAlgaeReefCollectNewL2 = Centimeters.of(37.0) ;
223223

224224
public static final Distance kAlgaeReefCollectNewPos2L3 = Centimeters.of(75.0) ;
225225
public static final Distance kAlgaeReefCollectNewPos2L2 = Centimeters.of(37.0) ;

0 commit comments

Comments
 (0)