Skip to content

Commit

Permalink
Restructure
Browse files Browse the repository at this point in the history
  • Loading branch information
@WindingMotor
WindingMotor committed Feb 3, 2025
1 parent df58f42 commit 669ff27
Show file tree
Hide file tree
Showing 17 changed files with 198 additions and 284 deletions.
16 changes: 1 addition & 15 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,12 +10,10 @@
import com.reduxrobotics.canand.CanandEventLoop;
import edu.wpi.first.wpilibj.PowerDistribution;
import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc.robot.constants.BuildConstants;
import frc.robot.constants.RobotConstants;
import frc.robot.util.AlertManager;
import org.littletonrobotics.junction.LogFileUtil;
import org.littletonrobotics.junction.LoggedRobot;
import org.littletonrobotics.junction.Logger;
Expand Down Expand Up @@ -80,13 +78,7 @@ public void robotPeriodic() {
}

@Override
public void disabledInit() {
// Check battery voltage when disabled
double voltage = RobotController.getBatteryVoltage();
if (voltage <= RobotConstants.BATTERY_VOLTAGE_CRITICAL) {
AlertManager.setAlert(AlertManager.Alerts.CRITICAL_BATTERY_ON_END, true);
}
}
public void disabledInit() {}

@Override
public void disabledPeriodic() {}
Expand Down Expand Up @@ -114,12 +106,6 @@ public void teleopInit() {
if (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
}

// Check battery voltage at teleop start
double voltage = RobotController.getBatteryVoltage();
if (voltage <= RobotConstants.BATTERY_VOLTAGE_CRITICAL) {
AlertManager.setAlert(AlertManager.Alerts.CRITICAL_BATTERY_ON_START, true);
}
}

@Override
Expand Down
205 changes: 84 additions & 121 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,139 +17,102 @@
import frc.robot.commands.drive.CMD_Drive;
import frc.robot.commands.generic.CMD_Elevator;
import frc.robot.commands.generic.CMD_Superstructure;
import frc.robot.constants.FieldConstants;
import frc.robot.constants.InputConstants;
import frc.robot.elevator.IO_ElevatorReal;
import frc.robot.elevator.SUB_Elevator;
import frc.robot.intake.IO_IntakeReal;
import frc.robot.intake.SUB_Intake;
import frc.robot.misc.SUB_Led;
import frc.robot.superstructure.SUB_Superstructure;
import frc.robot.superstructure.SuperstructureState;
import frc.robot.swerve.IO_SwerveReal;
import frc.robot.swerve.SUB_Swerve;
import frc.robot.util.SUB_Led;
import frc.robot.vision.IO_VisionReal;
import frc.robot.vision.IO_VisionSim;
import frc.robot.vision.SUB_Vision;
import frc.robot.webserver.WebServer;

import java.io.File;

public class RobotContainer {

private final CommandXboxController driverController;
private final CommandXboxController operatorController;

// private final WebServer webServer;

private final SUB_Swerve swerve;
private final SUB_Intake intake;
private final SUB_Vision vision;
private final SUB_Elevator elevator;
private final SUB_Superstructure superstructure;

private final SUB_Led led;

private final InputConstants globalInputMap;

public RobotContainer() {

// webServer = new WebServer();

// Controllers
driverController = new CommandXboxController(0); // port 0
globalInputMap = InputConstants.TX16S_MAIN; // Set the global input map to Xbox Controller
operatorController = new CommandXboxController(1);

// Driving Sybsystems
vision = new SUB_Vision(Robot.isSimulation() ? new IO_VisionSim() : new IO_VisionReal());
swerve = new SUB_Swerve(new IO_SwerveReal(new File(Filesystem.getDeployDirectory(), "swerve")));

// Mechanisms Subsystems
intake = new SUB_Intake(new IO_IntakeReal());
elevator = new SUB_Elevator(new IO_ElevatorReal());
led = new SUB_Led();

// Superstructure
superstructure = new SUB_Superstructure(intake, elevator, led);

configureDefaultCommands();
configureWebserverCommands();
configurePathPlannerCommands();
configureButtonBindings();
}

private void configureDefaultCommands() {

// swerve.setDefaultCommand(
// new CMD_DriveAlign(swerve, driverController, globalInputMap)); // Drive and aim at speaker

swerve.setDefaultCommand(new CMD_Drive(swerve, driverController, globalInputMap));

// elevator.setDefaultCommand(new CMD_ElevatorManual(elevator, operatorController));

// operatorController.a().onFalse(elevator.setVoltage(0));

// operatorController.b().onTrue(elevator.setVoltage(0));

/*
* operatorController.a().onTrue(elevator.setV(SUB_Elevator.State.L1_SCORING)); // 0.5m
operatorController.a().onFalse(elevator.setState(SUB_Elevator.State.STOWED));
operatorController.b().onTrue(elevator.setState(SUB_Elevator.State.STOWED));
*/
}

private void configureWebserverCommands() {
// webServer.registerCommand("T1", swerve.driveToPose(FieldConstants.BLUE_TOP_TOP_LEFT));
// webServer.registerCommand("T2", swerve.driveToPose(FieldConstants.BLUE_TOP_TOP_RIGHT));

// webServer.registerCommand("TL1", swerve.driveToPose(FieldConstants.BLUE_TOP_LEFT_BOTTOM));
// webServer.registerCommand("TL2", swerve.driveToPose(FieldConstants.BLUE_TOP_LEFT_TOP));
}

private void configurePathPlannerCommands() {

NamedCommands.registerCommand("Intake_Algae", new PrintCommand("Intake Algae"));
}

private void configureButtonBindings() {

// Move elevator up/down for coral scoring
operatorController.povUp().onTrue(new CMD_ElevatorCoral(superstructure, true));

operatorController.povDown().onTrue(new CMD_ElevatorCoral(superstructure, false));

// Lower elevator to stowed position
operatorController.b().onTrue(new CMD_Superstructure(superstructure, SuperstructureState.State.STOWED));

// Pick up coral from the Source
operatorController.a().onTrue(new CMD_IntakeCoral(superstructure));

// Climb
operatorController
.rightBumper()
.onTrue(new CMD_Elevator(elevator, led, SuperstructureState.State.CLIMB));

// Aim at 0,0 for testing
// driverController.a().onTrue(new CMD_AimAtPose(swerve, new Pose2d(), 0.1));

// Aim at tag 16 for testing
// driverController.b().onTrue(new CMD_AimAtAprilTag(swerve, 16, 0.1));

/*SequentialCommandGroup
driverController.a().onTrue(intake.setState(SUB_Intake.State.ALGAE_GROUND));
driverController.a().toggleOnFalse(intake.setState(SUB_Intake.State.STOWED));
}
*/

/*
driverController
.a()
.onTrue(
new ParallelCommandGroup(
intake.setState(SUB_Intake.State.ALGAE_GROUND),
swerve.driveToPose(FieldConstants.BLUE_TOP_TOP_LEFT)));
*/
}

public Command getAutonomousCommand() {
return swerve.getAutonomousCommand("R_3L4");
}
}
// Controller Configuration
private CommandXboxController driverController;
private CommandXboxController operatorController;
private InputConstants globalInputMap;

// Subsystems
private SUB_Swerve swerve;
private SUB_Intake intake;
private SUB_Vision vision;
private SUB_Elevator elevator;
private SUB_Superstructure superstructure;
private SUB_Led led;
private WebServer webServer;

public RobotContainer() {
// Initialize Controllers
initializeControllers();

// Initialize Subsystems
initializeSubsystems();

// Configure Robot Functionality
configureDefaultCommands();
configureWebserverCommands();
configurePathPlannerCommands();
configureButtonBindings();
}

private void initializeControllers() {
driverController = new CommandXboxController(0);
operatorController = new CommandXboxController(1);
globalInputMap = InputConstants.TX16S_MAIN;
}

private void initializeSubsystems() {
webServer = new WebServer();
vision = new SUB_Vision(Robot.isSimulation() ? new IO_VisionSim() : new IO_VisionReal());
swerve = new SUB_Swerve(new IO_SwerveReal(new File(Filesystem.getDeployDirectory(), "swerve")));
intake = new SUB_Intake(new IO_IntakeReal());
elevator = new SUB_Elevator(new IO_ElevatorReal());
led = new SUB_Led();
superstructure = new SUB_Superstructure(intake, elevator, led);
}

private void configureDefaultCommands() {
swerve.setDefaultCommand(new CMD_Drive(swerve, driverController, globalInputMap));
}

private void configureWebserverCommands() {
webServer.registerCommand("T1", swerve.driveToPose(FieldConstants.BLUE_TOP_TOP_LEFT));
webServer.registerCommand("T2", swerve.driveToPose(FieldConstants.BLUE_TOP_TOP_RIGHT));
webServer.registerCommand("TL1", swerve.driveToPose(FieldConstants.BLUE_TOP_LEFT_BOTTOM));
webServer.registerCommand("TL2", swerve.driveToPose(FieldConstants.BLUE_TOP_LEFT_TOP));
}

private void configurePathPlannerCommands() {
NamedCommands.registerCommand("Intake_Algae", new PrintCommand("Intake Algae"));
}

private void configureButtonBindings() {


// Coral Controls
operatorController.povUp().onTrue(new CMD_ElevatorCoral(superstructure, true)); // DPAD-UP - Coral up
operatorController.povDown().onTrue(new CMD_ElevatorCoral(superstructure, false)); // DPAD-DOWN - Coral down
operatorController.a().onTrue(new CMD_IntakeCoral(superstructure)); // A - Intake coral


// Superstructure Controls
operatorController.b().onTrue(new CMD_Superstructure(superstructure, SuperstructureState.State.IDLE));


// Climbing
operatorController.rightBumper().onTrue(new CMD_Elevator(elevator, led, SuperstructureState.State.CLIMB));
}

public Command getAutonomousCommand() {
return swerve.getAutonomousCommand("R_3L4");
}
}
8 changes: 4 additions & 4 deletions src/main/java/frc/robot/commands/algae/CMD_ElevatorAlgae.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,8 @@ private SuperstructureState.State determineNewState(
// Elevator Up Logic for Algae states
switch (currentState) {
case ALGAE_GROUND:
return SuperstructureState.State.ALGAE_REMOVAL;
case ALGAE_REMOVAL:
return SuperstructureState.State.ALGAE_L2;
case ALGAE_L2:
return SuperstructureState.State.ALGAE_PROCESSOR;
case ALGAE_PROCESSOR:
return SuperstructureState.State.ALGAE_BARGE;
Expand All @@ -41,8 +41,8 @@ private SuperstructureState.State determineNewState(
case ALGAE_BARGE:
return SuperstructureState.State.ALGAE_PROCESSOR;
case ALGAE_PROCESSOR:
return SuperstructureState.State.ALGAE_REMOVAL;
case ALGAE_REMOVAL:
return SuperstructureState.State.ALGAE_L2;
case ALGAE_L2:
return SuperstructureState.State.ALGAE_GROUND;
default:
return currentState;
Expand Down
18 changes: 9 additions & 9 deletions src/main/java/frc/robot/commands/algae/CMD_IntakeAlgae.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,13 +13,13 @@
import frc.robot.superstructure.SuperstructureState;

public class CMD_IntakeAlgae extends SequentialCommandGroup {
public CMD_IntakeAlgae(SUB_Superstructure superstructure) {
addCommands(
// Move to Algae Ground state
new CMD_Superstructure(superstructure, SuperstructureState.State.ALGAE_GROUND),
public CMD_IntakeAlgae(SUB_Superstructure superstructure) {
addCommands(
// Move to Algae Ground state
new CMD_Superstructure(superstructure, SuperstructureState.State.ALGAE_GROUND),

// TODO: Auto idle intake after algae is NOT sensed by the sensor and current limit is reached
new CMD_Superstructure(superstructure, SuperstructureState.State.STOWED)
);
}
}
// TODO: Auto idle intake after algae is NOT sensed by the sensor and current limit is
// reached
new CMD_Superstructure(superstructure, SuperstructureState.State.IDLE));
}
}
17 changes: 8 additions & 9 deletions src/main/java/frc/robot/commands/coral/CMD_IntakeCoral.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,13 +13,12 @@
import frc.robot.superstructure.SuperstructureState;

public class CMD_IntakeCoral extends SequentialCommandGroup {
public CMD_IntakeCoral(SUB_Superstructure superstructure) {
addCommands(
// Move to Coral Station state
new CMD_Superstructure(superstructure, SuperstructureState.State.CORAL_STATION),
public CMD_IntakeCoral(SUB_Superstructure superstructure) {
addCommands(
// Move to Coral Station state
new CMD_Superstructure(superstructure, SuperstructureState.State.CORAL_STATION),

// TODO: Auto idle intake after coral is sensed by the sensor
new CMD_Superstructure(superstructure, SuperstructureState.State.STOWED_CORAL)
);
}
}
// TODO: Auto idle intake after coral is sensed by the sensor
new CMD_Superstructure(superstructure, SuperstructureState.State.IDLE));
}
}
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/commands/generic/CMD_Elevator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,8 @@

import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.elevator.SUB_Elevator;
import frc.robot.misc.SUB_Led;
import frc.robot.superstructure.SuperstructureState;
import frc.robot.util.SUB_Led;

public class CMD_Elevator extends Command {
private final SUB_Elevator elevator;
Expand Down
28 changes: 28 additions & 0 deletions src/main/java/frc/robot/commands/generic/CMD_Intake.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,28 @@
// Copyright (c) 2024 - 2025 : FRC 2106 : The Junkyard Dogs
// https://www.team2106.org

// Use of this source code is governed by an MIT-style
// license that can be found in the LICENSE file at
// the root directory of this project.

package frc.robot.commands.generic;

import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.commands.generic.CMD_Superstructure;
import frc.robot.superstructure.SUB_Superstructure;
import frc.robot.superstructure.SuperstructureState;

public class CMD_Intake extends SequentialCommandGroup {
public CMD_Intake(SUB_Superstructure superstructure) {

//boolean isAtAnyCoralState = superstructure.getCurrentSuperstructureState();


addCommands(
// Move to Coral Station state
new CMD_Superstructure(superstructure, SuperstructureState.State.CORAL_STATION),

// TODO: Auto idle intake after coral is sensed by the sensor
new CMD_Superstructure(superstructure, SuperstructureState.State.IDLE));
}
}
25 changes: 25 additions & 0 deletions src/main/java/frc/robot/commands/generic/CMD_IntakeCoral.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
// Copyright (c) 2024 - 2025 : FRC 2106 : The Junkyard Dogs
// https://www.team2106.org

// Use of this source code is governed by an MIT-style
// license that can be found in the LICENSE file at
// the root directory of this project.

package frc.robot.commands.generic;

import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.commands.generic.CMD_Superstructure;
import frc.robot.superstructure.SUB_Superstructure;
import frc.robot.superstructure.SuperstructureState;

public class CMD_IntakeCoral extends SequentialCommandGroup {
public CMD_IntakeCoral(SUB_Superstructure superstructure) {

addCommands(
// Move to Coral Station state
new CMD_Superstructure(superstructure, SuperstructureState.State.CORAL_STATION),

// TODO: Auto idle intake after coral is sensed by the sensor
new CMD_Superstructure(superstructure, SuperstructureState.State.IDLE));
}
}
Loading

0 comments on commit 669ff27

Please sign in to comment.