From adcc07364debfed36dbf49810714de15f9fbb3ec Mon Sep 17 00:00:00 2001 From: PJ Reiniger Date: Sat, 4 Nov 2023 23:23:21 -0400 Subject: [PATCH] Make it more similar to GOS version --- src/main/java/frc/robot/Robot.java | 28 +------ src/main/java/frc/robot/RobotContainer.java | 23 +++++- .../frc/robot/commands/AutoAimCommand.java | 38 +++++---- .../frc/robot/commands/ClimbAutoCommand.java | 2 +- .../frc/robot/commands/ClimbPIDComand.java | 8 +- .../frc/robot/commands/ClimberCommand.java | 2 +- .../java/frc/robot/commands/DriveCommand.java | 4 +- .../frc/robot/commands/HopperCommand.java | 2 +- .../commands/KickIfShootSetRPMCommand.java | 7 +- .../KickIfShooterDistanceGoBrrCommand.java | 19 +++-- .../commands/RunClimberToBottomCommand.java | 2 +- .../frc/robot/commands/RunIntakeCommand.java | 9 +-- .../commands/SetShooterSpeedCommand.java | 2 +- .../commands/ShootFromDistanceCommand.java | 10 +-- .../frc/robot/commands/ShooterPIDCommand.java | 6 +- .../frc/robot/commands/TowerDownCommand.java | 2 +- .../robot/commands/TowerKickerCommand.java | 7 +- .../frc/robot/commands/TowerUpCommand.java | 10 +-- .../frc/robot/commands/TrajectoryCommand.java | 4 +- .../robot/subsystems/ClimberSubsystem.java | 10 +-- .../robot/subsystems/DrivetrainSubsystem.java | 81 ++++++++++++------- .../frc/robot/subsystems/HopperSubsystem.java | 2 +- .../frc/robot/subsystems/ShooterSubsytem.java | 69 +++++++++++----- .../frc/robot/subsystems/TowerSubsystem.java | 2 +- vendordeps/SnobotSim.json | 22 +++++ 25 files changed, 213 insertions(+), 158 deletions(-) create mode 100644 vendordeps/SnobotSim.json diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1e32f4a..73c1d5c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -14,7 +14,6 @@ * the package after creating this project, you must also update the build.gradle file in the * project. */ -@SuppressWarnings("PMD.TooManyMethods") public class Robot extends TimedRobot { private Command m_autonomousCommand; @@ -26,7 +25,7 @@ public class Robot extends TimedRobot { */ @Override public void robotInit() { - // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); } @@ -40,9 +39,9 @@ public void robotInit() { */ @Override public void robotPeriodic() { - // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled // commands, running already-scheduled commands, removing finished or interrupted commands, - // and running subsystem periodic() methods. This must be called from the robot's periodic + // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); } @@ -101,25 +100,4 @@ public void testInit() { // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); } - - /** - * This function is called periodically during test mode. - */ - @Override - public void testPeriodic() { - } - - /** - * This function is called once when the robot is first started up. - */ - @Override - public void simulationInit() { - } - - /** - * This function is called periodically whilst in simulation. - */ - @Override - public void simulationPeriodic() { - } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 15bdee9..5fc2236 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,15 +4,30 @@ package frc.robot; +import frc.robot.commands.ClimberCommand; +import frc.robot.commands.DriveCommand; +import frc.robot.commands.HopperCommand; +import frc.robot.commands.KickIfShootSetRPMCommand; +import frc.robot.commands.RunIntakeCommand; +import frc.robot.commands.SetShooterSpeedCommand; +import frc.robot.commands.StopIntakeCommand; +import frc.robot.commands.ShootFromDistanceCommand; +import frc.robot.commands.TowerDownCommand; +import frc.robot.commands.TowerUpCommand; +import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.DrivetrainSubsystem; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.LimelightSubsystem; +import frc.robot.subsystems.ShooterSubsytem; +import frc.robot.subsystems.TowerSubsystem; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.autos.AutonomousFactory; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import frc.robot.autos.AutonomousFactory; -import frc.robot.commands.*; -import frc.robot.subsystems.*; /** * This class is where the bulk of the robot should be declared. Since Command-based is a diff --git a/src/main/java/frc/robot/commands/AutoAimCommand.java b/src/main/java/frc/robot/commands/AutoAimCommand.java index 2e151a9..9a36166 100644 --- a/src/main/java/frc/robot/commands/AutoAimCommand.java +++ b/src/main/java/frc/robot/commands/AutoAimCommand.java @@ -6,14 +6,13 @@ import com.gos.lib.properties.PidProperty; import com.gos.lib.properties.WpiProfiledPidPropertyBuilder; - +import frc.robot.Constants; +import frc.robot.subsystems.DrivetrainSubsystem; +import frc.robot.subsystems.LimelightSubsystem; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.Constants; -import frc.robot.subsystems.DrivetrainSubsystem; -import frc.robot.subsystems.LimelightSubsystem; /** * An example command that uses an example subsystem. @@ -22,7 +21,7 @@ public class AutoAimCommand extends CommandBase { private final DrivetrainSubsystem m_driveTrainSubsystem; private final LimelightSubsystem m_limelightSubsystem; private final ProfiledPIDController m_controller; - private final PidProperty m_pidProperty; + private final PidProperty m_pidProperties; /** * Creates a new AutoAimCommand. @@ -30,23 +29,21 @@ public class AutoAimCommand extends CommandBase { * @param subsystem The subsystem used by this command. */ public AutoAimCommand( - DrivetrainSubsystem driveTrainSubsystem, LimelightSubsystem limelightSubsystem) { + DrivetrainSubsystem driveTrainSubsystem, LimelightSubsystem limelightSubsystem) { m_driveTrainSubsystem = driveTrainSubsystem; m_limelightSubsystem = limelightSubsystem; m_controller = - new ProfiledPIDController( - 0, - 0, - 0, - new TrapezoidProfile.Constraints( - Constants.MAX_TURN_VELOCITY, Constants.MAX_TURN_ACCELERATION)); - m_pidProperty = new WpiProfiledPidPropertyBuilder("AutoAim", false, m_controller) - .addP(0) - .addI(0) - .addD(0) - .addMaxVelocity(Constants.MAX_TURN_VELOCITY) - .addMaxAcceleration(Constants.MAX_TURN_ACCELERATION) - .build(); + new ProfiledPIDController( + 0, + 0, + 0, + new TrapezoidProfile.Constraints( + Constants.MAX_TURN_VELOCITY, Constants.MAX_TURN_ACCELERATION)); + m_pidProperties = new WpiProfiledPidPropertyBuilder("AutoAim", false, m_controller) + .addP(0) + .addI(0) + .addD(0) + .build(); m_controller.setTolerance(5); // Use addRequirements() here to declare subsystem dependencies. addRequirements(driveTrainSubsystem); @@ -61,7 +58,8 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_pidProperty.updateIfChanged(); + m_pidProperties.updateIfChanged(); + double turnSpeed = m_controller.calculate(m_limelightSubsystem.limelightAngle()); ChassisSpeeds chassisSpeed = new ChassisSpeeds(0, 0, turnSpeed); m_driveTrainSubsystem.applyChassisSpeed(chassisSpeed); diff --git a/src/main/java/frc/robot/commands/ClimbAutoCommand.java b/src/main/java/frc/robot/commands/ClimbAutoCommand.java index f29c15e..4d3eecb 100644 --- a/src/main/java/frc/robot/commands/ClimbAutoCommand.java +++ b/src/main/java/frc/robot/commands/ClimbAutoCommand.java @@ -1,7 +1,7 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.ClimberSubsystem; +import edu.wpi.first.wpilibj2.command.CommandBase; /** * An example command that uses an example subsystem. diff --git a/src/main/java/frc/robot/commands/ClimbPIDComand.java b/src/main/java/frc/robot/commands/ClimbPIDComand.java index 7eae37e..1113322 100644 --- a/src/main/java/frc/robot/commands/ClimbPIDComand.java +++ b/src/main/java/frc/robot/commands/ClimbPIDComand.java @@ -1,7 +1,7 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.ClimberSubsystem; +import edu.wpi.first.wpilibj2.command.CommandBase; public class ClimbPIDComand extends CommandBase { private final ClimberSubsystem m_climberSubsystem; @@ -24,13 +24,13 @@ public void execute() { @Override public boolean isFinished() { - return Math.abs(m_climberSubsystem.getPIDPosition()) < 100 - || (m_climberSubsystem.leftLimitSwitchPress() - || m_climberSubsystem.rightLimitSwitchPress()); + return Math.abs(m_climberSubsystem.getPIDPosition()) < 100 || (m_climberSubsystem.leftLimitSwitchPress() || m_climberSubsystem.rightLimitSwitchPress()); } @Override public void end(boolean interrupted) { m_climberSubsystem.set(0); } + + } diff --git a/src/main/java/frc/robot/commands/ClimberCommand.java b/src/main/java/frc/robot/commands/ClimberCommand.java index b4b5343..9f457fe 100644 --- a/src/main/java/frc/robot/commands/ClimberCommand.java +++ b/src/main/java/frc/robot/commands/ClimberCommand.java @@ -4,8 +4,8 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.ClimberSubsystem; +import edu.wpi.first.wpilibj2.command.CommandBase; /** * An example command that uses an example subsystem. diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java index 5530ed3..71a02cb 100644 --- a/src/main/java/frc/robot/commands/DriveCommand.java +++ b/src/main/java/frc/robot/commands/DriveCommand.java @@ -4,9 +4,9 @@ package frc.robot.commands; +import frc.robot.subsystems.DrivetrainSubsystem; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.DrivetrainSubsystem; /** * An example command that uses an example subsystem. @@ -37,7 +37,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_subsystem.control(-0.5 * m_joystick.getLeftY(), 0.5 * m_joystick.getRightX()); + m_subsystem.control(-m_joystick.getLeftY(), m_joystick.getRightX()); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/HopperCommand.java b/src/main/java/frc/robot/commands/HopperCommand.java index c0e50fc..42dbb8d 100644 --- a/src/main/java/frc/robot/commands/HopperCommand.java +++ b/src/main/java/frc/robot/commands/HopperCommand.java @@ -4,8 +4,8 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.HopperSubsystem; +import edu.wpi.first.wpilibj2.command.CommandBase; /** * An example command that uses an example subsystem. diff --git a/src/main/java/frc/robot/commands/KickIfShootSetRPMCommand.java b/src/main/java/frc/robot/commands/KickIfShootSetRPMCommand.java index dbdce8b..2d7d572 100644 --- a/src/main/java/frc/robot/commands/KickIfShootSetRPMCommand.java +++ b/src/main/java/frc/robot/commands/KickIfShootSetRPMCommand.java @@ -1,9 +1,9 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.ShooterSubsytem; import frc.robot.subsystems.TowerSubsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; /** * An example command that uses an example subsystem. @@ -43,8 +43,7 @@ public void execute() { m_towerSubsystem.setKickerSpeed(1); m_towerSubsystem.setTowerSpeed(0.75); } - SmartDashboard.putBoolean( - "kickIfShootRPM atSpeed", m_shooterSubsystem.checkAtSpeed(m_rpm)); + SmartDashboard.putBoolean("kickIfShootRPM atSpeed", m_shooterSubsystem.checkAtSpeed(m_rpm)); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/KickIfShooterDistanceGoBrrCommand.java b/src/main/java/frc/robot/commands/KickIfShooterDistanceGoBrrCommand.java index c4ac381..a1ef175 100644 --- a/src/main/java/frc/robot/commands/KickIfShooterDistanceGoBrrCommand.java +++ b/src/main/java/frc/robot/commands/KickIfShooterDistanceGoBrrCommand.java @@ -1,11 +1,11 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.ShooterLookupTable; import frc.robot.subsystems.LimelightSubsystem; import frc.robot.subsystems.ShooterSubsytem; import frc.robot.subsystems.TowerSubsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; /** * An example command that uses an example subsystem. @@ -24,10 +24,10 @@ public class KickIfShooterDistanceGoBrrCommand extends CommandBase { * @param subsystem The subsystem used by this command. */ public KickIfShooterDistanceGoBrrCommand( - ShooterSubsytem subsystem, - TowerSubsystem towerSubsystem, - LimelightSubsystem limelightSubsystem, - ShooterLookupTable shooterLookupTable) { + ShooterSubsytem subsystem, + TowerSubsystem towerSubsystem, + LimelightSubsystem limelightSubsystem, + ShooterLookupTable shooterLookupTable) { m_shooterSubsystem = subsystem; m_towerSubsystem = towerSubsystem; m_limelightSubsystem = limelightSubsystem; @@ -52,9 +52,8 @@ public void execute() { m_towerSubsystem.setKickerSpeed(1); m_towerSubsystem.setTowerSpeed(0.75); } - SmartDashboard.putBoolean( - "kickIfShootDist atSpeed", - m_shooterSubsystem.checkAtSpeed(m_shooterLookupTable.getRpmTable(distance))); + SmartDashboard.putBoolean("kickIfShootDist atSpeed", m_shooterSubsystem.checkAtSpeed(m_shooterLookupTable.getRpmTable(distance))); + } // Called once the command ends or is interrupted. @@ -68,6 +67,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { return m_shooterSubsystem.checkAtSpeed( - m_shooterLookupTable.getRpmTable(m_limelightSubsystem.limelightDistance())); + m_shooterLookupTable.getRpmTable(m_limelightSubsystem.limelightDistance())); } } diff --git a/src/main/java/frc/robot/commands/RunClimberToBottomCommand.java b/src/main/java/frc/robot/commands/RunClimberToBottomCommand.java index d7a68c2..f523641 100644 --- a/src/main/java/frc/robot/commands/RunClimberToBottomCommand.java +++ b/src/main/java/frc/robot/commands/RunClimberToBottomCommand.java @@ -4,8 +4,8 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.ClimberSubsystem; +import edu.wpi.first.wpilibj2.command.CommandBase; /** * An example command that uses an example subsystem. diff --git a/src/main/java/frc/robot/commands/RunIntakeCommand.java b/src/main/java/frc/robot/commands/RunIntakeCommand.java index 59efdde..bfa99fd 100644 --- a/src/main/java/frc/robot/commands/RunIntakeCommand.java +++ b/src/main/java/frc/robot/commands/RunIntakeCommand.java @@ -4,9 +4,9 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.IntakeSubsystem; +import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.TowerSubsystem; /** @@ -27,12 +27,7 @@ public class RunIntakeCommand extends CommandBase { * * @param subsystem The subsystem used by this command. */ - public RunIntakeCommand( - IntakeSubsystem intakeSubsystem, - double intakeSpeed, - HopperSubsystem hopperSubsystem, - TowerSubsystem towerSubsystem, - double towerSpeed) { + public RunIntakeCommand(IntakeSubsystem intakeSubsystem, double intakeSpeed, HopperSubsystem hopperSubsystem, TowerSubsystem towerSubsystem, double towerSpeed) { m_intakeSubsystem = intakeSubsystem; m_hopperSubsystem = hopperSubsystem; m_towerSubsystem = towerSubsystem; diff --git a/src/main/java/frc/robot/commands/SetShooterSpeedCommand.java b/src/main/java/frc/robot/commands/SetShooterSpeedCommand.java index 703e80d..e515b29 100644 --- a/src/main/java/frc/robot/commands/SetShooterSpeedCommand.java +++ b/src/main/java/frc/robot/commands/SetShooterSpeedCommand.java @@ -4,8 +4,8 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.ShooterSubsytem; +import edu.wpi.first.wpilibj2.command.CommandBase; /** * An example command that uses an example subsystem. diff --git a/src/main/java/frc/robot/commands/ShootFromDistanceCommand.java b/src/main/java/frc/robot/commands/ShootFromDistanceCommand.java index 8f299a0..b9e3469 100644 --- a/src/main/java/frc/robot/commands/ShootFromDistanceCommand.java +++ b/src/main/java/frc/robot/commands/ShootFromDistanceCommand.java @@ -1,9 +1,9 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.ShooterLookupTable; import frc.robot.subsystems.LimelightSubsystem; import frc.robot.subsystems.ShooterSubsytem; +import edu.wpi.first.wpilibj2.command.CommandBase; /** * An example command that uses an example subsystem. @@ -21,9 +21,9 @@ public class ShootFromDistanceCommand extends CommandBase { * @param subsystem The subsystem used by this command. */ public ShootFromDistanceCommand( - ShooterSubsytem subsystem, - LimelightSubsystem limelightsubsystem, - ShooterLookupTable shooterLookupTable) { + ShooterSubsytem subsystem, + LimelightSubsystem limelightsubsystem, + ShooterLookupTable shooterLookupTable) { m_shooterSubsystem = subsystem; m_limelightSubsystem = limelightsubsystem; m_shooterLookupTable = shooterLookupTable; @@ -52,6 +52,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { return m_shooterSubsystem.checkAtSpeed( - m_shooterLookupTable.getRpmTable(m_limelightSubsystem.limelightDistance())); + m_shooterLookupTable.getRpmTable(m_limelightSubsystem.limelightDistance())); } } diff --git a/src/main/java/frc/robot/commands/ShooterPIDCommand.java b/src/main/java/frc/robot/commands/ShooterPIDCommand.java index cb40eca..4454bbc 100644 --- a/src/main/java/frc/robot/commands/ShooterPIDCommand.java +++ b/src/main/java/frc/robot/commands/ShooterPIDCommand.java @@ -5,15 +5,13 @@ package frc.robot.commands; import com.gos.lib.properties.GosDoubleProperty; - -import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.ShooterSubsytem; +import edu.wpi.first.wpilibj2.command.CommandBase; /** * An example command that uses an example subsystem. */ public class ShooterPIDCommand extends CommandBase { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) private final ShooterSubsytem m_shooterSubsystem; private final GosDoubleProperty m_tunableShooterGoal = new GosDoubleProperty(false, "Shooter Goal", 1000); @@ -21,7 +19,7 @@ public class ShooterPIDCommand extends CommandBase { /** * Creates a new ExampleCommand. * - * @param subsystem The subsystem used by this command. + * @param shooterSubsystem The subsystem used by this command. */ public ShooterPIDCommand(ShooterSubsytem shooterSubsystem) { m_shooterSubsystem = shooterSubsystem; diff --git a/src/main/java/frc/robot/commands/TowerDownCommand.java b/src/main/java/frc/robot/commands/TowerDownCommand.java index 7edb36f..5e19b08 100644 --- a/src/main/java/frc/robot/commands/TowerDownCommand.java +++ b/src/main/java/frc/robot/commands/TowerDownCommand.java @@ -4,8 +4,8 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.TowerSubsystem; +import edu.wpi.first.wpilibj2.command.CommandBase; /** * An example command that uses an example subsystem. diff --git a/src/main/java/frc/robot/commands/TowerKickerCommand.java b/src/main/java/frc/robot/commands/TowerKickerCommand.java index 1b168bc..a4b7c96 100644 --- a/src/main/java/frc/robot/commands/TowerKickerCommand.java +++ b/src/main/java/frc/robot/commands/TowerKickerCommand.java @@ -4,22 +4,23 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.TowerSubsystem; +import edu.wpi.first.wpilibj2.command.CommandBase; /** * An example command that uses an example subsystem. */ public class TowerKickerCommand extends CommandBase { + private static final double SPEED = 0.75; + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) private final TowerSubsystem m_towerSubsystem; - private static final double SPEED = 0.75; /** * Creates a new TowerDownCommand. * - * @param subsystem The subsystem used by this command. + * @param towerSubsystem The subsystem used by this command. */ public TowerKickerCommand(TowerSubsystem towerSubsystem) { m_towerSubsystem = towerSubsystem; diff --git a/src/main/java/frc/robot/commands/TowerUpCommand.java b/src/main/java/frc/robot/commands/TowerUpCommand.java index ab3eff2..62f6aea 100644 --- a/src/main/java/frc/robot/commands/TowerUpCommand.java +++ b/src/main/java/frc/robot/commands/TowerUpCommand.java @@ -4,22 +4,21 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.TowerSubsystem; +import edu.wpi.first.wpilibj2.command.CommandBase; /** * An example command that uses an example subsystem. */ public class TowerUpCommand extends CommandBase { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final TowerSubsystem m_towerSubsystem; - private static final double SPEED = 0.5; + private final TowerSubsystem m_towerSubsystem; + /** * Creates a new TowerUpCommand. * - * @param subsystem The subsystem used by this command. + * @param towerSubsystem The subsystem used by this command. */ public TowerUpCommand(TowerSubsystem towerSubsystem) { m_towerSubsystem = towerSubsystem; @@ -39,6 +38,7 @@ public void execute() { m_towerSubsystem.setKickerSpeed(-0.5); } + // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { diff --git a/src/main/java/frc/robot/commands/TrajectoryCommand.java b/src/main/java/frc/robot/commands/TrajectoryCommand.java index 4a1e442..cc224e9 100644 --- a/src/main/java/frc/robot/commands/TrajectoryCommand.java +++ b/src/main/java/frc/robot/commands/TrajectoryCommand.java @@ -4,6 +4,7 @@ package frc.robot.commands; +import frc.robot.subsystems.DrivetrainSubsystem; import edu.wpi.first.math.controller.RamseteController; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.trajectory.Trajectory; @@ -12,7 +13,6 @@ import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.DrivetrainSubsystem; import java.io.IOException; import java.nio.file.Path; @@ -38,7 +38,7 @@ public TrajectoryCommand(DrivetrainSubsystem subsystem, String trajectoryPathNam m_trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath); } catch (IOException ex) { DriverStation.reportError( - "Unable to open trajectory: " + trajectoryPathName, ex.getStackTrace()); + "Unable to open trajectory: " + trajectoryPathName, ex.getStackTrace()); } m_timer = new Timer(); m_controller = new RamseteController(); diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index ae44b1c..52cb135 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -7,23 +7,23 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.RelativeEncoder; +import frc.robot.Constants; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; public class ClimberSubsystem extends SubsystemBase { private final CANSparkMax m_leftClimber = new CANSparkMax(Constants.CLIMBER_LEFT, MotorType.kBrushless); private final CANSparkMax m_rightClimber = - new CANSparkMax(Constants.CLIMBER_RIGHT, MotorType.kBrushless); + new CANSparkMax(Constants.CLIMBER_RIGHT, MotorType.kBrushless); private final DigitalInput m_leftLimitSwitch = new DigitalInput(Constants.LEFT_LIMIT_SWITCH); private final DigitalInput m_rightLimitSwitch = new DigitalInput(Constants.RIGHT_LIMIT_SWITCH); private final RelativeEncoder m_leftEncoder = m_leftClimber.getEncoder(); private final RelativeEncoder m_rightEncoder = m_rightClimber.getEncoder(); - private final ProfiledPIDController m_climberPID = - new ProfiledPIDController(3.5, 0.0, 0.0, new Constraints(20.0, 150.0), 0.02); + private final ProfiledPIDController m_climberPID = new ProfiledPIDController( + 3.5, 0.0, 0.0, new Constraints(20.0, 150.0), 0.02); /** * Creates a new ClimberSubsystem. @@ -39,7 +39,6 @@ public ClimberSubsystem() { m_rightClimber.follow(m_leftClimber, true); m_leftClimber.setSmartCurrentLimit(50); m_rightClimber.setSmartCurrentLimit(50); - } public void runClimberPID(double goal) { @@ -47,6 +46,7 @@ public void runClimberPID(double goal) { set(m_climberPID.calculate(m_leftEncoder.getPosition())); } + @SuppressWarnings("PMD.AvoidReassigningParameters") public void set(double speed) { if (speed < 0 && (leftLimitSwitchPress() || rightLimitSwitchPress())) { diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index 8ed9c64..168c10f 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -8,12 +8,16 @@ import com.gos.lib.rev.RevPidPropertyBuilder; import com.kauailabs.navx.frc.AHRS; import com.pathplanner.lib.auto.RamseteAutoBuilder; -import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.RelativeEncoder; +import com.revrobotics.SimableCANSparkMax; import com.revrobotics.SparkMaxPIDController; import edu.wpi.first.math.controller.RamseteController; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -24,40 +28,45 @@ import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; +import org.snobotv2.module_wrappers.navx.NavxWrapper; +import org.snobotv2.module_wrappers.rev.RevEncoderSimWrapper; +import org.snobotv2.module_wrappers.rev.RevMotorControllerSimWrapper; +import org.snobotv2.sim_wrappers.DifferentialDrivetrainSimWrapper; import java.util.Map; import java.util.function.Consumer; // Drive train -@SuppressWarnings("PMD.TooManyMethods") +@SuppressWarnings({"PMD.TooManyMethods", "PMD.TooManyFields"}) public class DrivetrainSubsystem extends SubsystemBase { - private final CANSparkMax m_leftLeader = - new CANSparkMax(Constants.DRIVE_LEFT_LEADER, MotorType.kBrushless); - private final CANSparkMax m_leftFollower = - new CANSparkMax(Constants.DRIVE_LEFT_FOLLOWER, MotorType.kBrushless); - private final CANSparkMax m_rightLeader = - new CANSparkMax(Constants.DRIVE_RIGHT_LEADER, MotorType.kBrushless); - private final CANSparkMax m_rightFollower = - new CANSparkMax(Constants.DRIVE_RIGHT_FOLLOWER, MotorType.kBrushless); + private final SimableCANSparkMax m_leftLeader = + new SimableCANSparkMax(Constants.DRIVE_LEFT_LEADER, MotorType.kBrushless); + private final SimableCANSparkMax m_leftFollower = + new SimableCANSparkMax(Constants.DRIVE_LEFT_FOLLOWER, MotorType.kBrushless); + private final SimableCANSparkMax m_rightLeader = + new SimableCANSparkMax(Constants.DRIVE_RIGHT_LEADER, MotorType.kBrushless); + private final SimableCANSparkMax m_rightFollower = + new SimableCANSparkMax(Constants.DRIVE_RIGHT_FOLLOWER, MotorType.kBrushless); private final DifferentialDrive m_drive = new DifferentialDrive(m_leftLeader, m_rightLeader); private final DifferentialDriveKinematics m_kinematics = - new DifferentialDriveKinematics(Constants.DRIVE_TRACK); + new DifferentialDriveKinematics(Constants.DRIVE_TRACK); // odometry private final RelativeEncoder m_leftEncoder = m_leftLeader.getEncoder(); private final RelativeEncoder m_rightEncoder = m_rightLeader.getEncoder(); private final AHRS m_gyro = new AHRS(SPI.Port.kMXP); - private final DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(0), 0, 0); + private final DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(new Rotation2d(), 0, 0); private final Field2d m_field = new Field2d(); // PID private final SparkMaxPIDController m_leftController = m_leftLeader.getPIDController(); + private final PidProperty m_leftProperties; private final SparkMaxPIDController m_rightController = m_rightLeader.getPIDController(); - private final PidProperty m_leftPidProperty; - private final PidProperty m_rightPidProperty; + private final PidProperty m_rightProperties; + + // Simulation + private DifferentialDrivetrainSimWrapper m_simulator; /** * Creates a new DrivetrainSubsystem. @@ -65,6 +74,9 @@ public class DrivetrainSubsystem extends SubsystemBase { public DrivetrainSubsystem() { m_gyro.calibrate(); + m_leftProperties = setupVelocityPidValues(m_leftController); + m_rightProperties = setupVelocityPidValues(m_rightController); + m_leftLeader.restoreFactoryDefaults(); m_leftFollower.restoreFactoryDefaults(); m_rightLeader.restoreFactoryDefaults(); @@ -92,17 +104,30 @@ public DrivetrainSubsystem() { SmartDashboard.putData(m_field); - m_leftPidProperty = createDrivetrainProperty(m_leftController); - m_rightPidProperty = createDrivetrainProperty(m_rightController); + if (RobotBase.isSimulation()) { + DifferentialDrivetrainSim drivetrainSim = DifferentialDrivetrainSim.createKitbotSim( + DifferentialDrivetrainSim.KitbotMotor.kDoubleNEOPerSide, + DifferentialDrivetrainSim.KitbotGearing.k5p95, + DifferentialDrivetrainSim.KitbotWheelSize.kSixInch, + null); + m_simulator = new DifferentialDrivetrainSimWrapper( + drivetrainSim, + new RevMotorControllerSimWrapper(m_leftLeader), + new RevMotorControllerSimWrapper(m_rightLeader), + RevEncoderSimWrapper.create(m_leftLeader), + RevEncoderSimWrapper.create(m_rightLeader), + new NavxWrapper().getYawGyro()); + m_simulator.setRightInverted(false); + } } - private PidProperty createDrivetrainProperty(SparkMaxPIDController pidController) { - return new RevPidPropertyBuilder("Drivetrain", false, pidController, 0) - .addP(1) - .addI(0) - .addD(0) - .build(); + private PidProperty setupVelocityPidValues(SparkMaxPIDController pidController) { + return new RevPidPropertyBuilder("ChassisVelocity", false, pidController, 0) + .addP(1) + .addI(0) + .addD(0) + .build(); } public void control(double speed, double rotation) { @@ -155,20 +180,20 @@ public RamseteAutoBuilder ramseteAutoBuilderNoPoseReset(Map eve @Override public void periodic() { - m_leftPidProperty.updateIfChanged(); - m_rightPidProperty.updateIfChanged(); // This method will be called once per scheduler run - Rotation2d gyroAngle = Rotation2d.fromDegrees(-m_gyro.getAngle()); + var gyroAngle = Rotation2d.fromDegrees(-m_gyro.getAngle()); m_odometry.update(gyroAngle, m_leftEncoder.getPosition(), m_rightEncoder.getPosition()); m_field.setRobotPose(m_odometry.getPoseMeters()); SmartDashboard.putNumber("Chassis Left Velocity", leftVelocity()); SmartDashboard.putNumber("Chassis Right Velocity", rightVelocity()); + m_leftProperties.updateIfChanged(); + m_rightProperties.updateIfChanged(); } @Override public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation + m_simulator.update(); } } diff --git a/src/main/java/frc/robot/subsystems/HopperSubsystem.java b/src/main/java/frc/robot/subsystems/HopperSubsystem.java index 19445be..7b920de 100644 --- a/src/main/java/frc/robot/subsystems/HopperSubsystem.java +++ b/src/main/java/frc/robot/subsystems/HopperSubsystem.java @@ -2,8 +2,8 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +import edu.wpi.first.wpilibj2.command.SubsystemBase; public class HopperSubsystem extends SubsystemBase { private final CANSparkMax m_hopperMotor; diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsytem.java b/src/main/java/frc/robot/subsystems/ShooterSubsytem.java index 9e9475b..0636924 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsytem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsytem.java @@ -10,61 +10,86 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.RelativeEncoder; +import com.revrobotics.SimableCANSparkMax; import com.revrobotics.SparkMaxPIDController; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; import frc.robot.Constants; import frc.robot.ShooterLookupTable; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.snobotv2.module_wrappers.rev.RevEncoderSimWrapper; +import org.snobotv2.module_wrappers.rev.RevMotorControllerSimWrapper; +import org.snobotv2.sim_wrappers.FlywheelSimWrapper; +import org.snobotv2.sim_wrappers.ISimWrapper; +import org.snobotv2.sim_wrappers.InstantaneousMotorSim; public class ShooterSubsytem extends SubsystemBase { - private static final double ENCODER_REDUCTION = 0.5; + private static final double AFTER_ENCODER_REDUCTION = 0.5; + + public static final double FENDER_RPM = 1500; /** * Creates a new Shooter. */ - private final CANSparkMax m_shooterMotor; + private final SimableCANSparkMax m_shooterMotor; - private final CANSparkMax m_hoodMotor; + private final SimableCANSparkMax m_hoodMotor; private final ShooterLookupTable m_shooterLookupTable; private final RelativeEncoder m_encoder; private final SparkMaxPIDController m_pidController; - private final PidProperty m_pidProperty; - private final GosDoubleProperty m_tunableAllowableError = - new GosDoubleProperty(false, "Shooter(AllowableError))", 50); - public static final double FENDER_RPM = 1500; + private final PidProperty m_pidProperties; + private final GosDoubleProperty m_tunableAllowableError = new GosDoubleProperty(false, "Shooter(AllowableError))", 50); + + // Simulation + private ISimWrapper m_shooterSimulator; + private ISimWrapper m_hoodSimulator; public ShooterSubsytem() { - m_shooterMotor = new CANSparkMax(Constants.SHOOTER_SPARK, MotorType.kBrushless); - m_hoodMotor = new CANSparkMax(Constants.SHOOTER_HOOD_SPARK, MotorType.kBrushless); + m_shooterMotor = new SimableCANSparkMax(Constants.SHOOTER_SPARK, MotorType.kBrushless); + m_hoodMotor = new SimableCANSparkMax(Constants.SHOOTER_HOOD_SPARK, MotorType.kBrushless); m_shooterMotor.setSmartCurrentLimit(50); m_hoodMotor.setSmartCurrentLimit(30); m_shooterLookupTable = new ShooterLookupTable(); m_encoder = m_shooterMotor.getEncoder(); m_pidController = m_shooterMotor.getPIDController(); - m_pidProperty = new RevPidPropertyBuilder("Shooter", false, m_pidController, 0) - .addP(0) - .addI(0) - .addD(0) - .addFF(0.00045) - .build(); - + m_pidProperties = new RevPidPropertyBuilder("Shooter", false, m_pidController, 0) + .addP(0) + .addI(0) + .addD(0) + .addFF(0.00045) + .build(); m_shooterMotor.setIdleMode(CANSparkMax.IdleMode.kCoast); m_shooterMotor.restoreFactoryDefaults(); m_shooterMotor.setInverted(true); m_shooterMotor.burnFlash(); + + if (RobotBase.isSimulation()) { + FlywheelSim shooterFlywheelSim = new FlywheelSim(DCMotor.getNeo550(2), 1, 0.01); + m_shooterSimulator = new FlywheelSimWrapper(shooterFlywheelSim, + new RevMotorControllerSimWrapper(m_shooterMotor), + RevEncoderSimWrapper.create(m_shooterMotor)); + + m_hoodSimulator = new InstantaneousMotorSim( + new RevMotorControllerSimWrapper(m_hoodMotor), + RevEncoderSimWrapper.create(m_hoodMotor), + 1 + ); + } } @Override public void periodic() { - // This method will be called once per scheduler run - m_pidProperty.updateIfChanged(); + m_pidProperties.updateIfChanged(); SmartDashboard.putNumber("shooterRpm", getRPM()); } @Override public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation + m_hoodSimulator.update(); + m_shooterSimulator.update(); } public void setPidRpm(double rpm) { @@ -77,7 +102,7 @@ public boolean checkAtSpeed(double goal) { } public double getRPM() { - return m_encoder.getVelocity() * ENCODER_REDUCTION; + return m_encoder.getVelocity() * AFTER_ENCODER_REDUCTION; } public void shootFromDistance(double distance) { diff --git a/src/main/java/frc/robot/subsystems/TowerSubsystem.java b/src/main/java/frc/robot/subsystems/TowerSubsystem.java index 24ada1a..e7dbce0 100644 --- a/src/main/java/frc/robot/subsystems/TowerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TowerSubsystem.java @@ -8,8 +8,8 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +import edu.wpi.first.wpilibj2.command.SubsystemBase; public class TowerSubsystem extends SubsystemBase { private final CANSparkMax m_towerMotor; diff --git a/vendordeps/SnobotSim.json b/vendordeps/SnobotSim.json new file mode 100644 index 0000000..da79e1b --- /dev/null +++ b/vendordeps/SnobotSim.json @@ -0,0 +1,22 @@ +{ + "fileName": "SnobotSim.json", + "name": "SnobotSim", + "version": "2023.10.28-18", + "uuid": "88e2fcb0-75bd-11ee-b0bf-396770175b4a", + "mavenUrls": ["https://raw.githubusercontent.com/snobotsim/maven_repo/master/release"], + "jsonUrl": "http://raw.githubusercontent.com/snobotsim/maven_repo/master/release/SnobotSim.json", + "javaDependencies": [ + { + "groupId": "org.snobotv2", + "artifactId": "snobot_sim_java", + "version": "2023.10.28-18" + }, + { + "groupId": "org.snobotv2", + "artifactId": "snobot_swerve_sim", + "version": "2023.10.28-18" + } + ], + "jniDependencies": [], + "cppDependencies": [] +}