Skip to content
This repository has been archived by the owner on Jan 10, 2025. It is now read-only.

Commit

Permalink
Refactor main robot class to use CommandRobot (#85)
Browse files Browse the repository at this point in the history
  • Loading branch information
@spacey-sooty
spacey-sooty authored Jul 22, 2024
1 parent be24647 commit 7b00f57
Show file tree
Hide file tree
Showing 2 changed files with 121 additions and 106 deletions.
81 changes: 81 additions & 0 deletions src/main/java/edu/wpi/first/wpilibj2/command/CommandRobot.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,81 @@
// Copyright (c) 2024 CurtinFRC
// Open Source Software, you can modify it according to the terms
// of the MIT License at the root of this project

package edu.wpi.first.wpilibj2.command;

import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;

/**
* CommandRobot is a wrapper over TimedRobot designed to work well with Command based programming.
*
* <p>The CommandRobot class is intended to be subclassed by a user creating a robot program.
*/
public class CommandRobot extends TimedRobot {
/** The CommandScheduler instance. */
protected CommandScheduler m_scheduler = CommandScheduler.getInstance();

/** The autonomous chooser. */
protected SendableChooser<Command> m_autoChooser = new SendableChooser<>();

private Command m_autonomousCommand;

/** Constructor for CommandRobot. */
protected CommandRobot() {
this(kDefaultPeriod);
}

/**
* Constructor for CommandRobot.
*
* @param period Period in seconds.
*/
protected CommandRobot(double period) {
super(period);
addPeriodic(() -> m_scheduler.run(), kDefaultPeriod);
m_autoChooser.setDefaultOption("No Auto Configured", Commands.print("No autos configured."));
}

@Override
public final void robotInit() {}

@Override
public final void robotPeriodic() {}

@Override
public void autonomousInit() {
m_autonomousCommand = m_autoChooser.getSelected();
if (m_autonomousCommand != null) {
m_scheduler.schedule(m_autonomousCommand);
}
}

@Override
public final void autonomousPeriodic() {}

@Override
public void autonomousExit() {}

@Override
public final void teleopInit() {
if (m_autonomousCommand != null) {
m_scheduler.cancel(m_autonomousCommand);
}
}

@Override
public final void teleopPeriodic() {}

@Override
public final void teleopExit() {}

@Override
public final void disabledInit() {}

@Override
public final void disabledPeriodic() {}

@Override
public final void disabledExit() {}
}
146 changes: 40 additions & 106 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,17 +4,13 @@

package frc.robot;

import com.choreo.lib.Auto;
import com.ctre.phoenix6.SignalLogger;
import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
import edu.wpi.first.wpilibj.DataLogManager;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.CommandRobot;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.autos.Centre2541;
import frc.robot.autos.Centre26541;
Expand All @@ -27,7 +23,7 @@
import frc.robot.subsystems.Shooter;
import java.util.HashMap;

public class Robot extends TimedRobot {
public class Robot extends CommandRobot {
private final CommandXboxController m_driver = new CommandXboxController(Constants.driverport);

private final Arm m_arm = new Arm();
Expand All @@ -36,33 +32,13 @@ public class Robot extends TimedRobot {
private final Intake m_intake = new Intake();
private static final CommandSwerveDrivetrain m_drivetrain = TunerConstants.DriveTrain;

private final SendableChooser<Auto> m_chooser = new SendableChooser<>();
private Command m_autonomousCommand;

private final SwerveRequest.FieldCentric drive =
private final SwerveRequest.FieldCentric m_drive =
new SwerveRequest.FieldCentric()
.withDeadband(Constants.DrivebaseMaxSpeed * 0.1)
.withRotationalDeadband(Constants.DrivebaseMaxAngularRate * 0.1)
.withDriveRequestType(DriveRequestType.OpenLoopVoltage);
private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake();
private final Telemetry logger = new Telemetry();

private void configureBindings() {
m_drivetrain.setDefaultCommand(
m_drivetrain.applyRequest(
() ->
drive
.withVelocityX(
Utils.deadzone(-m_driver.getLeftY() * Constants.DrivebaseMaxSpeed))
.withVelocityY(
Utils.deadzone(-m_driver.getLeftX() * Constants.DrivebaseMaxSpeed))
.withRotationalRate(
Utils.deadzone(
-m_driver.getRightX() * Constants.DrivebaseMaxAngularRate))));

m_driver.a().whileTrue(m_drivetrain.applyRequest(() -> brake));
m_driver.x().onTrue(m_drivetrain.runOnce(() -> m_drivetrain.seedFieldRelative()));
}
private final SwerveRequest.SwerveDriveBrake m_brake = new SwerveRequest.SwerveDriveBrake();
private final Telemetry m_logger = new Telemetry();

public Robot() {
HashMap<Integer, String> aliases = new HashMap<>();
Expand All @@ -76,84 +52,42 @@ public Robot() {
DriverStation.startDataLog(DataLogManager.getLog());
SignalLogger.setPath(DataLogManager.getLogDir());
SignalLogger.start();
m_drivetrain.registerTelemetry(m_logger::telemeterize);

m_scheduler.registerSubsystem(m_arm);
m_scheduler.registerSubsystem(m_shooter);
m_scheduler.registerSubsystem(m_climber);
m_scheduler.registerSubsystem(m_intake);

m_autoChooser.addOption(
"Centre2_5_4_1 Blue", new Centre2541(m_drivetrain, false).followTrajectory());
m_autoChooser.addOption(
"Centre2_5_4_1 Red", new Centre2541(m_drivetrain, true).followTrajectory());
m_autoChooser.addOption(
"Centre2_6_5_4_1 Red", new Centre26541(m_drivetrain, true).followTrajectory());
m_autoChooser.addOption(
"Centre2_6_5_4_1 Blue", new Centre26541(m_drivetrain, false).followTrajectory());
m_autoChooser.addOption(
"WompWompKieran Blue", new WompWompKieran(m_drivetrain, false).followTrajectory());
m_autoChooser.addOption(
"WompWompKieran Red", new WompWompKieran(m_drivetrain, true).followTrajectory());
m_autoChooser.setDefaultOption(
"WompWompKieran Blue", new WompWompKieran(m_drivetrain, false).followTrajectory());
SmartDashboard.putData(m_autoChooser);

m_drivetrain.registerTelemetry(logger::telemeterize);
m_drivetrain.addMusic("abba", "bad-piggies");
m_drivetrain.selectTrack("bad-piggies");
m_drivetrain.getOrchestra().play();

CommandScheduler.getInstance().registerSubsystem(m_arm);
CommandScheduler.getInstance().registerSubsystem(m_shooter);
CommandScheduler.getInstance().registerSubsystem(m_climber);
CommandScheduler.getInstance().registerSubsystem(m_intake);

m_chooser.addOption("Centre2_5_4_1 Blue", new Centre2541(m_drivetrain, false));
m_chooser.addOption("Centre2_5_4_1 Red", new Centre2541(m_drivetrain, true));
m_chooser.addOption("Centre2_6_5_4_1 Red", new Centre26541(m_drivetrain, true));
m_chooser.addOption("Centre2_6_5_4_1 Blue", new Centre26541(m_drivetrain, false));
m_chooser.addOption("WompWompKieran Blue", new WompWompKieran(m_drivetrain, false));
m_chooser.addOption("WompWompKieran Red", new WompWompKieran(m_drivetrain, true));
m_chooser.setDefaultOption("WompWompKieran Blue", new WompWompKieran(m_drivetrain, false));
SmartDashboard.putData(m_chooser);

configureBindings();
}

@Override
public void robotPeriodic() {
CommandScheduler.getInstance().run();
}

@Override
public void disabledInit() {}

@Override
public void disabledPeriodic() {
m_drivetrain.getOrchestra().play();
}

@Override
public void disabledExit() {}

@Override
public void autonomousInit() {
m_drivetrain.getOrchestra().stop();
m_autonomousCommand = m_chooser.getSelected().followTrajectory();

if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
}
}

@Override
public void autonomousPeriodic() {}

@Override
public void autonomousExit() {}

@Override
public void teleopInit() {
m_drivetrain.getOrchestra().stop();

if (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
}
}

@Override
public void teleopPeriodic() {}

@Override
public void teleopExit() {}
m_drivetrain.setDefaultCommand(
m_drivetrain.applyRequest(
() ->
m_drive
.withVelocityX(
Utils.deadzone(-m_driver.getLeftY() * Constants.DrivebaseMaxSpeed))
.withVelocityY(
Utils.deadzone(-m_driver.getLeftX() * Constants.DrivebaseMaxSpeed))
.withRotationalRate(
Utils.deadzone(
-m_driver.getRightX() * Constants.DrivebaseMaxAngularRate))));

@Override
public void testInit() {
CommandScheduler.getInstance().cancelAll();
m_driver.a().whileTrue(m_drivetrain.applyRequest(() -> m_brake));
m_driver.x().onTrue(m_drivetrain.runOnce(() -> m_drivetrain.seedFieldRelative()));
}

@Override
public void testPeriodic() {}

@Override
public void testExit() {}
}

0 comments on commit 7b00f57

Please sign in to comment.