diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8f54e4a..801c5ba 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -2,8 +2,6 @@ package frc.robot; -import edu.wpi.first.math.util.Units; - /** * 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 @@ -22,13 +20,13 @@ public static final class CANConstants { public static final int MOTORFRONTLEFTID = 12; public static final int MOTORBACKLEFTID = 11; /// Arm Motors - public static final int MOTORARMMAINID = 21; + public static final int MOTORARMMAINID = -1; /// Shooter Motors public static final int MOTORSHOOTERLEFTID = 31; public static final int MOTORSHOOTERRIGHTID = 32; /// Lifter Motors - public static final int MOTORLIFTERLEFTID = 41; - public static final int MOTORLIFTERRIGHTID = 42; + public static final int MOTORLIFTERLEFTID = -1; + public static final int MOTORLIFTERRIGHTID = -1; } public static final double MAX_SPEED = 0.8; @@ -45,13 +43,6 @@ public static final class CANConstants { // Joystick buttons public static final int TRIGGER = 1; - public static final int ARMAMPBUTTON = 3; - public static final int ARMSPEAKERBUTTON = 4; - public static final int ARMDEFAULTBUTTON = 2; - public static final int ARMLOADBUTTON = 11; - public static final int ARMTRAPBUTTON = 9; - public static final int ENABLEAXISBUTTON = 10; - public static final int ARMDISABLEENCODER = 8; public static final int AIMBUTTON = 12; // Analog Ports @@ -59,22 +50,7 @@ public static final class CANConstants { public static final int ULTRASONICSHOOTERPORT = 0; // Digital Ports - public static final int ARMLIMITSWITCHFRONT = 0; - public static final int ARMLIMITSWITCHBACK = 1; + public static final int EXAMPLELIMITSWITCH = 0; // Shooter Angles - public static final double ARMENCODEROFFSET = Units.radiansToDegrees(0.0305); - public static final double ARMSTARTINGANGLE = 22.5 + ARMENCODEROFFSET; // WHY MaTH HURT - public static final double ARMMINRELATVESTART = 0.0; - public static final double ARMLOADANGLE = 35 - ARMSTARTINGANGLE; - public static final double ARMSPEAKERANGLE = 59 - ARMSTARTINGANGLE; // to go to 75 you just put 75 - public static final double ARMAMPANGLE = 110 - ARMSTARTINGANGLE; - public static final double ARMTRAPANGLE = 63 - ARMSTARTINGANGLE; - public static final double ARMMAXRELATIVE = 120 - ARMSTARTINGANGLE; - // Shooter Speeds (M/s) - public static final double SHOOTERSOURCE = -6.0; - public static final double SHOOTERAMP = 3; - public static final double SHOOTERSPEAKER = 20.0; - public static final double SHOOTERTRAP = 8.0; - public static final double SHOOTERDEFAULT = 8.0; // somewhere around 8 is the cap - ma } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c1a8489..7535a4c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,11 +4,9 @@ package frc.robot; -import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.GenericHID; @@ -23,17 +21,11 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.AimCommand; -import frc.robot.commands.ArmCommand; import frc.robot.commands.BalanceCommand; import frc.robot.commands.DefaultDrive; import frc.robot.commands.FlywheelCommand; import frc.robot.commands.LifterCommand; import frc.robot.commands.StraightCommand; -import frc.robot.commands.UltrasonicShooterCommand; -import frc.robot.commands.auto.AimAmpCommand; -import frc.robot.commands.auto.AimSpeakerCommand; -import frc.robot.commands.auto.LowerArmCommand; -import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.CameraSubsystem; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.DriverCameraSubsystem; @@ -65,7 +57,6 @@ public class RobotContainer { private final DriveSubsystem m_driveSubsystem = new DriveSubsystem(); private final CameraSubsystem m_cameraSubsystem = new CameraSubsystem(m_driveSubsystem); - private final ArmSubsystem m_armSubsystem = new ArmSubsystem(m_shooterState); private final FlywheelSubsystem m_shooterSubsytem = new FlywheelSubsystem(); private final LifterSubsystem m_leftLifterSubsystem = new LifterSubsystem(Constants.CANConstants.MOTORLIFTERLEFTID); @@ -81,48 +72,29 @@ public class RobotContainer { new DefaultDrive(m_driveSubsystem, this::getControllerLeftY, this::getControllerRightY); private final StraightCommand m_straightCommand = new StraightCommand(m_driveSubsystem, this::getControllerLeftY, this::getControllerRightY); - private final UltrasonicShooterCommand m_ultrasonicShooterCommand = - new UltrasonicShooterCommand(m_ultrasonicShooterSubsystem, m_shooterState); - private final ArmCommand m_armCommand = - new ArmCommand(m_armSubsystem, m_shooterState, this::GetFlightStickY); private final FlywheelCommand m_shooterCommand = new FlywheelCommand(m_shooterSubsytem, m_shooterState); private final LifterCommand m_LeftLifterCommand = new LifterCommand(m_leftLifterSubsystem); private final LifterCommand m_RightLifterCommand = new LifterCommand(m_rightLifterSubsystem); - // these commands are used by autonomous only - private final AimAmpCommand m_AimAmpCommand = new AimAmpCommand(m_armSubsystem, m_shooterState); - private final LowerArmCommand m_AimLowerCommand = - new LowerArmCommand(m_armSubsystem, m_shooterState); - private final AimSpeakerCommand m_AimSpeakerCommand = - new AimSpeakerCommand(m_armSubsystem, m_shooterState); + // EX: these commands are used by autonomous only + // private final AimAmpCommand m_AimAmpCommand = new AimAmpCommand(m_armSubsystem, + // m_shooterState); // this command is used by on the fly path planning - private Command m_driveToSpeaker; - private Command m_driveToAmp; - private Command m_driveToSource; + // private Command m_driveToAmp; // Init Buttons // private Trigger m_balanceButton; private Trigger m_straightButton; private Trigger m_toggleBrakeButton; private Trigger m_lifterRightButton; private Trigger m_lifterLeftButton; - private Trigger m_driveToSpeakerButton; - private Trigger m_driveToSourceButton; - private Trigger m_driveToAmpButton; + // private Trigger m_driveToAmpButton; private Trigger m_lifterDirectionButton; // joystick buttons private JoystickButton m_aimButton; - private JoystickButton m_armRaiseToSpeakerButton; - private JoystickButton m_armRaiseToAmpButton; - private JoystickButton m_armRaiseToTrapButton; - private JoystickButton m_armWheelLoadButton; - private JoystickButton m_enableAxisButton; private JoystickButton m_shooterTrigger; - private JoystickButton m_armRaiseToDefault; - private JoystickButton m_armDisableEncoderButton; // Init For Autonomous - // private RamseteAutoBuilder autoBuilder; private SendableChooser autoDashboardChooser = new SendableChooser(); public final boolean enableAutoProfiling = false; @@ -136,8 +108,7 @@ public RobotContainer() { setupTriggers(); // Bind the commands to the triggers if (enableAutoProfiling) { - // bindDriveSysIDCommands(); - bindArmSysIDCommands(); + bindDriveSysIDCommands(); // bindShooterSysIDCommands(); } else { bindCommands(); @@ -145,10 +116,6 @@ public RobotContainer() { // set default drive command m_driveSubsystem.setDefaultCommand(m_defaultDrive); - // set default command for shooter ultrasonic sensor - m_ultrasonicShooterSubsystem.setDefaultCommand(m_ultrasonicShooterCommand); - // set default command for arm - m_armSubsystem.setDefaultCommand(m_armCommand); } /** @@ -163,20 +130,11 @@ private void setupTriggers() { m_straightButton = m_controller1.rightBumper(); m_lifterRightButton = m_controller1.rightTrigger(); m_lifterLeftButton = m_controller1.leftTrigger(); - m_driveToSpeakerButton = m_controller1.y(); - m_driveToSourceButton = m_controller1.b(); + // m_driveToAmpButton= m_controller1.y(); m_lifterDirectionButton = m_controller1.a(); // Joystick buttons m_aimButton = new JoystickButton(m_flightStick, Constants.AIMBUTTON); - // arm raise buttons - m_armWheelLoadButton = new JoystickButton(m_flightStick, Constants.ARMLOADBUTTON); - m_armDisableEncoderButton = new JoystickButton(m_flightStick, Constants.ARMDISABLEENCODER); - m_armRaiseToSpeakerButton = new JoystickButton(m_flightStick, Constants.ARMSPEAKERBUTTON); - m_armRaiseToAmpButton = new JoystickButton(m_flightStick, Constants.ARMAMPBUTTON); - m_armRaiseToTrapButton = new JoystickButton(m_flightStick, Constants.ARMTRAPBUTTON); - m_enableAxisButton = new JoystickButton(m_flightStick, Constants.ENABLEAXISBUTTON); - m_armRaiseToDefault = new JoystickButton(m_flightStick, Constants.ARMDEFAULTBUTTON); // load and shoot buttons m_shooterTrigger = new JoystickButton(m_flightStick, Constants.TRIGGER); @@ -187,9 +145,7 @@ private void bindCommands() { // m_balanceButton.whileTrue(m_balanceCommand); m_straightButton.whileTrue(m_straightCommand); m_aimButton.whileTrue(m_aimCommand); - m_driveToSpeakerButton.whileTrue(m_driveToSpeaker); // m_driveToAmpButton.whileTrue(m_driveToAmp); // TODO: Need to bind button - m_driveToSourceButton.whileTrue(m_driveToSource); m_lifterRightButton.whileTrue(m_RightLifterCommand); m_lifterLeftButton.whileTrue(m_LeftLifterCommand); m_lifterDirectionButton.whileTrue( @@ -198,18 +154,6 @@ private void bindCommands() { m_toggleBrakeButton.whileTrue(new InstantCommand(() -> m_driveSubsystem.SwitchBrakemode())); // shooter + arm commands m_shooterTrigger.whileTrue(m_shooterCommand); - m_armDisableEncoderButton.whileTrue(new InstantCommand(() -> m_armSubsystem.disableOffset())); - m_armRaiseToSpeakerButton.whileTrue( - new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.SPEAKER))); - m_armRaiseToAmpButton.whileTrue( - new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.AMP))); - m_armWheelLoadButton.whileTrue( - new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.SOURCE))); - m_armRaiseToTrapButton.whileTrue( - new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.TRAP))); - m_armRaiseToDefault.whileTrue( - new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.DEFAULT))); - m_enableAxisButton.whileTrue(new InstantCommand(() -> m_shooterState.toggleAxis())); } private void bindDriveSysIDCommands() { @@ -220,15 +164,6 @@ private void bindDriveSysIDCommands() { m_controller1.leftTrigger().whileTrue(new InstantCommand(() -> DataLogManager.stop())); } - private void bindArmSysIDCommands() { - m_controller1.a().whileTrue(m_armSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); - m_controller1.b().whileTrue(m_armSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); - m_controller1.x().whileTrue(m_armSubsystem.sysIdDynamic(SysIdRoutine.Direction.kForward)); - m_controller1.y().whileTrue(m_armSubsystem.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - m_controller1.leftTrigger().whileTrue(new InstantCommand(() -> DataLogManager.stop())); - m_armSubsystem.disablePID(); - } - private void bindShooterSysIDCommands() { m_controller1 .a() @@ -244,10 +179,6 @@ private void bindShooterSysIDCommands() { private void initializeAutonomous() { // Network Table Routine Options autoDashboardChooser.setDefaultOption("SFR", "SFR"); - autoDashboardChooser.addOption("ShootSpeakerCenter", "ShootSpeakerCenter"); - autoDashboardChooser.addOption("ShootSpeakerLeft", "ShootSpeakerLeft"); - autoDashboardChooser.addOption("ShootSpeakerRight", "ShootSpeakerRight"); - autoDashboardChooser.addOption("ShootSpeakerFar", "ShootSpeakerFar"); autoDashboardChooser.addOption("DriveForward", "DriveForward"); autoDashboardChooser.addOption("Do Nothing", "DoNothing"); SmartDashboard.putData(autoDashboardChooser); @@ -260,47 +191,18 @@ private void initializeAutonomous() { NamedCommands.registerCommand( "BrakeCommand", new InstantCommand(() -> m_driveSubsystem.SetBrakemode())); NamedCommands.registerCommand("ShooterCommand", m_shooterCommand); - NamedCommands.registerCommand("AimSpeakerCommand", m_AimSpeakerCommand); - NamedCommands.registerCommand("ArmCommand", m_armCommand); - NamedCommands.registerCommand("AimAmpCommand", m_AimAmpCommand); - NamedCommands.registerCommand("AimLowerCommand", m_AimLowerCommand); + // NamedCommands.registerCommand("AimAmpCommand", m_AimAmpCommand); - // autoBuilder = - // new RamseteAutoBuilder( - // m_driveSubsystem::getPose, // Pose2d supplier - // m_driveSubsystem - // ::resetPose, // Pose2d consumer, used to reset odometry at the beginning of auto - // new RamseteController( - // DriveConstants.kRamseteB, DriveConstants.kRamseteZeta), // RamseteController - // DriveConstants.kDriveKinematics, // DifferentialDriveKinematics - // DriveConstants - // .FeedForward, // A feedforward value to apply to the drive subsystem's - // controllers - // m_driveSubsystem::getWheelSpeeds, // A method for getting the current wheel speeds of - // the drive - // new PIDConstants( - // DriveConstants.kPDriveVel, 0, 0), // A PID controller for wheel velocity control - // m_driveSubsystem - // ::tankDriveVolts, // A consumer that takes left and right wheel voltages and sets - // // them to the drive subsystem's controllers - // autonomousEventMap, - // true, // change for either team - // m_driveSubsystem // Requirements of the commands (should be the drive subsystem) - // ); } private void configureTeleopPaths() { // Limits for all Paths PathConstraints constraints = new PathConstraints(3.0, 2.0, Units.degreesToRadians(540), Units.degreesToRadians(720)); + // EX + // PathPlannerPath ampPath = PathPlannerPath.fromPathFile("TeleopAmpPath"); - PathPlannerPath speakerPath = PathPlannerPath.fromPathFile("TeleopSpeakerPath"); - PathPlannerPath ampPath = PathPlannerPath.fromPathFile("TeleopAmpPath"); - PathPlannerPath sourcePath = PathPlannerPath.fromPathFile("TeleopSourcePath"); - - m_driveToSpeaker = AutoBuilder.pathfindThenFollowPath(speakerPath, constraints); - m_driveToAmp = AutoBuilder.pathfindThenFollowPath(ampPath, constraints); - m_driveToSource = AutoBuilder.pathfindThenFollowPath(sourcePath, constraints); + // m_driveToAmp = AutoBuilder.pathfindThenFollowPath(ampPath, constraints); } public double getControllerRightY() { diff --git a/src/main/java/frc/robot/ShooterState.java b/src/main/java/frc/robot/ShooterState.java index 3f184d7..995c9b1 100644 --- a/src/main/java/frc/robot/ShooterState.java +++ b/src/main/java/frc/robot/ShooterState.java @@ -51,6 +51,8 @@ public void setLowered() { } public double getShooterSpeed() { + /* + Example: switch (mode) { case SOURCE: // TODO return Constants.SHOOTERSOURCE; @@ -63,6 +65,8 @@ public double getShooterSpeed() { default: return Constants.SHOOTERDEFAULT; } + */ + return 0; } /** diff --git a/src/main/java/frc/robot/commands/ArmCommand.java b/src/main/java/frc/robot/commands/ArmCommand.java deleted file mode 100644 index 82a7d81..0000000 --- a/src/main/java/frc/robot/commands/ArmCommand.java +++ /dev/null @@ -1,96 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.ShooterState; -import frc.robot.subsystems.ArmSubsystem; -import frc.robot.utils.HelperFunctions; -import java.util.function.DoubleSupplier; - -/** An example command that uses an example subsystem. */ -public class ArmCommand extends Command { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final ArmSubsystem m_ArmSubsystem; - - private final ShooterState m_shooterState; - private final DoubleSupplier m_yAxis; - private final double kMaxRadiansPerInput = Units.degreesToRadians(5); - - /** - * Creates a new ExampleCommand. - * - * @param subsystem The subsystem used by this command. - */ - public ArmCommand(ArmSubsystem a_subsystem, ShooterState shooterState, DoubleSupplier yAxis) { - m_ArmSubsystem = a_subsystem; - m_shooterState = shooterState; - m_yAxis = yAxis; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(m_ArmSubsystem); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if (!m_shooterState.shooting) { - if (m_shooterState.axisEnabled) { - m_ArmSubsystem.disableOffset(); - if ((!HelperFunctions.inDeadzone(m_yAxis.getAsDouble(), Constants.CONTROLLERDEADZONE))) { - m_ArmSubsystem.MoveArmRelative(m_yAxis.getAsDouble() * kMaxRadiansPerInput); - } - } else if (m_shooterState.isLoaded & !m_shooterState.isLowered) { - m_ArmSubsystem.enableOffset(); - m_ArmSubsystem.lowerArm(); - m_shooterState.setLowered(); - } else if (!m_shooterState.isLoaded & !m_shooterState.isLowered) { - m_ArmSubsystem.enableOffset(); - m_ArmSubsystem.lowerArm(); - } else { - m_ArmSubsystem.enableOffset(); - followState(); - } - } - } - - private void followState() { - switch (m_shooterState.mode) { - case SOURCE: - m_ArmSubsystem.moveArmToLoad(); - break; - case AMP: - m_ArmSubsystem.moveArmToAmp(); - break; - case SPEAKER: - m_ArmSubsystem.moveArmToSpeaker(); - break; - case TRAP: - m_ArmSubsystem.moveArmToTrap(); - break; - case DEFAULT: - m_ArmSubsystem.lowerArm(); - break; - case STOP: - m_ArmSubsystem.stop(); - break; - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/UltrasonicShooterCommand.java b/src/main/java/frc/robot/commands/UltrasonicShooterCommand.java deleted file mode 100644 index 793bab6..0000000 --- a/src/main/java/frc/robot/commands/UltrasonicShooterCommand.java +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.ShooterState; -import frc.robot.subsystems.UltrasonicSubsystem; - -/** An example command that uses an example subsystem. */ -public class UltrasonicShooterCommand extends Command { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final UltrasonicSubsystem m_ultrasonicSubsystem; - - private final ShooterState m_shooterState; - - /** - * Creates a new ExampleCommand. - * - * @param subsystem The subsystem used by this command. - */ - public UltrasonicShooterCommand(UltrasonicSubsystem u_subsystem, ShooterState shooterState) { - m_ultrasonicSubsystem = u_subsystem; - m_shooterState = shooterState; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(m_ultrasonicSubsystem); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - double distance = m_ultrasonicSubsystem.DistanceCM(); - if (distance <= 30) { - m_shooterState.setLoaded(); - } else { - m_shooterState.setFired(); - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/auto/.gitkeep b/src/main/java/frc/robot/commands/auto/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc/robot/commands/auto/AimAmpCommand.java b/src/main/java/frc/robot/commands/auto/AimAmpCommand.java deleted file mode 100644 index e13633b..0000000 --- a/src/main/java/frc/robot/commands/auto/AimAmpCommand.java +++ /dev/null @@ -1,58 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands.auto; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.ShooterState; -import frc.robot.subsystems.ArmSubsystem; - -/** An example command that uses an example subsystem. */ -public class AimAmpCommand extends Command { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final ArmSubsystem m_ArmSubsystem; - - private final ShooterState m_shooterState; - - /** - * Creates a new ShootAmpCommand. - * - * @param subsystem The subsystem used by this command. - */ - public AimAmpCommand(ArmSubsystem a_subsystem, ShooterState shooterState) { - m_ArmSubsystem = a_subsystem; - m_shooterState = shooterState; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(m_ArmSubsystem); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - // this bypasses the shooter state and just moves the arm to the speaker position, as the arm - // subsystem gets paused during the auto shoot command - m_shooterState.setMode(ShooterState.ShooterMode.AMP); - m_ArmSubsystem.moveArmToAmp(); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - // if the arm is stopped, at goal, and the offset is set, then the command is finished, aka at - // correct angle - if (m_ArmSubsystem.ArmStopped() && m_ArmSubsystem.atGoal() && m_ArmSubsystem.isOffsetSet()) { - return true; - } else { - return false; - } - } -} diff --git a/src/main/java/frc/robot/commands/auto/AimSpeakerCommand.java b/src/main/java/frc/robot/commands/auto/AimSpeakerCommand.java deleted file mode 100644 index 98f79de..0000000 --- a/src/main/java/frc/robot/commands/auto/AimSpeakerCommand.java +++ /dev/null @@ -1,58 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands.auto; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.ShooterState; -import frc.robot.subsystems.ArmSubsystem; - -/** An example command that uses an example subsystem. */ -public class AimSpeakerCommand extends Command { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final ArmSubsystem m_ArmSubsystem; - - private final ShooterState m_shooterState; - - /** - * Creates a new ShootSpeakerCommand. - * - * @param subsystem The subsystem used by this command. - */ - public AimSpeakerCommand(ArmSubsystem a_subsystem, ShooterState shooterState) { - m_ArmSubsystem = a_subsystem; - m_shooterState = shooterState; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(m_ArmSubsystem); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - // this bypasses the shooter state and just moves the arm to the speaker position, as the arm - // subsystem gets paused during the auto shoot command - m_shooterState.setMode(ShooterState.ShooterMode.SPEAKER); - m_ArmSubsystem.moveArmToSpeaker(); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - // if the arm is stopped, at goal, and the offset is set, then the command is finished, aka at - // correct angle - if (m_ArmSubsystem.ArmStopped() && m_ArmSubsystem.atGoal() && m_ArmSubsystem.isOffsetSet()) { - return true; - } else { - return false; - } - } -} diff --git a/src/main/java/frc/robot/commands/auto/LowerArmCommand.java b/src/main/java/frc/robot/commands/auto/LowerArmCommand.java deleted file mode 100644 index caaa386..0000000 --- a/src/main/java/frc/robot/commands/auto/LowerArmCommand.java +++ /dev/null @@ -1,58 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands.auto; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.ShooterState; -import frc.robot.subsystems.ArmSubsystem; - -/** An example command that uses an example subsystem. */ -public class LowerArmCommand extends Command { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final ArmSubsystem m_ArmSubsystem; - - private final ShooterState m_shooterState; - - /** - * Creates a new ShootSpeakerCommand. - * - * @param subsystem The subsystem used by this command. - */ - public LowerArmCommand(ArmSubsystem a_subsystem, ShooterState shooterState) { - m_ArmSubsystem = a_subsystem; - m_shooterState = shooterState; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(m_ArmSubsystem); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - // this bypasses the shooter state and just moves the arm to the speaker position, as the arm - // subsystem gets paused during the auto shoot command - m_shooterState.setMode(ShooterState.ShooterMode.DEFAULT); - m_ArmSubsystem.lowerArm(); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - // if the arm is stopped, at goal, and the offset is set, then the command is finished, aka at - // correct angle - if (m_ArmSubsystem.ArmStopped() && m_ArmSubsystem.getFrontLimit()) { - return true; - } else { - return false; - } - } -} diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java deleted file mode 100644 index e09571a..0000000 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ /dev/null @@ -1,342 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import static edu.wpi.first.units.Units.Volts; - -import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkBase; -import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkPIDController; -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.Voltage; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.Constants; -import frc.robot.Constants.CANConstants; -import frc.robot.ShooterState; -import frc.robot.utils.HelperFunctions; - -public class ArmSubsystem extends SubsystemBase { - private final ShooterState m_shooterState; - private final CANSparkMax m_armMotorMain; - private final SparkPIDController m_armMainPIDController; - private final RelativeEncoder m_MainEncoder; - private final AbsoluteEncoder m_AbsoluteEncoder; - private final double kP, kI, kD, kIz, kMaxOutput, kMinOutput; - private static double kDt = 0.02; // 20ms (update rate for wpilib) - private final double ksArmVolts = 0.31531; - private final double kgArmGravityGain = 0.19588; - private final double kvArmVoltSecondsPerMeter = 2.0638; - private final double kaArmVoltSecondsSquaredPerMeter = 0.37994; - private final double kMinArmAngleRadians = Units.degreesToRadians(Constants.ARMMINRELATVESTART); - private final double kMaxArmAngleRadians = Units.degreesToRadians(Constants.ARMMAXRELATIVE); - private final double kArmLoadAngleRadians = - Units.degreesToRadians(Constants.ARMLOADANGLE); // angle to be when recieving ring - private final double kArmSpeakerAngleRadians = - Units.degreesToRadians(Constants.ARMSPEAKERANGLE); // angle to be when shooting into speaker - private final double kArmAmpAngleRadians = - Units.degreesToRadians(Constants.ARMAMPANGLE); // angle to be when shooting into amp - private final double kArmTrapAngleRadians = - Units.degreesToRadians(Constants.ARMTRAPANGLE); // angle to be when shooting into trap - - private final double karmMaxVelocity = 1.0; // rad/s - private final double karmMaxAcceleration = 0.5; // rad/s^2 - private boolean kPIDEnabled = true; - // general drive constants - // https://www.chiefdelphi.com/t/encoders-velocity-to-m-s/390332/2 - // https://sciencing.com/convert-rpm-linear-speed-8232280.html - private final double kGearRatio = 120; // TBD, 48:1 / kGearRatio:1 - // basically converted from rotations to radians by multiplying by 2 pi, then adjusting for the - // gear ratio by dividing by the gear ratio. - // remember that 2pi radians in 360 degrees. - private final double kRadiansConversionRatio = (Math.PI * 2) / kGearRatio; - private final double kVelocityConversionRatio = kRadiansConversionRatio / 60; - private final double kAbsoluteRadiansConversionRatio = (Math.PI * 2); - private final double kAbsoluteVelocityConversionRatio = kAbsoluteRadiansConversionRatio / 60; - private final ArmFeedforward m_armFeedforward = - new ArmFeedforward( - ksArmVolts, kgArmGravityGain, kvArmVoltSecondsPerMeter, kaArmVoltSecondsSquaredPerMeter); - private final TrapezoidProfile m_armTrapezoidProfile = - new TrapezoidProfile(new TrapezoidProfile.Constraints(karmMaxVelocity, karmMaxAcceleration)); - private TrapezoidProfile.State m_goal = new TrapezoidProfile.State(); - private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State(); - - // track if goal was changed - private boolean m_newGoal = true; - - // enable or disable offset - private boolean m_offsetEnabled = true; - - // last goal pre offset - private double m_lastGoal = 0.0; - - // setup SysID for auto profiling - private final SysIdRoutine m_sysIdRoutine; - - // setup front limit switch for rest - private final DigitalInput m_frontLimit; - - // last measured state of limit switch - private boolean m_frontLimitState = false; - - // makes limit switch input less finnicky - private final Debouncer m_frontLimitDebouncer = new Debouncer(0.2, Debouncer.DebounceType.kBoth); - - // offset to match the absolute encoder with the main encoder by adding offset to goal - private double m_curOffset = 0.0; - - /** Creates a new ArmSubsystem. */ - public ArmSubsystem(ShooterState shooterState) { - m_shooterState = shooterState; - // create the arm motors - m_armMotorMain = new CANSparkMax(CANConstants.MOTORARMMAINID, CANSparkMax.MotorType.kBrushless); - - // front limit switch - m_frontLimit = new DigitalInput(Constants.ARMLIMITSWITCHFRONT); - - // set the idle mode to brake - m_armMotorMain.setIdleMode(CANSparkMax.IdleMode.kBrake); - m_armMotorMain.setInverted(true); - - // connect to built in PID controller - m_armMainPIDController = m_armMotorMain.getPIDController(); - // allow us to read the encoder - m_MainEncoder = m_armMotorMain.getEncoder(); - m_AbsoluteEncoder = m_armMotorMain.getAbsoluteEncoder(); - m_AbsoluteEncoder.setInverted(false); - m_AbsoluteEncoder.setPositionConversionFactor(kAbsoluteRadiansConversionRatio); - m_AbsoluteEncoder.setVelocityConversionFactor(kAbsoluteVelocityConversionRatio); - // m_AbsoluteEncoder.setZeroOffset(Constants.ARMENCODEROFFSET); - // setup the encoders - m_MainEncoder.setPositionConversionFactor(kRadiansConversionRatio); - m_MainEncoder.setVelocityConversionFactor(kVelocityConversionRatio); - matchEncoders(); - // PID coefficients - kP = 2.3142; - kI = 0; - kD = 0.23128; - kIz = 0; - kMaxOutput = 0.4; - kMinOutput = -0.4; - - // set PID coefficients - m_armMainPIDController.setP(kP); - m_armMainPIDController.setI(kI); - m_armMainPIDController.setD(kD); - m_armMainPIDController.setIZone(kIz); - m_armMainPIDController.setOutputRange(kMinOutput, kMaxOutput); - m_armMainPIDController.setFeedbackDevice(m_MainEncoder); - // m_armMotorMain.setSoftLimit(SoftLimitDirection.kReverse, kMinArmAngleRadians)); - // m_armMotorMain.setSoftLimit(SoftLimitDirection.kForward, kMaxArmAngleRadians); - m_armMotorMain.burnFlash(); - - // setup SysID for auto profiling - m_sysIdRoutine = - new SysIdRoutine( - new SysIdRoutine.Config(), - new SysIdRoutine.Mechanism( - (voltage) -> this.setVoltage(voltage), - null, // No log consumer, since data is recorded by URCL - this)); - } - - public void setVoltage(Measure voltage) { - m_armMotorMain.setVoltage(voltage.in(Volts)); - } - - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutine.quasistatic(direction); - } - - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutine.dynamic(direction); - } - - public double getAverageEncoderPosition() { - // get the average encoder position - return (m_MainEncoder.getPosition()); - } - - /* - * Move arm a certain number of radians relative to current position - */ - public void MoveArmRelative(double radians) { - radians = radians + getAverageEncoderPosition(); - // update the PID controller with current encoder position - MoveArmToPosition(radians); - } - - public void lowerArm() { - // move to set lowered arm position - MoveArmToPosition(kMinArmAngleRadians); - } - - public void rasieArm() { - // move to set raised arm position - MoveArmToPosition(kMaxArmAngleRadians); - } - - public void moveArmToLoad() { - MoveArmToPosition(kArmLoadAngleRadians); - } - - public void moveArmToAmp() { - MoveArmToPosition(kArmAmpAngleRadians); - } - - public void moveArmToSpeaker() { - MoveArmToPosition(kArmSpeakerAngleRadians); - } - - public void moveArmToTrap() { - MoveArmToPosition(kArmTrapAngleRadians); - } - - /* - * Move arm to global position (by updating goal) - */ - public void MoveArmToPosition(double radians) { - // update the motion profile with new goal - // add minimum starting angle to the target angle to get the real angle - double final_radians = Math.max(radians, kMinArmAngleRadians); - final_radians = Math.min(final_radians, kMaxArmAngleRadians); - if (final_radians != m_lastGoal) { - m_newGoal = true; - m_lastGoal = final_radians; - resetOffset(); - } - m_goal = new TrapezoidProfile.State(final_radians + m_curOffset, 0); // set the goal - } - - /* - * attempt to hold arm at current location - */ - public void stop() { - if (!atGoal()) { - // update the PID controller with current encoder position - MoveArmToPosition(getAverageEncoderPosition()); - } - } - - public void disablePID() { - kPIDEnabled = false; - } - - /** Matches the position of the main encoder with the absolute encoder. */ - public void matchEncoders() { - m_MainEncoder.setPosition(m_AbsoluteEncoder.getPosition()); - } - - private void SetOffsetWithEncoder() { - m_curOffset = getError(); - } - - public void resetOffset() { - m_curOffset = 0.0; - } - - /** - * Calculates the error between the position reported by the absolute encoder and the main - * encoder. - * - * @return The error between the two encoder positions. - */ - public double getError() { - return m_MainEncoder.getPosition() - m_AbsoluteEncoder.getPosition(); - } - - /** - * Checks if the arm is at the goal position. - * - * @return true if the arm is at the goal position, false otherwise. - */ - public boolean atGoal() { - return HelperFunctions.inDeadzone( - m_goal.position - m_MainEncoder.getPosition(), Units.degreesToRadians(1)); - } - - /** - * Checks if the arm is stopped. - * - * @return true if the arm is stopped, false otherwise. - */ - public boolean ArmStopped() { - return HelperFunctions.inDeadzone(m_goal.velocity, 0.0001); - } - - public boolean isOffsetSet() { - return m_curOffset != 0.0; - } - - public boolean getFrontLimit() { - return !m_frontLimitDebouncer.calculate(m_frontLimit.get()); - } - - public void enableOffset() { - m_offsetEnabled = true; - } - - public void disableOffset() { - m_offsetEnabled = true; - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - if (kPIDEnabled) { - // update the setpoint - m_setpoint = m_armTrapezoidProfile.calculate(kDt, m_setpoint, m_goal); - // Call the controller and feedforward with the target position and velocity - m_armMainPIDController.setReference( - m_setpoint.position, - CANSparkBase.ControlType.kPosition, - 0, - m_armFeedforward.calculate(m_setpoint.position, m_setpoint.velocity)); - if (atGoal() && ArmStopped() && m_newGoal && m_offsetEnabled) { - double cur_error = getError(); - if (!HelperFunctions.inDeadzone(cur_error, Units.degreesToRadians(3))) { - SetOffsetWithEncoder(); - m_newGoal = - false; // reset the new goal flag, so that we dont try resyncing encoders again - m_goal = new TrapezoidProfile.State(m_lastGoal + m_curOffset, 0); // set the goal - } - } - } - if (m_frontLimitState != getFrontLimit()) { - m_frontLimitState = getFrontLimit(); - if (m_frontLimitState) { - matchEncoders(); - } - } - m_shooterState.updateDash(); - SmartDashboard.putNumber( - "Current Arm Angle (Degrees) (Relative)", - Units.radiansToDegrees(m_MainEncoder.getPosition()) + Constants.ARMSTARTINGANGLE); - SmartDashboard.putNumber( - "Current Arm Angle (Degrees) (Absolute)", - Units.radiansToDegrees(m_AbsoluteEncoder.getPosition()) + Constants.ARMSTARTINGANGLE); - SmartDashboard.putBoolean("Front Limit Switch Pressed", !m_frontLimit.get()); - SmartDashboard.putNumber("Current angle Offset", Units.radiansToDegrees(m_curOffset)); - SmartDashboard.putNumber( - "Requested Angle", Units.radiansToDegrees(m_lastGoal) + Constants.ARMSTARTINGANGLE); - SmartDashboard.putNumber( - "Current Goal Angle", Units.radiansToDegrees(m_goal.position) + Constants.ARMSTARTINGANGLE); - SmartDashboard.putBoolean("At Goal", atGoal()); - } - - @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation - } -}