Skip to content

Commit b89a8dd

Browse files
FIRSTTeam4909FIRSTTeam4909
committed
WIP BB
1 parent fb9d81c commit b89a8dd

File tree

4 files changed

+22
-100
lines changed

4 files changed

+22
-100
lines changed

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

Lines changed: 20 additions & 97 deletions
Original file line numberDiff line numberDiff line change
@@ -4,60 +4,43 @@
44

55
package frc.robot;
66

7-
import static edu.wpi.first.units.Units.*;
8-
9-
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
7+
import static edu.wpi.first.units.Units.MetersPerSecond;
8+
import static edu.wpi.first.units.Units.RadiansPerSecond;
9+
import static edu.wpi.first.units.Units.RotationsPerSecond;
1010

1111
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
12-
import com.fasterxml.jackson.databind.util.Named;
12+
import com.ctre.phoenix6.swerve.SwerveRequest;
1313
import com.pathplanner.lib.auto.AutoBuilder;
1414
import com.pathplanner.lib.auto.NamedCommands;
15-
import com.pathplanner.lib.commands.PathPlannerAuto;
16-
import com.ctre.phoenix6.swerve.SwerveRequest;
1715

1816
import edu.wpi.first.apriltag.AprilTagFieldLayout;
1917
import edu.wpi.first.apriltag.AprilTagFields;
2018
import edu.wpi.first.hal.HALUtil;
21-
import edu.wpi.first.math.geometry.Pose2d;
22-
import edu.wpi.first.math.geometry.Pose3d;
2319
import edu.wpi.first.math.geometry.Rotation2d;
2420
import edu.wpi.first.math.geometry.Rotation3d;
2521
import edu.wpi.first.math.geometry.Transform2d;
2622
import edu.wpi.first.math.geometry.Transform3d;
27-
import edu.wpi.first.math.geometry.Translation2d;
2823
import edu.wpi.first.math.geometry.Translation3d;
2924
import edu.wpi.first.math.kinematics.ChassisSpeeds;
30-
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
3125
import edu.wpi.first.math.util.Units;
32-
import edu.wpi.first.units.Unit;
33-
import edu.wpi.first.wpilibj.DriverStation;
34-
import edu.wpi.first.wpilibj.RobotBase;
3526
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
3627
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
3728
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
3829
import edu.wpi.first.wpilibj2.command.Command;
3930
import edu.wpi.first.wpilibj2.command.Commands;
40-
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
4131
import edu.wpi.first.wpilibj2.command.InstantCommand;
42-
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
4332
import edu.wpi.first.wpilibj2.command.RunCommand;
44-
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
45-
import edu.wpi.first.wpilibj2.command.Subsystem;
4633
import edu.wpi.first.wpilibj2.command.WaitCommand;
4734
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
48-
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
49-
import frc.robot.subsystems.drivetrain.DriveToPose;
50-
import frc.robot.subsystems.drivetrain.ReefBranchAlign;
5135
import frc.robot.generated.TunerConstants;
36+
import frc.robot.subsystems.Climber.Climber;
37+
import frc.robot.subsystems.Climber.ClimberIOTalonFX;
5238
import frc.robot.subsystems.Vision.Vision;
53-
import frc.robot.subsystems.Vision.VisionConstants;
5439
import frc.robot.subsystems.Vision.VisionIOPhotonVision;
5540
import frc.robot.subsystems.algae.Algae;
5641
import frc.robot.subsystems.algae.AlgaeIOTalonFX;
57-
import frc.robot.subsystems.Climber.Climber;
58-
import frc.robot.subsystems.Climber.ClimberIOTalonFX;
5942
import frc.robot.subsystems.drivetrain.CommandSwerveDrivetrain;
60-
import frc.robot.subsystems.drivetrain.DriveToFieldPose;
43+
import frc.robot.subsystems.drivetrain.DriveToPose;
6144
import frc.robot.subsystems.elevator.Elevator;
6245
import frc.robot.subsystems.elevator.ElevatorIOTalonFX;
6346
import frc.robot.subsystems.shooter.Shooter;
@@ -181,23 +164,11 @@ else if (HALUtil.getSerialNumber().equals(TunerConstants.RobotV2)) {
181164
}
182165

183166
public void periodic() {
184-
// System.out.println("Elevator Command"+s_Elevator.getCurrentCommand());
185-
186-
187-
// check if elevator is at L1, if so run the command that looks for current spike running intake
188-
// if (s_Elevator.isAtL1()) {
189-
// s_Shooter.setDefaultRunToCurrentSpike();
190-
// }
191167
}
192168

193169
public void stopDrive() {
194170
SwerveRequest.ApplyFieldSpeeds m_drive = new SwerveRequest.ApplyFieldSpeeds();
195171
drivetrain.setControl(m_drive.withSpeeds(new ChassisSpeeds(0,0,0)));
196-
// drivetrain.setControl(new );
197-
// drivetrain.applyRequest(() -> drive
198-
// .withVelocityX(-joystick.getLeftX() * 0)
199-
// .withVelocityY(-joystick.getLeftY() * 0)
200-
// .withRotationalRate(-joystick.getRightX() * 0));
201172
}
202173

203174
private Command driveWithJoystick() {
@@ -217,20 +188,8 @@ private Command driveWithJoystick() {
217188
private void configureBindings() {
218189
// Note that X is defined as forward according to WPILib convention,
219190
// and Y is defined as to the left according to WPILib convention.
220-
drivetrain.setDefaultCommand(
221-
// Drivetrain will execute this command periodically
222-
drivetrain.applyRequest(
223-
() -> drive
224-
.withVelocityX(
225-
-joystick.getLeftY() * MaxSpeed) // Drive forward with negative Y (forward)
226-
.withVelocityY(
227-
-joystick.getLeftX() * MaxSpeed) // Drive left with negative X (left)
228-
.withRotationalRate(
229-
-joystick.getRightX()
230-
* MaxAngularRate) // Drive counterclockwise with negative X (left)
231-
));
232-
233-
//joystick.a().whileTrue(drivetrain.applyRequest(() -> brake));
191+
drivetrain.setDefaultCommand(driveWithJoystick());
192+
234193
joystick.rightStick().whileTrue(
235194
drivetrain.applyRequest(() -> drive
236195
.withVelocityX(
@@ -239,20 +198,10 @@ private void configureBindings() {
239198
-joystick.getLeftY() * SlowSpeed)
240199
.withRotationalRate(
241200
-joystick.getRightX() * SlowAngularRate)));
242-
243-
joystick.rightTrigger().whileTrue(new ConditionalCommand(
244-
s_Shooter.shoot(),
245-
s_Shooter.slowShoot(),
246-
() -> s_Elevator.isAtL4()
247-
)).onFalse(Commands.parallel(driveWithJoystick(), s_Shooter.stop()));
248-
249201

250-
// joystick.y().whileTrue(s_Shooter.slowShoot()).onFalse(s_Shooter.stop());
251202

252203

253204
joystick.a().whileTrue(s_Climber.lower()).onFalse(s_Climber.stop());
254-
255-
256205

257206
//joystick.b().onTrue(s_Elevator.goToL1()).onFalse(s_Elevator.stop());
258207
joystick.leftBumper().whileTrue(s_Elevator.goToL2().repeatedly());//.onFalse(s_Elevator.goToL1());
@@ -263,46 +212,16 @@ private void configureBindings() {
263212
//joystick.povDown().whileTrue(s_Elevator.moveDown()).onFalse(s_Elevator.stop());
264213
zeroController.x().onTrue(s_Elevator.reZero());
265214

266-
// joystick.x().onTrue(s_Elevator.goToL3A()).onFalse(s_Elevator.goToL1());
267-
//joystick.a().onTrue(s_Elevator.goToL2A()).onFalse(s_Elevator.goToL1());
268215
joystick.povLeft().whileTrue(Commands.sequence(s_Elevator.goToL2A_wait(), Commands.parallel(s_Elevator.goToL2A().repeatedly(), Commands.sequence(s_Algae.extend(), s_Algae.intake())))).onFalse((s_Algae.home()));
269216
joystick.povRight().whileTrue(Commands.sequence(s_Elevator.goToL3A_wait(), Commands.parallel(s_Elevator.goToL3A().repeatedly(), Commands.sequence(s_Algae.extend(), s_Algae.intake())))).onFalse((s_Algae.home()));
270217
joystick.rightStick().onTrue(s_Algae.shoot()).onFalse(s_Algae.stopShooter());
271218
//joystick.back().whileTrue(s_Elevator.goToL1Intake().repeatedly().alongWith(s_Algae.intakeL1().andThen(s_Algae.shoot()))).onFalse(s_Algae.holdCoral().alongWith(s_Elevator.goToL1Shoot().repeatedly()));
272219
joystick.leftStick().whileTrue(s_Algae.shootL1low().andThen(s_Algae.slowShoot().andThen(s_Elevator.goToL1Shoot().repeatedly()))).onFalse(s_Algae.home().andThen(s_Algae.stopShooter().andThen(new WaitCommand(5).alongWith(s_Elevator.goToL1()))));
273-
//joystick.back().whileTrue(s_Elevator.goToDCMPL4().repeatedly());
274-
// joystick.leftStick().onTrue(s_Algae.intake()).onFalse(s_Algae.stopShooter());
275-
zeroController.a().onTrue(s_Climber.reZero());
276-
// joystick
277-
278-
// .b()
279-
// .whileTrue(
280-
// drivetrain.applyRequest(
281-
// () ->
282-
// point.withModuleDirection(
283-
// new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX()))));
284-
285-
// joystick.b().whileTrue(new DriveToPose(drivetrain,
286-
// new Transform2d(Units.inchesToMeters(-4.5), Units.inchesToMeters(0.5), new Rotation2d())));
287-
// joystick.x().whileTrue(new DriveToPose(drivetrain,
288-
// new Transform2d(Units.inchesToMeters(-4.5), Units.inchesToMeters(13.5), new Rotation2d())));
289-
zeroController.b().onTrue(s_Algae.reZero());
290220

291-
// joystick.b().whileTrue(Commands.sequence(
292-
// s_Shooter.setDefaultDoNotRun(),
293-
// Commands.parallel(
294-
// new ReefBranchAlign(drivetrain, new Transform2d(Units.inchesToMeters(-4.5), Units.inchesToMeters(0.5+2.25), new Rotation2d()),() -> -joystick.getLeftY()),
295-
// s_Shooter.shootTrough()
296-
// )
297-
// )).onFalse(s_Shooter.stop());
221+
zeroController.a().onTrue(s_Climber.reZero());
298222

299-
// joystick.x().whileTrue(Commands.sequence(
300-
// s_Shooter.setDefaultDoNotRun(),
301-
// Commands.parallel(
302-
// new ReefBranchAlign(drivetrain, new Transform2d(Units.inchesToMeters(-4.5), Units.inchesToMeters(13.5+2.25), new Rotation2d()),() -> -joystick.getLeftY()),
303-
// s_Shooter.shootTrough()
304-
// ))).onFalse(s_Shooter.stop());
305223

224+
zeroController.b().onTrue(s_Algae.reZero());
306225

307226
// auto align right
308227
// positive moves right for second param of translation
@@ -322,12 +241,16 @@ private void configureBindings() {
322241
new Rotation2d()), joystick),s_Shooter.shoot())).onFalse(new InstantCommand(()->joystick.setRumble(RumbleType.kBothRumble, 0))
323242
.andThen(Commands.parallel(s_Shooter.stop(), new RunCommand(() -> stopDrive(), drivetrain))));
324243

325-
// joystick.povDown().whileTrue(Commands.parallel(new DriveToPose(drivetrain,
326-
// new Transform2d(Units.inchesToMeters(-33.5/1.5), Units.inchesToMeters(0)
327-
// , new Rotation2d(1.5700)), joystick))).onFalse(new InstantCommand(()->joystick.setRumble(RumbleType.kBothRumble, 0)));
244+
245+
joystick.rightTrigger()
246+
.whileTrue(s_Shooter.shoot());
247+
//.onFalse(Commands.sequence(new WaitCommand(2), Commands.parallel(driveWithJoystick(), s_Shooter.stop())));
328248

329-
// joystick.y().whileTrue(new DriveToFieldPose(drivetrain,
330-
// new Pose2d(7.495, 5.026, Rotation2d.fromDegrees(-90)), joystick));
249+
// joystick.rightTrigger().whileTrue(new ConditionalCommand(
250+
// s_Shooter.shoot(),
251+
// s_Shooter.slowShoot(),
252+
// () -> s_Elevator.isAtL4()
253+
// )).onFalse(Commands.sequence(new WaitCommand(2), Commands.parallel(driveWithJoystick(), s_Shooter.stop())));
331254

332255

333256

‎src/main/java/frc/robot/subsystems/Vision/VisionIOPhotonVision.java‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,8 @@ public void updateInputs(VisionIOInputs inputs) {
4646
Set<Short> tagIds = new HashSet<>();
4747
List<PoseObservation> poseObservations = new LinkedList<>();
4848
for (var result : camera.getAllUnreadResults()) {
49+
50+
// result.getBestTarget()
4951
// Update latest target observation
5052
if (result.hasTargets()) {
5153
inputs.latestTargetObservation =

‎src/main/java/frc/robot/subsystems/shooter/Shooter.java‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,6 @@
44

55
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
66
import edu.wpi.first.wpilibj2.command.Command;
7-
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
87
import edu.wpi.first.wpilibj2.command.SubsystemBase;
98

109
public class Shooter extends SubsystemBase {

‎src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -5,8 +5,6 @@
55
import com.ctre.phoenix6.hardware.TalonFX;
66
import com.ctre.phoenix6.signals.NeutralModeValue;
77

8-
import frc.robot.subsystems.elevator.ElevatorIOInputsAutoLogged;
9-
108
public class ShooterIOTalonFX implements ShooterIO {
119

1210
private final TalonFX m_shootermotor;

0 commit comments

Comments
 (0)