Skip to content

Commit f2e2406

Browse files
Changes from last day of Granite State District event. Added semi-automatic firing sequence for "close shots," fixed intake constants to be static, tweaks to autonomous programs and other miscellaneous tweaks.
1 parent 7a4e3ee commit f2e2406

17 files changed

+154
-66
lines changed

src/main/java/org/mayheminc/robot2020/RobotContainer.java

Lines changed: 14 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -85,20 +85,15 @@ private void configureDefaultCommands() {
8585
// intake.setDefaultCommand(new IntakeExtenderVBus());
8686
revolver.setDefaultCommand(new RevolverDefault());
8787

88-
// TODO: Figure out if the current approach of "AirCompressorDefault()" is the
89-
// way to go for compressor control.
9088
// KBS doesn't think the below is the right way to have the compressor be on "by
91-
// default" because
92-
// it would require there to always be a command running to keep the compressor
93-
// off. However, that
94-
// is a good way to ensure it doesn't get left off by accident. Not quite sure
95-
// how to handle this;
89+
// default" because it would require there to always be a command running to
90+
// keep the compressor off.
91+
// However, that is a good way to ensure it doesn't get left off by accident.
92+
// Not quite sure how to handle this;
9693
// would really rather that other commands which need the compressor off (such
97-
// as a high-power command
98-
// which wants all the battery power available) would turn the compressor off
99-
// when the command starts
100-
// and off when the command ends.) Then again, maybe the "defaultCommand" is a
101-
// good way to do this
94+
// as a high-power command which wants all the battery power available) would
95+
// turn the compressor off when the command starts and off when the command
96+
// ends.) Then again, maybe the "defaultCommand" is a good way to do this
10297
// and I just don't understand the style yet.
10398
// compressor.setDefaultCommand(new AirCompressorDefault());
10499
}
@@ -206,7 +201,11 @@ private void configureDriverPadButtons() {
206201
DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whenHeld(new ShooterFiringSequence(60.0));
207202
DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whenReleased(new ShooterCeaseFire());
208203

209-
DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whileHeld(new FeederSet(1.0));
204+
DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whileHeld(new ShooterCloseFiringSequence(60.0));
205+
DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whenReleased(new ShooterCeaseFire());
206+
207+
// DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whileHeld(new
208+
// FeederSet(1.0));
210209

211210
}
212211

@@ -215,9 +214,9 @@ private void configureOperatorStickButtons() {
215214

216215
private void configureOperatorPadButtons() {
217216
OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whileHeld(new RevolverSetTurntable(0.2));
218-
OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN));
217+
OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO.whenPressed(new IntakeSetPosition(Intake.PIVOT_DOWN));
219218
OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whileHeld(new RevolverSetTurntable(1.0));
220-
OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_UP));
219+
OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new IntakeSetPosition(Intake.PIVOT_UP));
221220

222221
// new ShooterSetWheel(1000));
223222
OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new ChimneySet(1.0));

src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWDriveOnlyToRP.java

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -8,8 +8,9 @@
88
package org.mayheminc.robot2020.autonomousroutines;
99

1010
import org.mayheminc.robot2020.commands.*;
11-
import org.mayheminc.robot2020.RobotContainer;
1211
import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits;
12+
import org.mayheminc.robot2020.subsystems.Intake;
13+
1314
import edu.wpi.first.wpilibj2.command.*;
1415

1516
public class StartBWDriveOnlyToRP extends SequentialCommandGroup {
@@ -22,7 +23,7 @@ public StartBWDriveOnlyToRP() {
2223
addCommands(new DriveZeroGyro(180.0));
2324

2425
// lower the intake
25-
addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN));
26+
addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN));
2627

2728
// then, drive to the rendezvous point
2829
addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 12, 180));

src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWDriveOnlyToWall.java

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -8,8 +8,9 @@
88
package org.mayheminc.robot2020.autonomousroutines;
99

1010
import org.mayheminc.robot2020.commands.*;
11-
import org.mayheminc.robot2020.RobotContainer;
1211
import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits;
12+
import org.mayheminc.robot2020.subsystems.Intake;
13+
1314
import edu.wpi.first.wpilibj2.command.*;
1415

1516
public class StartBWDriveOnlyToWall extends SequentialCommandGroup {
@@ -22,7 +23,7 @@ public StartBWDriveOnlyToWall() {
2223
addCommands(new DriveZeroGyro(180.0));
2324

2425
// lower the intake
25-
addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN));
26+
addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN));
2627

2728
// then, drive towards the wall
2829
addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 40, 180));

src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java

Lines changed: 9 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -7,10 +7,10 @@
77

88
package org.mayheminc.robot2020.autonomousroutines;
99

10-
import org.mayheminc.robot2020.RobotContainer;
1110
import org.mayheminc.robot2020.commands.*;
1211
import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits;
1312
import org.mayheminc.robot2020.subsystems.Hood;
13+
import org.mayheminc.robot2020.subsystems.Intake;
1414
import org.mayheminc.robot2020.subsystems.ShooterWheel;
1515
import org.mayheminc.robot2020.subsystems.Turret;
1616

@@ -24,10 +24,11 @@ public StartBWOppTrench() {
2424

2525
addCommands(new DriveZeroGyro(180.0));
2626

27-
// lower the intake and turn it on before driving forwards
28-
addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN));
29-
addCommands(new IntakeSetRollers(-1.0));
27+
// lower the intake and wait for it to be on before turning on rollers or
28+
// driving forwards
29+
addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN));
3030
addCommands(new Wait(2.5));
31+
addCommands(new IntakeSetRollers(-1.0));
3132

3233
// make sure the hood is down
3334
addCommands(new HoodSetAbsWhileHeld(Hood.TARGET_ZONE_POSITION));
@@ -37,7 +38,7 @@ public StartBWOppTrench() {
3738

3839
// now driven to get the balls from opposing trench
3940
addCommands(new Wait(0.8), // wait for last two balls to get into robot
40-
new IntakeSetRollers(0), // turn off the intake
41+
// new IntakeSetRollers(0), // slow down the intake
4142
new DriveStraightOnHeading(-0.4, DistanceUnits.INCHES, 36, 180)); // start backing up
4243
// slowly
4344

@@ -46,7 +47,7 @@ public StartBWOppTrench() {
4647

4748
// in shooting position, prepare everything for shooting
4849
addCommands(new ParallelCommandGroup( // run the following commands in parallel:
49-
new ShooterWheelSet(ShooterWheel.INITIATION_LINE_SPEED),
50+
new ShooterWheelSet(ShooterWheel.IDLE_SPEED),
5051
new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION + 3000.0),
5152
new TurretSetAbs((105.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE)));
5253

@@ -57,13 +58,13 @@ public StartBWOppTrench() {
5758
addCommands(new Wait(0.2));
5859

5960
// use the "one button" firing sequence
60-
addCommands(new ShooterFiringSequence(5.0));
61+
addCommands(new ShooterFiringSequence(6.0));
6162

6263
// turn the shooter wheel and intake off now that the shooting is all done
6364
addCommands(new ShooterWheelSet(ShooterWheel.IDLE_SPEED));
6465
addCommands(new IntakeSetRollers(0.0));
6566

6667
// turn the wheel off now that the shooting is all done
67-
addCommands(new HoodSetAbsWhileHeld(Hood.TARGET_ZONE_POSITION));
68+
addCommands(new HoodSetAbsWhileHeld(Hood.STARTING_POSITION));
6869
}
6970
}

src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -7,9 +7,9 @@
77

88
package org.mayheminc.robot2020.autonomousroutines;
99

10-
import org.mayheminc.robot2020.RobotContainer;
1110
import org.mayheminc.robot2020.commands.*;
1211
import org.mayheminc.robot2020.subsystems.Hood;
12+
import org.mayheminc.robot2020.subsystems.Intake;
1313
import org.mayheminc.robot2020.subsystems.ShooterWheel;
1414
import org.mayheminc.robot2020.subsystems.Turret;
1515

@@ -26,8 +26,7 @@ public StartBWShoot3() {
2626
// first, lower the intake, start the shooter wheel, and wait until the turret
2727
// is turned towards the target
2828
addCommands(new ParallelCommandGroup( // run the following commands in parallel:
29-
new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN),
30-
new ShooterWheelSet(ShooterWheel.IDLE_SPEED),
29+
new IntakeSetPosition(Intake.PIVOT_DOWN), new ShooterWheelSet(ShooterWheel.IDLE_SPEED),
3130
new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION),
3231
new TurretSetAbs((180.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE)));
3332

src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -39,13 +39,13 @@ public StartBWTrench(double extraDistance) {
3939

4040
// now driven to the balls at far end of trench
4141
addCommands(new Wait(0.8), // wait for last two balls to get into robot
42-
new IntakeSetRollers(0), // turn off the intake
4342
new DriveStraightOnHeading(-0.5, DistanceUnits.INCHES, 12, 180)); // start backing up
4443
// slowly
4544

4645
// after getting all three balls, go back to shooting position
4746
// first, make sure we drive straight out from under the control panel
4847
addCommands(new DriveStraightOnHeading(-0.6, DistanceUnits.INCHES, 8 + extraDistance, 180));
48+
addCommands(new IntakeSetRollers(0.0)); // turn off the intake in case it has been stalled for a while
4949

5050
// drive diagonally over towards the shooting position, while turning on shooter
5151
// wheels, raising the hood, and re-aiming the turret
@@ -67,7 +67,7 @@ public StartBWTrench(double extraDistance) {
6767
addCommands(new ShooterWheelSet(ShooterWheel.IDLE_SPEED));
6868
addCommands(new IntakeSetRollers(0.0));
6969

70-
// turn the wheel off now that the shooting is all done
71-
addCommands(new HoodSetAbsWhileHeld(Hood.TARGET_ZONE_POSITION));
70+
// put the hood down now that the shooting is all done
71+
addCommands(new HoodSetAbsWhileHeld(Hood.STARTING_POSITION));
7272
}
7373
}

src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWDriveOnlyToRP.java

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -8,8 +8,9 @@
88
package org.mayheminc.robot2020.autonomousroutines;
99

1010
import org.mayheminc.robot2020.commands.*;
11-
import org.mayheminc.robot2020.RobotContainer;
1211
import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits;
12+
import org.mayheminc.robot2020.subsystems.Intake;
13+
1314
import edu.wpi.first.wpilibj2.command.*;
1415

1516
public class StartFWDriveOnlyToRP extends SequentialCommandGroup {
@@ -22,7 +23,7 @@ public StartFWDriveOnlyToRP() {
2223
addCommands(new DriveZeroGyro(0.0));
2324

2425
// lower the intake
25-
addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN));
26+
addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN));
2627

2728
// then, drive to the RP
2829
addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 12, 0));

src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWDriveOnlyToWall.java

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -8,8 +8,9 @@
88
package org.mayheminc.robot2020.autonomousroutines;
99

1010
import org.mayheminc.robot2020.commands.*;
11-
import org.mayheminc.robot2020.RobotContainer;
1211
import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits;
12+
import org.mayheminc.robot2020.subsystems.Intake;
13+
1314
import edu.wpi.first.wpilibj2.command.*;
1415

1516
public class StartFWDriveOnlyToWall extends SequentialCommandGroup {
@@ -22,7 +23,7 @@ public StartFWDriveOnlyToWall() {
2223
addCommands(new DriveZeroGyro(0.0));
2324

2425
// lower the intake
25-
addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN));
26+
addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN));
2627

2728
// then, drive to the RP
2829
addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 40, 0));

src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java

Lines changed: 4 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -7,10 +7,10 @@
77

88
package org.mayheminc.robot2020.autonomousroutines;
99

10-
import org.mayheminc.robot2020.RobotContainer;
1110
import org.mayheminc.robot2020.commands.*;
1211
import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits;
1312
import org.mayheminc.robot2020.subsystems.Hood;
13+
import org.mayheminc.robot2020.subsystems.Intake;
1414
import org.mayheminc.robot2020.subsystems.ShooterWheel;
1515
import org.mayheminc.robot2020.subsystems.Turret;
1616

@@ -25,14 +25,13 @@ public StartFWRendezvous() {
2525
// start out facing in the normal direction
2626
addCommands(new DriveZeroGyro(0.0));
2727

28-
addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN));
28+
addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN));
2929

3030
// shoot the 3 balls we started with
3131
// first, lower the intake, start the shooter wheel, and wait until the turret
3232
// is turned towards the target
3333
addCommands(new ParallelCommandGroup( // run the following commands in parallel:
34-
new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN),
35-
new ShooterWheelSet(ShooterWheel.IDLE_SPEED),
34+
new IntakeSetPosition(Intake.PIVOT_DOWN), new ShooterWheelSet(ShooterWheel.IDLE_SPEED),
3635
new HoodSetAbs(Hood.INITIATION_LINE_POSITION),
3736
new TurretSetAbs((10.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE)));
3837

@@ -43,7 +42,7 @@ public StartFWRendezvous() {
4342

4443
// now that we are clear of other robots, lower the intake while backing up
4544
// further
46-
addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN));
45+
addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN));
4746

4847
// raise the hood a little to shoot from this increased distance
4948
addCommands(new HoodSetAbsWhileHeld((Hood.INITIATION_LINE_POSITION + Hood.TRENCH_MID_POSITION) / 2.0));

src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java

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

88
package org.mayheminc.robot2020.autonomousroutines;
99

10-
import org.mayheminc.robot2020.RobotContainer;
1110
import org.mayheminc.robot2020.commands.*;
1211
import org.mayheminc.robot2020.subsystems.Hood;
12+
import org.mayheminc.robot2020.subsystems.Intake;
1313
import org.mayheminc.robot2020.subsystems.ShooterWheel;
1414
import org.mayheminc.robot2020.subsystems.Turret;
1515

@@ -26,7 +26,7 @@ public StartFWShoot3() {
2626
// first, lower the intake, start the shooter wheel, and wait until the turret
2727
// is turned towards the target
2828
addCommands(new ParallelCommandGroup( // run the following commands in parallel:
29-
new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN),
29+
new IntakeSetPosition(Intake.PIVOT_DOWN),
3030
new ShooterWheelSet(ShooterWheel.INITIATION_LINE_SPEED),
3131
new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION),
3232
new TurretSetAbs((0.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE)));

0 commit comments

Comments
 (0)