Skip to content

Commit

Permalink
Merge remote-tracking branch 'origin/broom-time' into ramsete-controller
Browse files Browse the repository at this point in the history
  • Loading branch information
@willitcode
willitcode committed Oct 27, 2023
2 parents cbda510 + 8355ede commit d042ced
Show file tree
Hide file tree
Showing 5 changed files with 76 additions and 96 deletions.
151 changes: 72 additions & 79 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,76 +12,87 @@
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.Constants.OperatorConstants;
import frc.robot.commands.Balance;
import frc.robot.commands.DeployPlunger;
import frc.robot.commands.DriveArcade;
import frc.robot.commands.DriveForwards;
import frc.robot.commands.DriveReverse;
import frc.robot.commands.DriveTank;
import frc.robot.commands.ExtendArm;
import frc.robot.commands.ManualArm;
import frc.robot.commands.ResetDrivePose;
import frc.robot.commands.StartLeavingCommunity;
import frc.robot.commands.StopLeavingCommunity;
import frc.robot.commands.TestDrivePID;
import frc.robot.commands.TrackTrajectory;
import frc.robot.positioning.Pose;
import frc.robot.subsystems.Camera;
import frc.robot.subsystems.Drive;
import frc.robot.subsystems.I2CManager;
import frc.robot.subsystems.Limelight;
import frc.robot.subsystems.LimelightManager;
import frc.robot.subsystems.Plunger;
import frc.robot.subsystems.arm.ArmBase;
import frc.robot.subsystems.arm.ArmClaw;
import frc.robot.subsystems.arm.ArmExtender;
import frc.robot.subsystems.arm.ArmShoulder;
import frc.robot.subsystems.arm.CompilationArm;

@SuppressWarnings("unused")
/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
* periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
* subsystems, commands, and trigger mappings) should be declared here.
*/
public class RobotContainer {
// The robot's subsystems and commands are defined here...
/* Messy, ugly commands and subsystems section
* TODO: organize later

/* ===============================================================
* SUBSYSTEMS
* ===============================================================
*/
/* Autotasks are mostly commented out in here for now because I don't care that they exist */

/* LIMELIGHT */
private final Limelight limelightBack = new Limelight("limelight-back");
private final Limelight limelightFront = new Limelight("limelight-front");
private final LimelightManager limelightManager =
new LimelightManager(limelightBack, limelightFront);
private final Drive driveSubsystem = new Drive(limelightManager);
/* BIG CHUNGUS ARM CODE */
// private final I2CManager I2CManager = new I2CManager();
// private final ArmBase armBase = new ArmBase();
// private final ArmShoulder armShoulder = new ArmShoulder();
// private final ArmExtender armExtender = new ArmExtender(I2CManager);
// private final ArmClaw armClaw = new ArmClaw();
private final LimelightManager limelightManager = new LimelightManager(limelightBack, limelightFront);
/* CAMERA */
private final Camera aimCamera = new Camera(Constants.Camera.PORT_CAMERA_AIM);
// private final CompilationArm compilationArm =
// new CompilationArm(armBase, armClaw, armExtender, armShoulder);
// private final ManualArm manualArm = new ManualArm(armBase, armShoulder, compilationArm);
// private RetractArm retractArmCommand = new RetractArm(armExtender);
private final Plunger plunger = new Plunger();
private final DeployPlunger deployPlunger = new DeployPlunger(plunger);
private final StartLeavingCommunity startLeavingCommunity =
new StartLeavingCommunity(driveSubsystem);
private final StopLeavingCommunity stopLeavingCommunity =
new StopLeavingCommunity(driveSubsystem);
private final DriveForwards driveForwards = new DriveForwards(driveSubsystem);
private final DriveReverse driveReverse = new DriveReverse(driveSubsystem);
private final InstantCommand displayAimVideo =
new InstantCommand(aimCamera::startCamera, aimCamera);
/* DRIVE */
private final Drive driveSubsystem = new Drive(limelightManager);
/* ARM */
private final I2CManager I2CManager = new I2CManager();
private final ArmBase armBase = new ArmBase();
private final ArmShoulder armShoulder = new ArmShoulder();
private final ArmExtender armExtender = new ArmExtender(I2CManager);
private final ArmClaw armClaw = new ArmClaw();
private final CompilationArm compilationArm =
new CompilationArm(armBase, armClaw, armExtender, armShoulder);
private final ManualArm manualArm = new ManualArm(armBase, armShoulder, compilationArm);
private RetractArm retractArmCommand = new RetractArm(armExtender);

/* ===============================================================
* COMMANDS
* ===============================================================
*/

/* AUTO COMMANDS */
private final StartLeavingCommunity startLeavingCommunity = new StartLeavingCommunity(driveSubsystem);
private final StopLeavingCommunity stopLeavingCommunity = new StopLeavingCommunity(driveSubsystem);
/* CAMERA COMMAND */
private final InstantCommand displayAimVideo = new InstantCommand(aimCamera::startCamera, aimCamera);
/* BALANCE COMMAND */
private final Balance balance = new Balance(driveSubsystem);
/* DRIVE COMMANDS*/
private final DriveArcade driveRO = new DriveArcade(driveSubsystem);
private final DriveTank driveFO = new DriveTank(driveSubsystem);
// private final Command openClaw = armClaw.manualOpenClawCommand();
// private final Command closeClaw = armClaw.manualCloseClawCommand();
// private final CloseClawCone closeClawCone = new CloseClawCone(armClaw);
// private final CloseClawCube closeClawCube = new CloseClawCube(armClaw);
// private final ExtendArm extend = new ExtendArm(armExtender);
// private final RetractArm retract = new RetractArm(armExtender);
// private final TaskScheduler taskScheduler = new TaskScheduler();
private final TrackTrajectory demoTrajectoryTrackingCommand =
new TrackTrajectory(Constants.Drive.Trajectories.DEMO_TRAJECTORY, driveSubsystem);
private final TestDrivePID testDrivePID = new TestDrivePID(driveSubsystem);

private final DriveForwards driveForwards = new DriveForwards(driveSubsystem);
private final DriveReverse driveReverse = new DriveReverse(driveSubsystem);
/* ARM COMMANDS */
private final Command openClaw = armClaw.manualOpenClawCommand();
private final Command closeClaw = armClaw.manualCloseClawCommand();
private final CloseClawCone closeClawCone = new CloseClawCone(armClaw);
private final CloseClawCube closeClawCube = new CloseClawCube(armClaw);
private final ExtendArm extend = new ExtendArm(armExtender);
private final RetractArm retract = new RetractArm(armExtender);
/* TRAJECTORY COMMAND */
private final TrackTrajectory demoTrajectoryTrackingCommand = new TrackTrajectory(Constants.Drive.Trajectories.DEMO_TRAJECTORY, driveSubsystem);

// Replace with CommandPS4Controller or CommandJoystick if needed
public static CommandXboxController controller =
new CommandXboxController(OperatorConstants.CONTROLLER_NUMBER);
Expand All @@ -96,14 +107,9 @@ public RobotContainer() {

/* TODO: add ResetDrivePose into auto*/
autoChooser = new SendableChooser<>();
// autoChooser.setDefaultOption("Fling cube + mobility bonus",
// Commands.sequence(Autos.homingRoutine(armShoulder, armBase, armExtender, armClaw,
// compilationArm), new ParallelRaceGroup(new DriveForwards(driveSubsystem), new
// WaitCommand(0.5)), new ParallelRaceGroup(new DriveReverse(driveSubsystem), new
// WaitCommand(0.8)), new ParallelRaceGroup(new DriveForwards(driveSubsystem), new
// WaitCommand(2.5))));
// autoChooser.addOption("Home arm (DOES NOT MOVE)", Autos.homingRoutine(armShoulder, armBase,
// armExtender, armClaw, compilationArm));
autoChooser.setDefaultOption("Fling cube + mobility bonus",
Commands.sequence(Autos.homingRoutine(armShoulder, armBase, armExtender, armClaw, compilationArm), new ParallelRaceGroup(new DriveForwards(driveSubsystem), new WaitCommand(0.5)), new ParallelRaceGroup(new DriveReverse(driveSubsystem), new WaitCommand(0.8)), new ParallelRaceGroup(new DriveForwards(driveSubsystem), new WaitCommand(2.5))));
autoChooser.addOption("Home arm (DOES NOT MOVE)", Autos.homingRoutine(armShoulder, armBase, armExtender, armClaw, compilationArm));

SmartDashboard.putData("Choose auto", autoChooser);

Expand All @@ -112,9 +118,7 @@ public RobotContainer() {
}

public Pose containerGetArmPose() {

// return compilationArm.getArmPose();
return new Pose();
return compilationArm.getArmPose();
}

/**
Expand All @@ -128,33 +132,24 @@ public Pose containerGetArmPose() {
*/
private void configureBindings() {

// controller.y().whileTrue(deployPlunger);

controller.rightBumper().whileTrue(balance);

controller.leftBumper().whileTrue(new InstantCommand(driveSubsystem::printPoses));

controller.y().toggleOnTrue(driveFO);

controller.b().whileTrue(deployPlunger);

controller.x().whileTrue(demoTrajectoryTrackingCommand);

controller.a().whileTrue(testDrivePID);
joystick.button(9).whileTrue(MoveToPose.extendingMoveToPose(Constants.Arm.Poses.PICKUP, armBase,
armShoulder, armExtender, compilationArm));
joystick.button(4).whileTrue(Autos.homingNoOpenClaw(armShoulder, armBase, armExtender, compilationArm));
joystick.povUp().whileTrue(extend);
joystick.povDown().whileTrue(retract);

// joystick.button(9).whileTrue(MoveToPose.extendingMoveToPose(Constants.Arm.Poses.PICKUP,
// armBase,
// armShoulder, armExtender, compilationArm));

// joystick.button(4).whileTrue(Autos.homingNoOpenClaw(armShoulder, armBase, armExtender,
// compilationArm));

// joystick.povUp().whileTrue(extend);
// joystick.povDown().whileTrue(retract);

// joystick.button(11).whileTrue(openClaw);
joystick.button(11).whileTrue(openClaw);
/* TODO: get better buttons with Gabriel */
/* Or just get color sensor working */
joystick.trigger().whileTrue(closeClaw);
joystick.button(2).whileTrue(closeClawCube);

/*joystick
.button(2)
Expand Down Expand Up @@ -222,10 +217,10 @@ private void configureBindings() {
compilationArm));
joystick
.button(1)
.onTrue(armClaw.manualCloseClawCommand());*/
.onTrue(armClaw.manualCloseClawCommand());
*/
}

private void verifyAutoTasks() {}


/**
* Use this to pass the autonomous command to the main {@link Robot} class.
Expand All @@ -238,15 +233,13 @@ public Command getAutonomousCommand() {
}

public Command getResetCommand() {
// return MoveToPose.retractingMoveToPose(
// Constants.Arm.Poses.RESET, armBase, armShoulder, armExtender, compilationArm)
// .alongWith(armClaw.openClawCommand());
return new InstantCommand();
return MoveToPose.retractingMoveToPose(
Constants.Arm.Poses.RESET, armBase, armShoulder, armExtender, compilationArm)
.alongWith(armClaw.openClawCommand());
}

public Command getRetractCommand() {
// return retractArmCommand;
return new InstantCommand();
return retractArmCommand;
}

public Command getDisplayAimVideoCommand() {
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/commands/DriveForwards.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
drivetrain.moveSimple(0.5, 0.5);
drivetrain.moveTank(0.5, 0.5);
}

// Called once the command ends or is interrupted.
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/commands/DriveReverse.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
drivetrain.moveSimple(-0.5, -0.5);
drivetrain.moveTank(-0.5, -0.5);
}

// Called once the command ends or is interrupted.
Expand Down
11 changes: 1 addition & 10 deletions src/main/java/frc/robot/subsystems/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -247,16 +247,7 @@ public void moveArcade(double x, double z) {
public void moveTank(double left, double right) {
drivetrain.tankDrive(left, right);
}

/**
* @deprecated Use {@link #moveTank}
*/
@Deprecated(forRemoval = true)
public void moveSimple(double leftSpeed, double rightSpeed) {
left.set(ControlMode.PercentOutput, leftSpeed);
right.set(ControlMode.PercentOutput, rightSpeed);
}


/**
* Sets the velocity setpoint of the left and right sides of the drivetrain.
*
Expand Down
6 changes: 1 addition & 5 deletions src/main/java/frc/robot/subsystems/Limelight.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,14 +56,10 @@ public Limelight(String name) {
txSubscriber = NetworkTableInstance.getDefault().getDoubleTopic(fmtPath("tx")).subscribe(0.0);
tySubscriber = NetworkTableInstance.getDefault().getDoubleTopic(fmtPath("ty")).subscribe(0.0);
taSubscriber = NetworkTableInstance.getDefault().getDoubleTopic(fmtPath("ta")).subscribe(0.0);
/* In theory this won't break. It got mad when I tried to insert the array into the
* method like .subscribe({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) so ¯\_(ツ)_/¯
*/
double[] defaultBotpos = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
botposSubscriber =
NetworkTableInstance.getDefault()
.getDoubleArrayTopic(fmtPath("botpose"))
.subscribe(defaultBotpos);
.subscribe(new double[]{0.0, 0.0, 0.0, 0.0, 0.0, 0.0});
}

/* now its time for getter method chaingun, which I have to write manually because VS Code */
Expand Down

0 comments on commit d042ced

Please sign in to comment.