From 051ad1c9f06747f091b611daf2efc9bbebc9d232 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Wed, 8 Nov 2023 16:36:12 -0800 Subject: [PATCH] bye bye arm (#15) --- src/main/java/frc/robot/Constants.java | 13 +-- src/main/java/frc/robot/RobotContainer.java | 25 ----- .../java/frc/robot/commands/PlaceCommand.java | 50 ---------- .../robot/commands/arm/ArmDownCommand.java | 46 --------- .../frc/robot/commands/arm/ArmUpCommand.java | 45 --------- .../frc/robot/commands/arm/ClawCommand.java | 59 ------------ .../robot/subsystems/arm/ClawSubsystem.java | 72 -------------- .../subsystems/arm/ElevatorSubsystem.java | 93 ------------------- 8 files changed, 4 insertions(+), 399 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/PlaceCommand.java delete mode 100644 src/main/java/frc/robot/commands/arm/ArmDownCommand.java delete mode 100644 src/main/java/frc/robot/commands/arm/ArmUpCommand.java delete mode 100644 src/main/java/frc/robot/commands/arm/ClawCommand.java delete mode 100644 src/main/java/frc/robot/subsystems/arm/ClawSubsystem.java delete mode 100644 src/main/java/frc/robot/subsystems/arm/ElevatorSubsystem.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 200cc32..c1e442f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -19,9 +19,6 @@ public static final class CANConstants { public static final int MOTORBACKRIGHTID = 12; public static final int MOTORFRONTLEFTID = 13; public static final int MOTORBACKLEFTID = 14; - /// Arm movement motors - public static final int ARMELEVATORMOTORID = 22; - public static final int ARMCLAWMOTORID = 23; } public static final double MAX_SPEED = 0.8; @@ -30,11 +27,12 @@ public static final class CANConstants { public static final int CONTROLLERUSBINDEX = 0; public static final int FLIGHTSTICKUSBINDEX = 1; + // Game Controller Buttons + // Now In RobotContainer as Native Triggers. + // Joystick buttons public static final int AIMBUTTON = 12; - public static final int CLAWBUTTON = 1; - public static final int ARMDOWNBUTTON = 3; - public static final int ARMUPBUTTON = 4; + // Analog Ports /// Ultrasonic Sensors and ports. public static final int ULTRASONIC1PORT = 0; @@ -45,7 +43,4 @@ public static final class CANConstants { public static final int DRIVEENCODERLEFTB = 1; public static final int DRIVEENCODERRIGHTA = 2; public static final int DRIVEENCODERRIGHTB = 3; - /// Limit Switches - public static final int LSWITCHCLAWOPEN = 4; - public static final int LSWITCHCLAWCLOSE = 5; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dc2a4f6..63e29b8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,15 +20,9 @@ import frc.robot.commands.AimCommand; import frc.robot.commands.BalanceCommand; import frc.robot.commands.DefaultDrive; -import frc.robot.commands.PlaceCommand; import frc.robot.commands.StraightCommand; -import frc.robot.commands.arm.ArmDownCommand; -import frc.robot.commands.arm.ArmUpCommand; -import frc.robot.commands.arm.ClawCommand; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.UltrasonicSubsystem; -import frc.robot.subsystems.arm.ClawSubsystem; -import frc.robot.subsystems.arm.ElevatorSubsystem; import java.util.HashMap; import java.util.List; @@ -52,30 +46,20 @@ public class RobotContainer { new UltrasonicSubsystem(Constants.ULTRASONIC1PORT); private final DriveSubsystem m_driveSubsystem = new DriveSubsystem(); - private final ElevatorSubsystem m_elevatorSubsystem = new ElevatorSubsystem(); - private final ClawSubsystem m_clawSubsystem = new ClawSubsystem(); // The robots commands are defined here.. // private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem); private final AimCommand m_aimCommand = new AimCommand(m_driveSubsystem); private final BalanceCommand m_balanceCommand = new BalanceCommand(m_driveSubsystem); - private final PlaceCommand m_placeCommand = - new PlaceCommand(m_clawSubsystem, m_elevatorSubsystem); private final DefaultDrive m_defaultDrive = new DefaultDrive(m_driveSubsystem, this::getControllerLeftY, this::getControllerRightY); private final StraightCommand m_straightCommand = new StraightCommand(m_driveSubsystem, this::getControllerLeftY, this::getControllerRightY); - private final ClawCommand m_clawCommand = new ClawCommand(m_clawSubsystem, m_flightStick::getY); - private final ArmUpCommand m_armUpCommand = new ArmUpCommand(m_elevatorSubsystem); - private final ArmDownCommand m_armDownCommand = new ArmDownCommand(m_elevatorSubsystem); // misc init private Trigger m_switchCameraButton; private Trigger m_balanceButton; private Trigger m_straightButton; private JoystickButton m_aimButton; - private JoystickButton m_clawButton; - private JoystickButton m_armUpButton; - private JoystickButton m_armDownButton; // Init For Autonomous private RamseteAutoBuilder autoBuilder; private final HashMap autonomousEventMap = new HashMap(); @@ -91,8 +75,6 @@ public RobotContainer() { // set default drive command m_driveSubsystem.setDefaultCommand(m_defaultDrive); - // set claw default command - m_clawSubsystem.setDefaultCommand(m_clawCommand); } /** @@ -107,17 +89,11 @@ private void configureButtonBindings() { m_balanceButton = m_controller1.rightBumper(); m_straightButton = m_controller1.rightTrigger(); // Joystick buttons - m_clawButton = new JoystickButton(m_flightStick, Constants.CLAWBUTTON); m_aimButton = new JoystickButton(m_flightStick, Constants.AIMBUTTON); - m_armDownButton = new JoystickButton(m_flightStick, Constants.ARMDOWNBUTTON); - m_armUpButton = new JoystickButton(m_flightStick, Constants.ARMUPBUTTON); // commands m_balanceButton.whileTrue(m_balanceCommand); m_straightButton.whileTrue(m_straightCommand); m_aimButton.whileTrue(m_aimCommand); - m_clawButton.toggleOnTrue(m_clawCommand).toggleOnFalse(m_clawCommand); - m_armDownButton.whileTrue(m_armDownCommand); - m_armUpButton.whileTrue(m_armUpCommand); m_controller1.a().whileTrue(new InstantCommand(() -> m_driveSubsystem.SetBrakemode())); m_controller1.b().whileTrue(new InstantCommand(() -> m_driveSubsystem.SetCoastmode())); @@ -134,7 +110,6 @@ private void initializeAutonomous() { // ex: // autonomousEventMap.put("A", new PathFollowingCommand(m_driveSubsystem, pathGroup.get(0))); autonomousEventMap.put("BalanceRobot", m_balanceCommand); - autonomousEventMap.put("PlaceCube", m_placeCommand); // Create the AutoBuilder. This only needs to be created once when robot code starts, not every // time you want to create an auto command. diff --git a/src/main/java/frc/robot/commands/PlaceCommand.java b/src/main/java/frc/robot/commands/PlaceCommand.java deleted file mode 100644 index 2ea0682..0000000 --- a/src/main/java/frc/robot/commands/PlaceCommand.java +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright (c) Jack Nelson, Max Aitel & Miami Beach Bots - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.arm.ClawSubsystem; -import frc.robot.subsystems.arm.ElevatorSubsystem; - -/** The claw command that uses the claw subsystem. */ -public class PlaceCommand extends CommandBase { - private final ClawSubsystem m_clawSubsystem; - private final ElevatorSubsystem m_elevatorSubsystem; - /** - * Creates a new AutoCommand command. - * - * @param a_subsystem The claw subsystem used by this command. - * @param e_subsystem The Elevator subsystem. - */ - public PlaceCommand(ClawSubsystem a_subsystem, ElevatorSubsystem e_subsystem) { - m_clawSubsystem = a_subsystem; - m_elevatorSubsystem = e_subsystem; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(a_subsystem); - addRequirements(e_subsystem); - } - - // 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() { - // // random number - // while (!m_elevatorSubsystem.moveArm(5)) { - // continue; - // } - // m_clawSubsystem.clawOpen(); - } - - // 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/arm/ArmDownCommand.java b/src/main/java/frc/robot/commands/arm/ArmDownCommand.java deleted file mode 100644 index b63edc0..0000000 --- a/src/main/java/frc/robot/commands/arm/ArmDownCommand.java +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright (c) Jack Nelson & Miami Beach Bots - -package frc.robot.commands.arm; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.arm.ElevatorSubsystem; - -/** The Arm Down Command that uses the elevator subsystem */ -public class ArmDownCommand extends CommandBase { - private final ElevatorSubsystem m_elevatorSubsystem; - - /** - * Creates a new Arm Down command. - * - * @param e_subsystem The elevator subsystem used by this command. - */ - public ArmDownCommand(ElevatorSubsystem e_subsystem) { - m_elevatorSubsystem = e_subsystem; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(e_subsystem); - } - - // 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() { - System.out.println("DownCommand"); - m_elevatorSubsystem.armElevateBasic(-0.2); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - m_elevatorSubsystem.armStop(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - - return false; - } -} diff --git a/src/main/java/frc/robot/commands/arm/ArmUpCommand.java b/src/main/java/frc/robot/commands/arm/ArmUpCommand.java deleted file mode 100644 index 8e3ff78..0000000 --- a/src/main/java/frc/robot/commands/arm/ArmUpCommand.java +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright (c) Jack Nelson & Miami Beach Bots - -package frc.robot.commands.arm; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.arm.ElevatorSubsystem; - -/** The Arm Up Command that uses the elevator subsystem */ -public class ArmUpCommand extends CommandBase { - private final ElevatorSubsystem m_elevatorSubsystem; - - /** - * Creates a new Arm Up command. - * - * @param e_subsystem The elevator subsystem used by this command. - */ - public ArmUpCommand(ElevatorSubsystem e_subsystem) { - m_elevatorSubsystem = e_subsystem; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(e_subsystem); - } - - // 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() { - System.out.println("UpCommand"); - m_elevatorSubsystem.armElevateBasic(0.2); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - m_elevatorSubsystem.armStop(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/arm/ClawCommand.java b/src/main/java/frc/robot/commands/arm/ClawCommand.java deleted file mode 100644 index 47a8f31..0000000 --- a/src/main/java/frc/robot/commands/arm/ClawCommand.java +++ /dev/null @@ -1,59 +0,0 @@ -// Copyright (c) Jack Nelson, Max Aitel & Miami Beach Bots - -package frc.robot.commands.arm; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.arm.ClawSubsystem; -import java.util.function.DoubleSupplier; - -/** The claw command that uses the claw subsystem. */ -public class ClawCommand extends CommandBase { - private final ClawSubsystem m_clawSubsystem; - private final DoubleSupplier m_joy_y; - private boolean openClaw = true; - - /** - * Creates a new ClawCommand command. - * - * @param a_subsystem The claw subsystem used by this command. - * @param joystick_y_func A function that returns the value of the Y axis / height axis for said - * joystick. - */ - public ClawCommand(ClawSubsystem a_subsystem, DoubleSupplier joystick_y_func) { - m_joy_y = joystick_y_func; - m_clawSubsystem = a_subsystem; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(a_subsystem); - } - - // 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 (this.openClaw) { - // m_clawSubsystem.clawOpen(); - // } else { - // m_clawSubsystem.clawClose(); - // } - if (m_joy_y.getAsDouble() < 0.1 && m_joy_y.getAsDouble() > -0.1) { - m_clawSubsystem.clawMotor(0.05); - } else { - m_clawSubsystem.clawMotor(m_joy_y.getAsDouble()); - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - // m_clawSubsystem.clawMotor(0.0); // change claw action on end. - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/subsystems/arm/ClawSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ClawSubsystem.java deleted file mode 100644 index 4d97eae..0000000 --- a/src/main/java/frc/robot/subsystems/arm/ClawSubsystem.java +++ /dev/null @@ -1,72 +0,0 @@ -// Copyright (c) Max Aitel, Jack Nelson & Miami Beach Bots - -package frc.robot.subsystems.arm; - -import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; -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 frc.robot.Constants.CANConstants; - -public class ClawSubsystem extends SubsystemBase { - /** Creates a new ClawSubsystem. */ - // motors - private final WPI_VictorSPX m_clawMotor; - // limit switches - private final DigitalInput m_clawLimitSwitchOpen; - private final DigitalInput m_clawLimitSwitchClose; - // constants - private final double movementSpeed = 0.1; - private final double closeSpeed = 0.01; - - public ClawSubsystem() { - // claw go grabby grabby - m_clawMotor = new WPI_VictorSPX(CANConstants.ARMCLAWMOTORID); - m_clawMotor.setInverted(false); // invert if needed here. - // limit switches - m_clawLimitSwitchOpen = new DigitalInput(Constants.LSWITCHCLAWOPEN); - m_clawLimitSwitchClose = new DigitalInput(Constants.LSWITCHCLAWCLOSE); - } - - public boolean clawOpen() { - if (!m_clawLimitSwitchOpen.get()) { - m_clawMotor.set(-movementSpeed); - return false; - } else { - m_clawMotor.set(0.0); - return true; - } - } - - public void clawMotor(double motorSpeed) { - m_clawMotor.set(motorSpeed); - } - - public boolean clawClose() { - if (!m_clawLimitSwitchClose.get()) { - m_clawMotor.set(movementSpeed); - return false; - } else { - m_clawMotor.set(closeSpeed); - return true; - } - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - if (m_clawLimitSwitchOpen.get()) { - SmartDashboard.putBoolean("Claw Open", true); - } else if (m_clawLimitSwitchClose.get()) { - SmartDashboard.putBoolean("Claw Open", false); - } else { - System.out.println("Claw is moving between states"); - } - } - - @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation - } -} diff --git a/src/main/java/frc/robot/subsystems/arm/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ElevatorSubsystem.java deleted file mode 100644 index d3258db..0000000 --- a/src/main/java/frc/robot/subsystems/arm/ElevatorSubsystem.java +++ /dev/null @@ -1,93 +0,0 @@ -// Copyright (c) Max Aitel, Jack Nelson & Miami Beach Bots - -package frc.robot.subsystems.arm; - -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import com.revrobotics.RelativeEncoder; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.CANConstants; - -public class ElevatorSubsystem extends SubsystemBase { - /** Creates a new ElevatorSubsystem. */ - // elevator motor - private final CANSparkMax m_elevatorMotor; - // built in pid controller - // private final SparkMaxPIDController m_elevatorPidController; - // built in encoder - private final RelativeEncoder m_elevatorEncoder; - - // Elevator PIDF constants - private final double kP = 0; - private final double kI = 0; - private final double kD = 0; - private final double kIz = 0; - private final double kFF = 0; - // Other Speed Constants - private final double kMaxOutput = 1; - private final double kMinOutput = -1; - // Rotation Constants - private final float kmaxRotations = 0; - private final double rotationsPerCall = 5; - - public ElevatorSubsystem() { - // arm up/y motor - m_elevatorMotor = new CANSparkMax(CANConstants.ARMELEVATORMOTORID, MotorType.kBrushless); - // m_elevatorPidController = m_elevatorMotor.getPIDController(); - m_elevatorEncoder = m_elevatorMotor.getEncoder(); - // // configure elevator - // m_elevatorPidController.setP(kP); - // m_elevatorPidController.setI(kI); - // m_elevatorPidController.setD(kD); - // m_elevatorPidController.setOutputRange(kMinOutput, kMaxOutput); - // m_elevatorPidController.setIZone(kIz); - // m_elevatorPidController.setFF(kFF); - // m_elevatorMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kForward, true); - // m_elevatorMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, true); - // m_elevatorMotor.setSoftLimit(CANSparkMax.SoftLimitDirection.kForward, kmaxRotations); - // m_elevatorMotor.setSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, 0); - } - - // public void armElevate(boolean direction) { - // System.out.println("Arm is Elevating"); - // if (direction) { - // // move up - // moveArm(rotationsPerCall); - // } else { - // // move down - // moveArm(-rotationsPerCall); - // } - - public void armElevateBasic(double speed) { - m_elevatorMotor.set(speed); - } - - public void armStop() { - m_elevatorMotor.set(0); - } - - // public boolean moveArm(double rotations) { - // double curPos = m_elevatorEncoder.getPosition(); - // double newPos = curPos + rotations; - // if (newPos > kmaxRotations) { - // newPos = kmaxRotations; - // } else if (newPos < 0) { - // newPos = 0; - // } - // m_elevatorPidController.setReference(newPos, CANSparkMax.ControlType.kPosition); - // if (newPos <= kmaxRotations) { - // return true; - // } - // return false; - // } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } - - @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation - } -}