Skip to content

Commit 9692587

Browse files
mwitcpalekmwitcpalek
authored
Merge pull request #102 from strykeforce/Worlds-Fixes
Worlds fixes
2 parents e0bc839 + 90a6121 commit 9692587

37 files changed

+866
-37
lines changed

‎src/main/deploy/paths/MiddleInitial1_WingNote1.toml‎

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
max_velocity = 3.0 #m/s
2+
max_acceleration = 3.0 #m/s
3+
start_velocity = 0.0 #m/s
4+
end_velocity = 0.0 #m/s
5+
is_reversed = false
6+
target_yaw = 22.0 #degrees
7+
8+
[start_pose]
9+
dataPoint = "MI1"
10+
dX = 0.0
11+
angle = 180.0
12+
13+
[end_pose]
14+
dataPoint "W1"
15+
dY = 0.1
16+
dX = 1.0
17+
angle = 0.0
18+
19+
[[internal_points]]
20+
x = 1.9
21+
y = 6.46
22+
23+
[[internal_points]]
24+
x = 3.09
25+
y = 6.6 # 6.95

‎src/main/deploy/paths/MiddleNote3_AmpShoot2.toml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
max_velocity = 2.0 #m/s
22
max_acceleration = 2.5 #m/s
33
start_velocity = 0.0 #m/s
4-
end_velocity = 2.0 #m/s
4+
end_velocity = 0.0 #m/s
55
is_reversed = false
66
target_yaw = 0.0 #degrees
77

‎src/main/deploy/paths/NonAmpInitial1_MiddleNote5.toml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@ target_yaw = 0.0 #degrees
1010

1111
[end_pose]
1212
dataPoint = "M5"
13-
dX = -0.8
13+
dX = -0.5
1414
dY = 0.35
1515

1616
[[internal_points]]

‎src/main/deploy/paths/WingNote1_Leave.toml‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
max_velocity = 3.0 #m/s
2+
max_acceleration = 3.0 #m/s
3+
start_velocity = 0.0 #m/s
4+
end_velocity = 0.0 #m/s
5+
is_reversed = false
6+
target_yaw = 0.0 #degrees
7+
8+
[start_pose]
9+
dataPoint = "W1"
10+
dY = 0.1
11+
dX = 1.0
12+
angle = 0.0
13+
14+
[end_pose]
15+
dataPoint = "W2"
16+
dY = 0.1
17+
dX = 0.0
18+
angle = 0.0
19+
20+
[[internal_points]]
21+
x = 3.0
22+
y = 6.1

‎src/main/deploy/paths/WingNote1_MiddleInitial1.toml‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
max_velocity = 3.0 #m/s
2+
max_acceleration = 3.0 #m/s
3+
start_velocity = 0.0 #m/s
4+
end_velocity = 0.0 #m/s
5+
is_reversed = false
6+
target_yaw = 0.0 #degrees
7+
8+
[start_pose]
9+
dataPoint "W1"
10+
dY = 0.1
11+
dX = 1.0
12+
angle = 0.0
13+
14+
[end_pose]
15+
dataPoint = "MI1"
16+
dX = 0.0
17+
angle = 180.0
18+
19+
[[internal_points]]
20+
x = 1.87
21+
y = 5.94

‎src/main/deploy/paths/WingNote2_MiddleInitial1.toml‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
max_velocity = 3.0 #m/s
2+
max_acceleration = 3.0 #m/s
3+
start_velocity = 0.0 #m/s
4+
end_velocity = 0.0 #m/s
5+
is_reversed = false
6+
target_yaw = 0.0 #degrees
7+
8+
[start_pose]
9+
dataPoint "W2"
10+
dX = 0.25 # .2
11+
dY = -0.3 # Red - 0.3 | Blue - 0.3
12+
angle = 0.0
13+
14+
[end_pose]
15+
dataPoint = "MI1"
16+
dX = 0.0
17+
angle = 180.0
18+
19+
[[internal_points]]
20+
x = 1.99
21+
y = 5.55

‎src/main/deploy/paths/WingNote3_MiddleInitial1.toml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@ target_yaw = 0.0 #degrees
1212
angle = 150.0
1313
[end_pose]
1414
dataPoint = "MI1"
15-
dX = 0.4
15+
dX = 0.0
1616
angle = 180.0
1717

1818
[[internal_points]]

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

Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111
import edu.wpi.first.wpilibj.DriverStation.Alliance;
1212
import edu.wpi.first.wpilibj.Joystick;
1313
import edu.wpi.first.wpilibj.Preferences;
14+
import edu.wpi.first.wpilibj.RobotController;
1415
import edu.wpi.first.wpilibj.XboxController;
1516
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
1617
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
@@ -38,6 +39,8 @@
3839
import frc.robot.commands.climb.ZeroClimbCommand;
3940
import frc.robot.commands.drive.DriveAutonCommand;
4041
import frc.robot.commands.drive.DriveTeleopCommand;
42+
import frc.robot.commands.drive.HoldDriveSafeCommand;
43+
import frc.robot.commands.drive.IdleDriveCommand;
4144
import frc.robot.commands.drive.LockZeroCommand;
4245
import frc.robot.commands.drive.ResetGyroCommand;
4346
import frc.robot.commands.drive.SetAzimuthVelocityCommand;
@@ -55,8 +58,10 @@
5558
import frc.robot.commands.intake.OpenLoopIntakeCommand;
5659
import frc.robot.commands.magazine.OpenLoopMagazineCommand;
5760
import frc.robot.commands.magazine.RecoverMagazineCommand;
61+
import frc.robot.commands.magazine.UnJamUpperNoteCommand;
5862
import frc.robot.commands.robotState.AirwaveHealthCheck;
5963
import frc.robot.commands.robotState.AmpCommand;
64+
import frc.robot.commands.robotState.BlockCommand;
6065
import frc.robot.commands.robotState.ClimbCommand;
6166
import frc.robot.commands.robotState.ClimbTrapDecendCommand;
6267
import frc.robot.commands.robotState.DecendCommand;
@@ -84,9 +89,11 @@
8489
import frc.robot.commands.wrist.WriteWristToStowCommand;
8590
import frc.robot.commands.wrist.ZeroWristCommand;
8691
import frc.robot.constants.AutonConstants;
92+
import frc.robot.constants.DriveConstants;
8793
import frc.robot.constants.MagazineConstants;
8894
import frc.robot.constants.RobotConstants;
8995
import frc.robot.constants.RobotStateConstants;
96+
import frc.robot.constants.SuperStructureConstants;
9097
import frc.robot.controllers.FlyskyJoystick;
9198
import frc.robot.controllers.FlyskyJoystick.Button;
9299
import frc.robot.subsystems.auto.AutoSwitch;
@@ -306,6 +313,8 @@ public RobotContainer() {
306313
configureTuningDashboard();
307314
robotStateSubsystem.setAllianceColor(Alliance.Blue);
308315

316+
RobotController.setBrownoutVoltage(6.3);
317+
309318
// configureTelemetry();
310319
// configurePitDashboard();
311320
}
@@ -348,6 +357,23 @@ public void noNote() {
348357

349358
public void configurePitDashboard() {
350359

360+
Shuffleboard.getTab("Pit")
361+
.add("Block Shot", new BlockCommand(superStructure))
362+
.withPosition(3, 2)
363+
.withSize(1, 1);
364+
Shuffleboard.getTab("Pit")
365+
.add(
366+
"Trap Elbow Loc",
367+
new ClosedLoopElbowCommand(elbowSubsystem, SuperStructureConstants.kElbowTrapSetPoint))
368+
.withPosition(1, 2)
369+
.withSize(1, 1);
370+
Shuffleboard.getTab("Pit")
371+
.add(
372+
"Trap Wrist Loc",
373+
new ClosedLoopWristCommand(wristSubsystem, SuperStructureConstants.kWristTrapSetPoint))
374+
.withPosition(2, 2)
375+
.withSize(1, 1);
376+
351377
Shuffleboard.getTab("Pit")
352378
.add(
353379
"Position Shot Location",
@@ -379,6 +405,11 @@ public void configurePitDashboard() {
379405
climbSubsystem))
380406
.withSize(1, 1)
381407
.withPosition(4, 1);
408+
409+
Shuffleboard.getTab("Pit")
410+
.add("Turn Mag Off", new OpenLoopMagazineCommand(magazineSubsystem, 0.0))
411+
.withPosition(5, 2)
412+
.withSize(1, 1);
382413
Shuffleboard.getTab("Pit")
383414
.add("Turn Intake Off", new OpenLoopIntakeCommand(intakeSubsystem, 0.0))
384415
.withSize(1, 1)
@@ -668,13 +699,31 @@ private void configureMatchDashboard() {
668699
.addBoolean("CANivore Connected", () -> canivoreStatus)
669700
.withSize(1, 1)
670701
.withPosition(8, 0);
702+
Shuffleboard.getTab("Match")
703+
.addBoolean(
704+
"BREAKER TEMP GOOD", () -> (driveSubsystem.getTemp() < DriveConstants.kNotifyTemp))
705+
.withSize(3, 1)
706+
.withPosition(7, 2);
671707
// Shuffleboard.getTab("Match")
672708
// .add("ZeroRecoveryElbowCommand", new ZeroRecoveryElbowCommand(elbowSubsystem))
673709
// .withSize(1, 1)
674710
// .withPosition(7, 1);
675711
}
676712

677713
public void configureDebugDashboard() {
714+
Shuffleboard.getTab("Debug")
715+
.add("Drive SAFE", new HoldDriveSafeCommand(driveSubsystem))
716+
.withPosition(6, 0)
717+
.withSize(1, 1);
718+
Shuffleboard.getTab("Debug")
719+
.add("Drive IDLE", new IdleDriveCommand(driveSubsystem))
720+
.withPosition(6, 1)
721+
.withSize(1, 1);
722+
Shuffleboard.getTab("Debug")
723+
.addDouble("Breaker Temp", () -> driveSubsystem.getTemp())
724+
.withPosition(6, 2)
725+
.withSize(1, 1);
726+
678727
Shuffleboard.getTab("Debug")
679728
.add(new OperatorRumbleCommand(robotStateSubsystem, xboxController))
680729
.withSize(1, 1)
@@ -842,6 +891,10 @@ private void configureOperatorBindings() {
842891
&& magazineSubsystem.getSpeed() >= MagazineConstants.kPodiumRumbleSpeed))
843892
.onTrue(new OperatorRumbleCommand(robotStateSubsystem, xboxController));
844893

894+
// UnJam Note
895+
new JoystickButton(xboxController, XboxController.Button.kLeftStick.value)
896+
.onTrue(new UnJamUpperNoteCommand(magazineSubsystem));
897+
845898
// Open Loop Elbow
846899
// new Trigger((() -> xboxController.getRightY() > RobotConstants.kJoystickDeadband))
847900
// .onTrue(new OpenLoopElbowCommand(elbowSubsystem, 0.1))

‎src/main/java/frc/robot/commands/auton/FallBack4PieceCommand.java‎

Lines changed: 114 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,114 @@
1+
package frc.robot.commands.auton;
2+
3+
import edu.wpi.first.wpilibj.DriverStation.Alliance;
4+
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
5+
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
6+
import frc.robot.commands.auto.SetHoloContKPCommand;
7+
import frc.robot.commands.drive.DriveAutonCommand;
8+
import frc.robot.commands.drive.ResetGyroCommand;
9+
import frc.robot.commands.drive.setAngleOffsetCommand;
10+
import frc.robot.commands.elbow.ZeroElbowCommand;
11+
import frc.robot.commands.robotState.SubWooferCommand;
12+
import frc.robot.commands.superStructure.SpinUpWheelsCommand;
13+
import frc.robot.constants.SuperStructureConstants;
14+
import frc.robot.subsystems.auto.AutoCommandInterface;
15+
import frc.robot.subsystems.drive.DriveSubsystem;
16+
import frc.robot.subsystems.elbow.ElbowSubsystem;
17+
import frc.robot.subsystems.intake.IntakeSubsystem;
18+
import frc.robot.subsystems.led.LedSubsystem;
19+
import frc.robot.subsystems.magazine.MagazineSubsystem;
20+
import frc.robot.subsystems.robotState.RobotStateSubsystem;
21+
import frc.robot.subsystems.superStructure.SuperStructure;
22+
import frc.robot.subsystems.vision.DeadEyeSubsystem;
23+
24+
public class FallBack4PieceCommand extends SequentialCommandGroup implements AutoCommandInterface {
25+
26+
private DriveAutonCommand midInitWingNote3;
27+
private DriveAutonCommand wingNote3MidInit;
28+
private DriveAutonCommand midInitWingNote2;
29+
private DriveAutonCommand wingNote2MidInit;
30+
private DriveAutonCommand midInitWingNote1;
31+
private DriveAutonCommand wingNote1MidInit;
32+
private boolean hasGenerated = false;
33+
private Alliance alliance = Alliance.Blue;
34+
private RobotStateSubsystem robotStateSubsystem;
35+
private ElbowSubsystem elbowSubsystem;
36+
private DeadEyeSubsystem deadeye;
37+
38+
public FallBack4PieceCommand(
39+
DriveSubsystem driveSubsystem,
40+
RobotStateSubsystem robotStateSubsystem,
41+
SuperStructure superStructure,
42+
MagazineSubsystem magazineSubsystem,
43+
IntakeSubsystem intakeSubsystem,
44+
ElbowSubsystem elbowSubsystem,
45+
DeadEyeSubsystem deadeye,
46+
LedSubsystem ledSubsystem) {
47+
this.robotStateSubsystem = robotStateSubsystem;
48+
this.elbowSubsystem = elbowSubsystem;
49+
this.deadeye = deadeye;
50+
51+
midInitWingNote3 =
52+
new DriveAutonCommand(driveSubsystem, "MiddleInitial1_WingNote3", true, true);
53+
wingNote3MidInit =
54+
new DriveAutonCommand(driveSubsystem, "WingNote3_MiddleInitial1", true, false);
55+
midInitWingNote2 =
56+
new DriveAutonCommand(driveSubsystem, "MiddleInitial1_WingNote2", true, false);
57+
58+
wingNote2MidInit =
59+
new DriveAutonCommand(driveSubsystem, "WingNote2_MiddleInitial1", true, false);
60+
midInitWingNote1 =
61+
new DriveAutonCommand(driveSubsystem, "MiddleInitial1_WingNote1", true, false);
62+
wingNote1MidInit =
63+
new DriveAutonCommand(driveSubsystem, "WingNote1_MiddleInitial1", true, false);
64+
65+
addCommands(
66+
new ResetGyroCommand(driveSubsystem),
67+
new ParallelCommandGroup(
68+
new SpinUpWheelsCommand(
69+
superStructure,
70+
SuperStructureConstants.kShooterSubwooferSetPoint,
71+
SuperStructureConstants.kShooterSubwooferSetPoint),
72+
new setAngleOffsetCommand(driveSubsystem, 0.0),
73+
new SetHoloContKPCommand(driveSubsystem, 1.0),
74+
new ZeroElbowCommand(elbowSubsystem)),
75+
new SubWooferCommand(robotStateSubsystem, superStructure, magazineSubsystem),
76+
midInitWingNote3,
77+
// new WaitCommand(0.05),
78+
// new AutoWaitNoteStagedCommand(robotStateSubsystem),
79+
80+
wingNote3MidInit,
81+
new SubWooferCommand(robotStateSubsystem, superStructure, magazineSubsystem),
82+
midInitWingNote2,
83+
wingNote2MidInit,
84+
// new WaitCommand(0.05),
85+
new AutoWaitNoteStagedCommand(robotStateSubsystem),
86+
new SubWooferCommand(robotStateSubsystem, superStructure, magazineSubsystem),
87+
midInitWingNote1,
88+
wingNote1MidInit,
89+
new ParallelCommandGroup(
90+
// new WaitCommand(0.05),
91+
new SequentialCommandGroup(
92+
new AutoWaitNoteStagedCommand(robotStateSubsystem),
93+
new SpinUpWheelsCommand(
94+
superStructure,
95+
SuperStructureConstants.kShooterSubwooferSetPoint,
96+
SuperStructureConstants.kShooterSubwooferSetPoint))),
97+
new SubWooferCommand(robotStateSubsystem, superStructure, magazineSubsystem));
98+
}
99+
100+
public void generateTrajectory() {
101+
midInitWingNote3.generateTrajectory();
102+
wingNote3MidInit.generateTrajectory();
103+
midInitWingNote2.generateTrajectory();
104+
wingNote2MidInit.generateTrajectory();
105+
midInitWingNote1.generateTrajectory();
106+
wingNote1MidInit.generateTrajectory();
107+
hasGenerated = true;
108+
alliance = robotStateSubsystem.getAllianceColor();
109+
}
110+
111+
public boolean hasGenerated() {
112+
return hasGenerated && (alliance == robotStateSubsystem.getAllianceColor());
113+
}
114+
}

0 commit comments

Comments
 (0)