Skip to content

Commit

Permalink
post offseason (#77)
Browse files Browse the repository at this point in the history
* added noRamsete autos to the shuffleboard

* offseason - end of day 1

* offseason - end of day 1

* offseason - final

* post offseason - rewrite intakeBallsCommand

Co-authored-by: Excalibur FRC | 6738 <[email protected]>
  • Loading branch information
TapChap and ExcaliburFRC6738 authored Oct 16, 2022
1 parent 6b50e54 commit 62ccc82
Show file tree
Hide file tree
Showing 17 changed files with 988 additions and 73 deletions.
674 changes: 674 additions & 0 deletions comp.json

Large diffs are not rendered by default.

167 changes: 167 additions & 0 deletions shuffleboard.json
Original file line number Diff line number Diff line change
Expand Up @@ -476,6 +476,173 @@
}
}
}
},
{
"title": "Tab 3",
"autoPopulate": false,
"autoPopulatePrefix": "",
"widgetPane": {
"gridSize": 100.0,
"showGrid": true,
"hgap": 16.0,
"vgap": 16.0,
"titleType": 0,
"tiles": {
"1,2": {
"size": [
1,
1
],
"content": {
"_type": "ComboBox Chooser",
"_source0": "network_table:///SmartDashboard/Autos",
"_title": "/SmartDashboard/Autos"
}
},
"1,3": {
"size": [
1,
1
],
"content": {
"_type": "Text View",
"_source0": "network_table:///LiveWindow/Intake/Cargo Count",
"_title": "/LiveWindow/Intake/Cargo Count"
}
},
"1,1": {
"size": [
1,
1
],
"content": {
"_type": "Boolean Box",
"_source0": "network_table:///LiveWindow/Intake/Intake Cargo",
"_title": "/LiveWindow/Intake/Intake Cargo",
"Colors/Color when true": "#7CFC00FF",
"Colors/Color when false": "#8B0000FF"
}
},
"1,0": {
"size": [
1,
1
],
"content": {
"_type": "Boolean Box",
"_source0": "network_table:///LiveWindow/Intake/Upper Cargo",
"_title": "/LiveWindow/Intake/Upper Cargo",
"Colors/Color when true": "#7CFC00FF",
"Colors/Color when false": "#8B0000FF"
}
},
"0,3": {
"size": [
1,
1
],
"content": {
"_type": "Text View",
"_source0": "network_table:///LiveWindow/Shooter/rps",
"_title": "/LiveWindow/Shooter/rps"
}
},
"0,4": {
"size": [
1,
1
],
"content": {
"_type": "Text View",
"_source0": "network_table:///LiveWindow/Shooter/velocity",
"_title": "/LiveWindow/Shooter/velocity"
}
},
"0,0": {
"size": [
1,
1
],
"content": {
"_type": "Boolean Box",
"_source0": "network_table:///LiveWindow/Ungrouped/Compressor[0]/Enabled",
"_title": "/LiveWindow/Ungrouped/Compressor[0]/Enabled",
"Colors/Color when true": "#7CFC00FF",
"Colors/Color when false": "#8B0000FF"
}
},
"0,2": {
"size": [
1,
1
],
"content": {
"_type": "ComboBox Chooser",
"_source0": "network_table:///SmartDashboard/initial balls",
"_title": "/SmartDashboard/initial balls"
}
},
"7,0": {
"size": [
6,
4
],
"content": {
"_type": "Field",
"_source0": "network_table:///SmartDashboard/Field",
"_title": "/SmartDashboard/Field",
"Game/Game": "A2022_Rapid_React",
"Visuals/Robot Icon Size": 50.0,
"Visuals/Show Outside Circles": false
}
},
"0,1": {
"size": [
1,
1
],
"content": {
"_type": "Boolean Box",
"_source0": "network_table:///LiveWindow/Intake/Allow",
"_title": "/LiveWindow/Intake/Allow",
"Colors/Color when true": "#7CFC00FF",
"Colors/Color when false": "#8B0000FF"
}
},
"2,0": {
"size": [
5,
5
],
"content": {
"_type": "Camera Stream",
"_source0": "camera_server://mmal_service_16.1-input",
"_title": "mmal_service_16.1-input",
"Crosshair/Show crosshair": true,
"Crosshair/Crosshair color": "#FFFFFFFF",
"Controls/Show controls": true,
"Controls/Rotation": "NONE",
"compression": -1.0,
"fps": -1,
"imageWidth": 0,
"imageHeight": 0
}
}
}
}
},
{
"title": "Tab 4",
"autoPopulate": false,
"autoPopulatePrefix": "",
"widgetPane": {
"gridSize": 100.0,
"showGrid": true,
"hgap": 16.0,
"vgap": 16.0,
"titleType": 0,
"tiles": {}
}
}
],
"windowGeometry": {
Expand Down
6 changes: 3 additions & 3 deletions src/main/java/io/excaliburfrc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -19,10 +19,10 @@ public final class Constants {
public static final class IntakeConstants {
public static final int INTAKE_MOTOR_ID = 40; // PDP 4
public static final int UPPER_MOTOR_ID = 41; // PDP 11
public static final int UPPER_PING = 9;
public static final int UPPER_ECHO = 8;
public static final int UPPER_PING = 7;
public static final int UPPER_ECHO = 6;
public static final int COLOR_LIMIT = 55;
public static final int SONIC_LIMIT = 110;
public static final int SONIC_LIMIT = 150;
public static final int MAX_BALLS = 2;
public static final int FWD_CHANNEL = 3;
public static final int REV_CHANNEL = 4;
Expand Down
32 changes: 27 additions & 5 deletions src/main/java/io/excaliburfrc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,10 @@
import edu.wpi.first.wpilibj2.command.*;
import edu.wpi.first.wpilibj2.command.button.Button;
import edu.wpi.first.wpilibj2.command.button.POVButton;
import io.excaliburfrc.lib.RepeatingCommand;
import io.excaliburfrc.robot.commands.auto.noramsete.DontMove;
import io.excaliburfrc.robot.commands.auto.noramsete.NoRamseteBottomFender;
import io.excaliburfrc.robot.commands.auto.noramsete.NoRamseteTopFender;
import io.excaliburfrc.robot.commands.auto.oneBall.BallFive;
import io.excaliburfrc.robot.commands.auto.oneBall.BallFour;
import io.excaliburfrc.robot.commands.auto.twoBalls.TwoBalls_FiveTerminal;
Expand Down Expand Up @@ -33,6 +37,12 @@ public class RobotContainer {
public final SendableChooser<Integer> initialBallCounter = new SendableChooser<>();

public RobotContainer() {
chooser.addOption(
"dontMove", new DontMove(drive, superstructure, leds));
chooser.addOption(
"topFenderSimple", new NoRamseteTopFender(drive, superstructure, leds));
chooser.addOption(
"bottomFenderSimple", new NoRamseteBottomFender(drive, superstructure, leds));
chooser.addOption(
"fiveFour", new TwoBalls_FiveFour(drive, leds, superstructure));
chooser.addOption(
Expand Down Expand Up @@ -71,7 +81,8 @@ void configureButtonBindings() {
new Button(() -> CommandScheduler.getInstance().requiring(superstructure.intake) != null)
.whenReleased(superstructure.intake.closePiston());

new Button(armJoystick::getL2Button).toggleWhenPressed(superstructure.intakeBallCommand());
new Button(armJoystick::getL2ButtonPressed)
.toggleWhenPressed(superstructure.intakeBallsCommand());

new Button(armJoystick::getL1Button).toggleWhenPressed(superstructure.ejectBallCommand());

Expand Down Expand Up @@ -102,6 +113,13 @@ void configureButtonBindings() {
new Button(()-> driveJoystick.getRawButtonPressed(8))
.toggleWhenPressed(superstructure.intake.allowCommand());

new Button(armJoystick::getSquareButtonPressed)
.toggleWhenPressed(
new ConditionalCommand(
superstructure.intake.closePiston(),
superstructure.intake.openPiston(),
superstructure.intake::isOpen));

// new Button(()-> driveJoystick.getRightStickButton())
// .whenPressed(
// new PrintCommand(drive.getOdometryPose().toString()));
Expand All @@ -122,13 +140,16 @@ void manualButton() {
new Button(()-> driveJoystick.getRightTriggerAxis() > 0.1)
.toggleWhenPressed(superstructure.shootBallsCommand(leds));

// when intake is required
// when intake is required
new Button(() -> CommandScheduler.getInstance().requiring(superstructure.intake) != null)
.whenReleased(superstructure.intake.closePiston());

new Button(()-> driveJoystick.getLeftTriggerAxis() > 0.1).toggleWhenPressed(superstructure.intakeBallCommand());

new Button(driveJoystick::getLeftBumperPressed).toggleWhenPressed(superstructure.ejectBallCommand());
new Button(()-> driveJoystick.getLeftTriggerAxis() > 0.1)
.toggleWhenPressed(superstructure.intakeBallsCommand());

new Button(driveJoystick::getLeftBumperPressed)
.toggleWhenPressed(superstructure.ejectBallCommand());

climber
.climberManualCommand(
Expand All @@ -149,7 +170,8 @@ void manualButton() {
superstructure.intake::isOpen));

new Button(()-> driveJoystick.getRawButtonPressed(7))
.toggleWhenPressed(new StartEndCommand(compressor::enableDigital, compressor::disable));
.toggleWhenPressed(
new StartEndCommand(compressor::enableDigital, compressor::disable));

new Button(()-> driveJoystick.getRawButtonPressed(8))
.toggleWhenPressed(superstructure.intake.allowCommand());
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -5,15 +5,13 @@
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import io.excaliburfrc.robot.commands.BlindShootBallsCommand;
import io.excaliburfrc.robot.subsystems.Drive;
import io.excaliburfrc.robot.subsystems.Intake;
import io.excaliburfrc.robot.subsystems.LEDs;
import io.excaliburfrc.robot.subsystems.Shooter;
import io.excaliburfrc.robot.subsystems.*;

public class DontMove extends SequentialCommandGroup {
public DontMove(Drive drive, Intake intake, Shooter shooter, LEDs leds) {
public DontMove(Drive drive, Superstructure superstructure, LEDs leds) {
super(
drive.resetOdometryCommand(new Pose2d(8, 3.3, fromDegrees(204.0))),
new BlindShootBallsCommand(intake, shooter, leds).withTimeout(3));
superstructure.shootBallsCommand(leds)
);
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -6,15 +6,14 @@
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import io.excaliburfrc.robot.commands.BlindShootBallsCommand;
import io.excaliburfrc.robot.subsystems.Drive;
import io.excaliburfrc.robot.subsystems.Intake;
import io.excaliburfrc.robot.subsystems.LEDs;
import io.excaliburfrc.robot.subsystems.Shooter;
import io.excaliburfrc.robot.subsystems.Superstructure;

public class NoRamseteBottomFender extends SequentialCommandGroup {
public NoRamseteBottomFender(Drive drive, Intake intake, Shooter shooter, LEDs leds) {
public NoRamseteBottomFender(Drive drive, Superstructure superstracture, LEDs leds) {
super(
drive.resetOdometryCommand(new Pose2d(8, 3.3, fromDegrees(204.0))),
new BlindShootBallsCommand(intake, shooter, leds).withTimeout(3),
superstracture.shootBallsCommand(leds),
drive.arcadeDriveCommand(() -> -0.4, () -> 0.0).withTimeout(4),
drive.arcadeDriveCommand(() -> 0.0, () -> 0.0));
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -6,15 +6,14 @@
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import io.excaliburfrc.robot.commands.BlindShootBallsCommand;
import io.excaliburfrc.robot.subsystems.Drive;
import io.excaliburfrc.robot.subsystems.Intake;
import io.excaliburfrc.robot.subsystems.LEDs;
import io.excaliburfrc.robot.subsystems.Shooter;
import io.excaliburfrc.robot.subsystems.Superstructure;

public class NoRamseteTopFender extends SequentialCommandGroup {
public NoRamseteTopFender(Drive drive, Intake intake, Shooter shooter, LEDs leds) {
public NoRamseteTopFender(Drive drive, Superstructure superstructure, LEDs leds) {
super(
drive.resetOdometryCommand(new Pose2d(7.5, 4.45, fromDegrees(159.0))),
new BlindShootBallsCommand(intake, shooter, leds).withTimeout(3),
superstructure.shootBallsCommand(leds),
drive.arcadeDriveCommand(() -> -0.4, () -> 0.0).withTimeout(4),
drive.arcadeDriveCommand(() -> 0.0, () -> 0.0));
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ public BallFive(Drive drive, LEDs leds, Superstructure superstructure) {
superstructure.shootBallsCommand(leds),
drive.followTrajectoryCommand(START, innerWaypoints(STOP_1), STOP_2, REVERSE),
drive.followTrajectoryCommand(STOP_2, innerWaypoints(BALL_1), START, FORWARD)
.alongWith(superstructure.intakeBallCommand()),
.alongWith(superstructure.intakeBallsCommand()),
superstructure.shootBallsCommand(leds));
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ public BallFour(Drive drive, LEDs leds, Superstructure superstructure) {
drive.resetOdometryCommand(start),
drive.followTrajectoryCommand(start, innerWaypoints(), stop1, Rconfig),
drive.followTrajectoryCommand(stop1, innerWaypoints(), ball1, Fconfig)
.alongWith(superstructure.intakeBallCommand()),
.alongWith(superstructure.intakeBallsCommand()),
drive.followTrajectoryCommand(ball1, innerWaypoints(), stop1, Rconfig),
drive.followTrajectoryCommand(stop1, innerWaypoints(), start, Fconfig),
superstructure.shootBallsCommand(leds)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ public BallSeven(Drive drive, LEDs leds, Superstructure superstructure) {
superstructure.shootBallsCommand(leds),
drive.followTrajectoryCommand(
start, innerWaypoints(stop1, ball7), end, FORWARD)
.alongWith(superstructure.intakeBallCommand())
.alongWith(superstructure.intakeBallsCommand())
.andThen(superstructure.shootBallsCommand(leds))
);
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ public BallSix(Drive drive, LEDs leds, Superstructure superstructure) {
drive.followTrajectoryCommand(
start, innerWaypoints(), STOP1, REVERSE),
drive.followTrajectoryCommand(STOP1, innerWaypoints(ball1), start, FORWARD)
.alongWith(superstructure.intakeBallCommand()),
.alongWith(superstructure.intakeBallsCommand()),
superstructure.shootBallsCommand(leds));
}
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -14,21 +14,24 @@
import static io.excaliburfrc.robot.commands.auto.Trajectories.*;

public class TwoBalls_FiveFour extends SequentialCommandGroup {
static final Pose2d START = new Pose2d(7.95, 2.6, Rotation2d.fromDegrees(90));
static final Pose2d START = new Pose2d(7.95, 2.6, Rotation2d.fromDegrees(90));
static final Pose2d ball1 = new Pose2d(Trajectories.OUR_CARGO_5.getX(), Trajectories.OUR_CARGO_5.getY(), fromDegrees(-135));
static final Pose2d ball2 = new Pose2d(Trajectories.OUR_CARGO_4.getX(), Trajectories.OUR_CARGO_4.getY(), fromDegrees(0));
static final Pose2d STOP1 = new Pose2d(7.95, 1.8,START.getRotation());
static final Pose2d ball2 = new Pose2d(Trajectories.OUR_CARGO_4.getX() + 0.15, Trajectories.OUR_CARGO_4.getY(), fromDegrees(0));
static final Pose2d STOP1 = new Pose2d(7.95, 1.8, START.getRotation());
static final Pose2d END = new Pose2d(START.getX(), START.getY() + 0.2, fromDegrees(78));

public TwoBalls_FiveFour(Drive drive, LEDs leds, Superstructure superstructure) {
super(
drive.resetOdometryCommand(START),
superstructure.shootBallsCommand(leds),
drive.followTrajectoryCommand(START, innerWaypoints(), STOP1, REVERSE),
new SequentialCommandGroup(
drive.followTrajectoryCommand(STOP1, innerWaypoints(), ball1, FORWARD),
drive.followTrajectoryCommand(ball1, innerWaypoints(), ball2, FORWARD),
drive.followTrajectoryCommand(ball2, innerWaypoints(), START, FORWARD)
).alongWith(superstructure.intakeBallCommand().andThen(superstructure.intakeBallCommand())),
superstructure.shootBallsCommand(leds));
}
super(
drive.resetOdometryCommand(START),
superstructure.shootBallsCommand(leds),
drive.followTrajectoryCommand(START, innerWaypoints(), STOP1, REVERSE),
new SequentialCommandGroup(
drive.followTrajectoryCommand(STOP1, innerWaypoints(), ball1, FORWARD),
drive.followTrajectoryCommand(ball1, innerWaypoints(), ball2, FORWARD),
drive.followTrajectoryCommand(ball2, innerWaypoints(), END, FORWARD))
.deadlineWith(
superstructure.intakeBallsCommand()),
superstructure.intake.closePiston(),
superstructure.shootBallsCommand(leds));
}
}
Loading

0 comments on commit 62ccc82

Please sign in to comment.