Skip to content

Commit

Permalink
Split subsystems (#6)
Browse files Browse the repository at this point in the history 
* first split into many subsystems

* shooter spasms

* tune sim pid (why is that a thing)

* make feed use intake motor as well

* remove copyright comments in many files

* move registerCommands to RobotContainer

* two joysticks, disable vision

* shuffleboard entry

* offset front left by 180 due to bad zeroing
  • Loading branch information
@AvidCoder27
AvidCoder27 authored Sep 11, 2024
1 parent c22bfc0 commit 5b44c47
Show file tree
Hide file tree
Showing 31 changed files with 767 additions and 734 deletions.
27 changes: 3 additions & 24 deletions src/main/java/frc/team5115/Constants.java
View file
Original file line number Diff line number Diff line change
@@ -1,16 +1,3 @@
// Copyright 2021-2024 FRC 6328
// http://github.com/Mechanical-Advantage
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// version 3 as published by the Free Software Foundation or
// available in the root directory of this project.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.

package frc.team5115;

import edu.wpi.first.math.geometry.Rotation2d;
Expand All @@ -20,14 +7,6 @@
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.RobotBase;

/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
* constants. This class should not be used for any other purpose. All constants should be declared
* globally (i.e. public static). Do not put anything functional in this class.
*
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {
private static final boolean isReplay = false;
public static final Mode currentMode =
Expand All @@ -48,9 +27,9 @@ public static enum Mode {
public static final byte SNOWBLOWER_MOTOR_ID = 21;
public static final byte ARM_LEFT_MOTOR_ID = 3;
public static final byte ARM_RIGHT_MOTOR_ID = 33;
public static final byte SHOOTER_RIGHT_MOTOR_ID = 27;
public static final byte SHOOTER_LEFT_MOTOR_ID = 20;
public static final byte SHOTER_AUX_MOTOR_ID = 35;
public static final byte FEEDER_RIGHT_MOTOR_ID = 27;
public static final byte FEEDER_LEFT_MOTOR_ID = 20;
public static final byte SHOOTER_MOTOR_ID = 35;
public static final byte CLIMBER_LEFT_MOTOR_ID = 30;
public static final byte CLIMBER_RIGHT_MOTOR_ID = 31;

Expand Down
13 changes: 0 additions & 13 deletions src/main/java/frc/team5115/Main.java
View file
Original file line number Diff line number Diff line change
@@ -1,16 +1,3 @@
// Copyright 2021-2024 FRC 6328
// http://github.com/Mechanical-Advantage
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// version 3 as published by the Free Software Foundation or
// available in the root directory of this project.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.

package frc.team5115;

import edu.wpi.first.wpilibj.RobotBase;
Expand Down
14 changes: 1 addition & 13 deletions src/main/java/frc/team5115/Robot.java
View file
Original file line number Diff line number Diff line change
@@ -1,16 +1,3 @@
// Copyright 2021-2024 FRC 6328
// http://github.com/Mechanical-Advantage
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// version 3 as published by the Free Software Foundation or
// available in the root directory of this project.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.

package frc.team5115;

import edu.wpi.first.wpilibj2.command.Command;
Expand Down Expand Up @@ -104,6 +91,7 @@ public void robotPeriodic() {
// This must be called from the robot's periodic block in order for anything in
// the Command-based framework to work.
CommandScheduler.getInstance().run();
robotContainer.robotPeriodic();
}

/** This function is called once when the robot is disabled. */
Expand Down
150 changes: 105 additions & 45 deletions src/main/java/frc/team5115/RobotContainer.java
View file
Original file line number Diff line number Diff line change
@@ -1,28 +1,20 @@
// Copyright 2021-2024 FRC 6328
// http://github.com/Mechanical-Advantage
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// version 3 as published by the Free Software Foundation or
// available in the root directory of this project.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.

package frc.team5115;

import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.team5115.commands.AutoCommands;
import frc.team5115.commands.DriveCommands;
import frc.team5115.subsystems.amper.Amper;
import frc.team5115.subsystems.amper.AmperIO;
import frc.team5115.subsystems.amper.AmperIOSim;
import frc.team5115.subsystems.amper.AmperIOSparkMax;
import frc.team5115.subsystems.arm.Arm;
import frc.team5115.subsystems.arm.ArmIO;
import frc.team5115.subsystems.arm.ArmIOSim;
Expand All @@ -33,6 +25,14 @@
import frc.team5115.subsystems.drive.ModuleIO;
import frc.team5115.subsystems.drive.ModuleIOSim;
import frc.team5115.subsystems.drive.ModuleIOSparkMax;
import frc.team5115.subsystems.feeder.Feeder;
import frc.team5115.subsystems.feeder.FeederIO;
import frc.team5115.subsystems.feeder.FeederIOSim;
import frc.team5115.subsystems.feeder.FeederIOSparkMax;
import frc.team5115.subsystems.intake.Intake;
import frc.team5115.subsystems.intake.IntakeIO;
import frc.team5115.subsystems.intake.IntakeIOSim;
import frc.team5115.subsystems.intake.IntakeIOSparkMax;
import frc.team5115.subsystems.shooter.Shooter;
import frc.team5115.subsystems.shooter.ShooterIO;
import frc.team5115.subsystems.shooter.ShooterIOSim;
Expand All @@ -51,59 +51,80 @@ public class RobotContainer {
private final GyroIO gyro;
private final Drivetrain drivetrain;
private final PhotonVision vision;
private final Shooter shooter;
private final Arm arm;
private final Amper amper;
private final Intake intake;
private final Feeder feeder;
private final Shooter shooter;

// Controller
private final CommandXboxController joyDrive = new CommandXboxController(0);
private final CommandXboxController joyManip = new CommandXboxController(1);

// Dashboard inputs
private final LoggedDashboardChooser<Command> autoChooser;

// Shuffleboard
private final GenericEntry noteDetectedEntry;

/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
switch (Constants.currentMode) {
case REAL:
// Real robot, instantiate hardware IO implementations
gyro = new GyroIONavx();
shooter = new Shooter(new ShooterIOSparkMax());
arm = new Arm(new ArmIOSparkMax());
amper = new Amper(new AmperIOSparkMax());
intake = new Intake(new IntakeIOSparkMax());
feeder = new Feeder(new FeederIOSparkMax());
shooter = new Shooter(new ShooterIOSparkMax());
drivetrain =
new Drivetrain(
gyro,
new ModuleIOSparkMax(0),
new ModuleIOSparkMax(1),
new ModuleIOSparkMax(2),
new ModuleIOSparkMax(3));
vision = new PhotonVision(drivetrain);
// vision = new PhotonVision(drivetrain);
vision = null;
noteDetectedEntry =
Shuffleboard.getTab("SmartDashboard").add("Has note?", false).getEntry();
break;

case SIM:
// Sim robot, instantiate physics sim IO implementations
gyro = new GyroIO() {};
shooter = new Shooter(new ShooterIOSim());
arm = new Arm(new ArmIOSim());
amper = new Amper(new AmperIOSim());
intake = new Intake(new IntakeIOSim());
feeder = new Feeder(new FeederIOSim());
shooter = new Shooter(new ShooterIOSim());
drivetrain =
new Drivetrain(
gyro, new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim());
vision = null;
noteDetectedEntry = null;
break;

default:
// Replayed robot, disable IO implementations
gyro = new GyroIO() {};
shooter = new Shooter(new ShooterIO() {});
arm = new Arm(new ArmIO() {});
amper = new Amper(new AmperIO() {});
intake = new Intake(new IntakeIO() {});
feeder = new Feeder(new FeederIO() {});
shooter = new Shooter(new ShooterIO() {});
drivetrain =
new Drivetrain(
gyro, new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {});
vision = null;
noteDetectedEntry = null;
break;
}

// Register auto commands for pathplanner
// PhotonVision is passed in here to prevent warnings, i.e. "unused variable: vision"
AutoCommands.registerCommands(shooter, arm, drivetrain, vision);
registerCommands(drivetrain, vision, arm, amper, intake, feeder, shooter);

// Set up auto routines
autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
Expand All @@ -117,33 +138,26 @@ public RobotContainer() {
// drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
// autoChooser.addOption(
// "Drive SysId (Dynamic Forward)",
// drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward));
// drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward));
// autoChooser.addOption(
// "Drive SysId (Dynamic Reverse)",
// drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse));

// drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse));
// autoChooser.addOption(
// "Shooter Aux SysId (Quasistatic Forward)",
// "Shooter SysId (Quasistatic Forward)",
// shooter.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
// autoChooser.addOption(
// "Shooter Aux SysId (Quasistatic Reverse)",
// "Shooter SysId (Quasistatic Reverse)",
// shooter.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
// autoChooser.addOption(
// "Shooter Aux SysId (Dynamic Forward)",
// "Shooter SysId (Dynamic Forward)",
// shooter.sysIdDynamic(SysIdRoutine.Direction.kForward));
// autoChooser.addOption(
// "Shooter Aux SysId (Dynamic Reverse)",
// "Shooter SysId (Dynamic Reverse)",
// shooter.sysIdDynamic(SysIdRoutine.Direction.kReverse));
// Configure the button bindings

configureButtonBindings();
}

/**
* Use this method to define your button->command mappings. Buttons can be created by
* instantiating a {@link GenericHID} or one of its subclasses ({@link
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
* edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
drivetrain.setDefaultCommand(
DriveCommands.joystickDrive(
Expand All @@ -155,17 +169,63 @@ private void configureButtonBindings() {
// joyDrive.x().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain));
joyDrive.start().onTrue(resetFieldOrientation());

joyDrive.back().onTrue(shooter.vomit()).onFalse(shooter.stop());
joyDrive.a().onTrue(DriveCommands.intakeUntilNote(shooter, arm));
joyDrive.y().onTrue(DriveCommands.stowArm(shooter, arm));
joyDrive
joyManip
.back()
.onTrue(DriveCommands.vomit(intake, feeder, shooter))
.onFalse(DriveCommands.forceStop(intake, feeder, shooter));

joyManip.a().onTrue(DriveCommands.intakeUntilNote(arm, intake, feeder));

joyManip
.y()
.onTrue(
Commands.parallel(arm.stow(), shooter.stop(), feeder.stop(), intake.stop())
.withInterruptBehavior(InterruptionBehavior.kCancelSelf));

joyManip
.b()
.onTrue(DriveCommands.prepareShoot(shooter, arm, 15, true))
.onFalse(DriveCommands.triggerShoot(shooter));
joyDrive
.onTrue(DriveCommands.prepareShoot(arm, intake, feeder, shooter, 15, 5000))
.onFalse(
DriveCommands.feed(intake, feeder)
.andThen(shooter.stop())
.withInterruptBehavior(InterruptionBehavior.kCancelIncoming));

joyManip
.x()
.onTrue(DriveCommands.prepareAmp(shooter, arm))
.onFalse(DriveCommands.triggerAmp(shooter, arm));
.onTrue(DriveCommands.prepareAmp(arm, amper, intake, feeder))
.onFalse(DriveCommands.triggerAmp(arm, amper, intake, feeder));
}

public void robotPeriodic() {
if (noteDetectedEntry != null) {
noteDetectedEntry.setBoolean(feeder.noteDetected());
}
}

/**
* Registers commands for pathplanner to use in autos
*
* @param shooter the shooter subsystem
* @param arm the arm subsystem
* @param drivetrain the drivetrain subsytem (not currently used)
* @param photonVision the photonvision subsystem (not currently used)
*/
public static void registerCommands(
Drivetrain drivetrain,
PhotonVision vision,
Arm arm,
Amper amper,
Intake intake,
Feeder feeder,
Shooter shooter) {
NamedCommands.registerCommand(
"InitialShoot",
DriveCommands.prepareShoot(arm, intake, feeder, shooter, 15, 5000)
.andThen(DriveCommands.feed(intake, feeder)));
NamedCommands.registerCommand("Intake", DriveCommands.intakeUntilNote(arm, intake, feeder));
NamedCommands.registerCommand("Shoot", DriveCommands.feed(intake, feeder));
NamedCommands.registerCommand(
"PrepareClose", DriveCommands.prepareShoot(arm, intake, feeder, shooter, 15, 5000));
}

/**
Expand Down
31 changes: 0 additions & 31 deletions src/main/java/frc/team5115/commands/AutoCommands.java
View file

This file was deleted.

Loading

0 comments on commit 5b44c47

Please sign in to comment.