From 818c3d7bfd71e64a635ffc1c23b4f36d850e8875 Mon Sep 17 00:00:00 2001 From: Mohnish <95386817+MussMan67@users.noreply.github.com> Date: Mon, 30 Oct 2023 18:49:55 -0500 Subject: [PATCH 1/3] Remove limitswitch --- src/main/java/frc/robot/Robot.java | 3 +- src/main/java/frc/robot/RobotContainer.java | 10 ++--- .../commands/extensionarm/BaseExtension.java | 7 +--- .../extensionarm/ExtensionExtend.java | 7 +--- ...eroExtensionArm.java => ZeroEncoders.java} | 37 +++++++------------ .../frc/robot/subsystems/ExtensionArm.java | 13 +------ 6 files changed, 24 insertions(+), 53 deletions(-) rename src/main/java/frc/robot/commands/extensionarm/{AutoZeroExtensionArm.java => ZeroEncoders.java} (56%) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1a8a3e5..0255aab 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -57,7 +57,6 @@ public void disabledPeriodic() {} @Override public void autonomousInit() { CommandScheduler.getInstance().cancelAll(); - CommandScheduler.getInstance().schedule(m_robotContainer.zeroCommand()); } /** This function is called periodically during autonomous. */ @@ -72,7 +71,9 @@ public void teleopInit() { // this line or comment it out. if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); + } + CommandScheduler.getInstance().schedule(m_robotContainer.zeroEncoders()); } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 027f8a9..6554419 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,21 +5,18 @@ package frc.robot; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj.Joystick; +import frc.robot.commands.extensionarm.*; import frc.robot.commands.manipulator.IntakeCone; import frc.robot.commands.manipulator.IntakeCube; import frc.robot.commands.manipulator.OuttakeCone; import frc.robot.commands.manipulator.OuttakeCube; -import frc.robot.commands.extensionarm.DumbExtend; -import frc.robot.commands.extensionarm.DumbRetract; -import frc.robot.commands.extensionarm.BaseExtension; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.commands.*; -import frc.robot.commands.extensionarm.AutoZeroExtensionArm; -import frc.robot.commands.extensionarm.ExtensionExtend; import frc.robot.commands.framework.Autos; import frc.robot.subsystems.Chassis; import frc.robot.subsystems.ExampleSubsystem; @@ -91,6 +88,9 @@ private void configureBindings() { public CommandBase zeroCommand() { return (new AutoZeroExtensionArm(m_extension)); } + public CommandBase zeroEncoders() { + return (new ZeroEncoders(m_extension)); + } /** * Use this to pass the autonomous command to the main {@link Robot} class. diff --git a/src/main/java/frc/robot/commands/extensionarm/BaseExtension.java b/src/main/java/frc/robot/commands/extensionarm/BaseExtension.java index 562ae62..a9a6fbb 100644 --- a/src/main/java/frc/robot/commands/extensionarm/BaseExtension.java +++ b/src/main/java/frc/robot/commands/extensionarm/BaseExtension.java @@ -22,12 +22,7 @@ public BaseExtension(ExtensionArm subsystem, RobotContainer container) { // Called when the command is initially scheduled. @Override - public void initialize() { - if(m_ExtensionArm.LimitSwitch()){ // if limit switch is hit, then zero encoders - m_ExtensionArm.resetEncoders(); - m_ExtensionArm.setZeroed(); - } - } + public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override diff --git a/src/main/java/frc/robot/commands/extensionarm/ExtensionExtend.java b/src/main/java/frc/robot/commands/extensionarm/ExtensionExtend.java index 281beac..4dd7ce4 100644 --- a/src/main/java/frc/robot/commands/extensionarm/ExtensionExtend.java +++ b/src/main/java/frc/robot/commands/extensionarm/ExtensionExtend.java @@ -28,12 +28,7 @@ public ExtensionExtend(ExtensionArm subsystem, RobotContainer container) { // Called when the command is initially scheduled. @Override - public void initialize() { - if(m_ExtensionArm.LimitSwitch()){ - m_ExtensionArm.resetEncoders(); - m_ExtensionArm.setZeroed(); - } - } + public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override diff --git a/src/main/java/frc/robot/commands/extensionarm/AutoZeroExtensionArm.java b/src/main/java/frc/robot/commands/extensionarm/ZeroEncoders.java similarity index 56% rename from src/main/java/frc/robot/commands/extensionarm/AutoZeroExtensionArm.java rename to src/main/java/frc/robot/commands/extensionarm/ZeroEncoders.java index 1db7d7f..cda2803 100644 --- a/src/main/java/frc/robot/commands/extensionarm/AutoZeroExtensionArm.java +++ b/src/main/java/frc/robot/commands/extensionarm/ZeroEncoders.java @@ -5,50 +5,39 @@ package frc.robot.commands.extensionarm; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.RobotContainer; import frc.robot.subsystems.ExtensionArm; /** An example command that uses an example subsystem. */ -public class AutoZeroExtensionArm extends CommandBase { +public class ZeroEncoders extends CommandBase { @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final ExtensionArm m_extensionArm; - - /** - * Creates a new ExampleCommand. - * - * @param subsystem The subsystem used by this command. - */ - public AutoZeroExtensionArm(ExtensionArm subsystem) { - m_extensionArm = subsystem; - // Use addRequirements() here to declare subsystem dependencies. + private final ExtensionArm m_ExtensionArm; + + public ZeroEncoders(ExtensionArm subsystem) { + m_ExtensionArm = subsystem; addRequirements(subsystem); } // Called when the command is initially scheduled. @Override public void initialize() { - + m_ExtensionArm.resetEncoders(); + m_ExtensionArm.setZeroed(); } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() { - m_extensionArm.runMotor(-.25); - } + public void execute() {} // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) { - m_extensionArm.stop(); - if (!interrupted) { - m_extensionArm.resetEncoders(); - } - m_extensionArm.setZeroed(); - } + public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return m_extensionArm.LimitSwitch(); + return false; } - } + + diff --git a/src/main/java/frc/robot/subsystems/ExtensionArm.java b/src/main/java/frc/robot/subsystems/ExtensionArm.java index 3bb5af2..81958b7 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionArm.java +++ b/src/main/java/frc/robot/subsystems/ExtensionArm.java @@ -23,9 +23,7 @@ public class ExtensionArm extends SubsystemBase { private final WPI_TalonSRX m_rightMotor; private final MotorControllerGroup m_extensionMotors; - - private final DigitalInput m_limitSwitch; - private boolean isZeroed = false; + private boolean isZeroed = true; /**Extension arm testing constants*/ public static double maxExtensionTicks = 100; // TODO public static double kExtensionDeadband = 0.05; //The % of max extension where it will slow down (works on both ends) @@ -42,8 +40,6 @@ public ExtensionArm() { m_leftMotor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); m_rightMotor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); - m_limitSwitch = new DigitalInput(CAN.limitSwitch); - m_rightMotor.configFactoryDefault(); m_leftMotor.configFactoryDefault(); @@ -63,10 +59,6 @@ public ExtensionArm() { } /**returns if the limit switch has been hit*/ - public boolean LimitSwitch(){ - return m_limitSwitch.get(); - } - public double getPosition(){ return m_leftMotor.getSelectedSensorPosition() * extensionTicksToArmDistance; } @@ -154,7 +146,7 @@ public double rawMotorSpeed(double y) { } public boolean allowedToMovePastEnds(double y) { - if (LimitSwitch() && (y <= 0)) { // if trying to move past the limit switch, return false + if ((getPosition() <= 0) && (y <= 0)) { // if trying to move past the limit switch, return false return false; } else if ((getPosition() >= maxExtensionTicks) && (y >= 0)) { // if trying to move past maxTicks, return false return false; @@ -218,7 +210,6 @@ public double getPercentage() { public void initSendable(SendableBuilder builder){ builder.addDoubleProperty("Position", this::getPosition, null); builder.addDoubleProperty("Dumb Speed", this::getDumbSpeed, this::setDumbSpeed); - builder.addBooleanProperty("Hit Limit Switch", this::LimitSwitch, null); builder.addDoubleProperty("CMax Extension Ticks", this::getMaxExtensionTicks, this::setMaxExtensionTicks); builder.addDoubleProperty("CExtension Deadband", this::getkExtensionDeadband, this::setkExtensionDeadband); builder.addDoubleProperty("CSlow Zone Extension Percentage", this::getPercentage, this::setSlowExtensionEndsDistance); From 3ef63589b6c14b0754b0abe8962ca33581738b0b Mon Sep 17 00:00:00 2001 From: Albert Wright Date: Mon, 30 Oct 2023 18:53:45 -0500 Subject: [PATCH 2/3] changed things a bit, just look at the commit log --- src/main/java/frc/robot/RobotContainer.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6554419..45a0530 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -73,7 +73,6 @@ public Manipulator getManipulator() { */ private void configureBindings() { - new JoystickButton(m_WeaponsGamepad, 1).whileTrue(new AutoZeroExtensionArm(m_extension)); new POVButton(m_weaponsGamepad, Constants.XBOXButtons.LST_POV_N).whileTrue(new DumbExtend(m_extension, this)); new POVButton(m_weaponsGamepad, Constants.XBOXButtons.LST_POV_S).whileTrue(new DumbRetract(m_extension, this)); new JoystickButton(m_weaponsGamepad, 3).whileTrue(new IntakeCone(getManipulator())); @@ -86,7 +85,7 @@ private void configureBindings() { * Schedules a command to zero the extension arm */ public CommandBase zeroCommand() { - return (new AutoZeroExtensionArm(m_extension)); + return null; } public CommandBase zeroEncoders() { return (new ZeroEncoders(m_extension)); From 09e314dab71f936545c75d609841f1dad1c9ef78 Mon Sep 17 00:00:00 2001 From: Mohnish <95386817+MussMan67@users.noreply.github.com> Date: Mon, 30 Oct 2023 18:49:55 -0500 Subject: [PATCH 3/3] Remove limitswitch --- src/main/java/frc/robot/Robot.java | 3 +- src/main/java/frc/robot/RobotContainer.java | 12 ++---- .../commands/extensionarm/BaseExtension.java | 7 +--- .../extensionarm/ExtensionExtend.java | 7 +--- ...eroExtensionArm.java => ZeroEncoders.java} | 37 +++++++------------ .../frc/robot/subsystems/ExtensionArm.java | 13 +------ 6 files changed, 23 insertions(+), 56 deletions(-) rename src/main/java/frc/robot/commands/extensionarm/{AutoZeroExtensionArm.java => ZeroEncoders.java} (56%) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1a8a3e5..0255aab 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -57,7 +57,6 @@ public void disabledPeriodic() {} @Override public void autonomousInit() { CommandScheduler.getInstance().cancelAll(); - CommandScheduler.getInstance().schedule(m_robotContainer.zeroCommand()); } /** This function is called periodically during autonomous. */ @@ -72,7 +71,9 @@ public void teleopInit() { // this line or comment it out. if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); + } + CommandScheduler.getInstance().schedule(m_robotContainer.zeroEncoders()); } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 027f8a9..7945674 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,21 +5,18 @@ package frc.robot; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj.Joystick; +import frc.robot.commands.extensionarm.*; import frc.robot.commands.manipulator.IntakeCone; import frc.robot.commands.manipulator.IntakeCube; import frc.robot.commands.manipulator.OuttakeCone; import frc.robot.commands.manipulator.OuttakeCube; -import frc.robot.commands.extensionarm.DumbExtend; -import frc.robot.commands.extensionarm.DumbRetract; -import frc.robot.commands.extensionarm.BaseExtension; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.commands.*; -import frc.robot.commands.extensionarm.AutoZeroExtensionArm; -import frc.robot.commands.extensionarm.ExtensionExtend; import frc.robot.commands.framework.Autos; import frc.robot.subsystems.Chassis; import frc.robot.subsystems.ExampleSubsystem; @@ -76,7 +73,6 @@ public Manipulator getManipulator() { */ private void configureBindings() { - new JoystickButton(m_WeaponsGamepad, 1).whileTrue(new AutoZeroExtensionArm(m_extension)); new POVButton(m_weaponsGamepad, Constants.XBOXButtons.LST_POV_N).whileTrue(new DumbExtend(m_extension, this)); new POVButton(m_weaponsGamepad, Constants.XBOXButtons.LST_POV_S).whileTrue(new DumbRetract(m_extension, this)); new JoystickButton(m_weaponsGamepad, 3).whileTrue(new IntakeCone(getManipulator())); @@ -88,8 +84,8 @@ private void configureBindings() { /** * Schedules a command to zero the extension arm */ - public CommandBase zeroCommand() { - return (new AutoZeroExtensionArm(m_extension)); + public CommandBase zeroEncoders() { + return (new ZeroEncoders(m_extension)); } /** diff --git a/src/main/java/frc/robot/commands/extensionarm/BaseExtension.java b/src/main/java/frc/robot/commands/extensionarm/BaseExtension.java index 562ae62..a9a6fbb 100644 --- a/src/main/java/frc/robot/commands/extensionarm/BaseExtension.java +++ b/src/main/java/frc/robot/commands/extensionarm/BaseExtension.java @@ -22,12 +22,7 @@ public BaseExtension(ExtensionArm subsystem, RobotContainer container) { // Called when the command is initially scheduled. @Override - public void initialize() { - if(m_ExtensionArm.LimitSwitch()){ // if limit switch is hit, then zero encoders - m_ExtensionArm.resetEncoders(); - m_ExtensionArm.setZeroed(); - } - } + public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override diff --git a/src/main/java/frc/robot/commands/extensionarm/ExtensionExtend.java b/src/main/java/frc/robot/commands/extensionarm/ExtensionExtend.java index 281beac..4dd7ce4 100644 --- a/src/main/java/frc/robot/commands/extensionarm/ExtensionExtend.java +++ b/src/main/java/frc/robot/commands/extensionarm/ExtensionExtend.java @@ -28,12 +28,7 @@ public ExtensionExtend(ExtensionArm subsystem, RobotContainer container) { // Called when the command is initially scheduled. @Override - public void initialize() { - if(m_ExtensionArm.LimitSwitch()){ - m_ExtensionArm.resetEncoders(); - m_ExtensionArm.setZeroed(); - } - } + public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override diff --git a/src/main/java/frc/robot/commands/extensionarm/AutoZeroExtensionArm.java b/src/main/java/frc/robot/commands/extensionarm/ZeroEncoders.java similarity index 56% rename from src/main/java/frc/robot/commands/extensionarm/AutoZeroExtensionArm.java rename to src/main/java/frc/robot/commands/extensionarm/ZeroEncoders.java index 1db7d7f..cda2803 100644 --- a/src/main/java/frc/robot/commands/extensionarm/AutoZeroExtensionArm.java +++ b/src/main/java/frc/robot/commands/extensionarm/ZeroEncoders.java @@ -5,50 +5,39 @@ package frc.robot.commands.extensionarm; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.RobotContainer; import frc.robot.subsystems.ExtensionArm; /** An example command that uses an example subsystem. */ -public class AutoZeroExtensionArm extends CommandBase { +public class ZeroEncoders extends CommandBase { @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final ExtensionArm m_extensionArm; - - /** - * Creates a new ExampleCommand. - * - * @param subsystem The subsystem used by this command. - */ - public AutoZeroExtensionArm(ExtensionArm subsystem) { - m_extensionArm = subsystem; - // Use addRequirements() here to declare subsystem dependencies. + private final ExtensionArm m_ExtensionArm; + + public ZeroEncoders(ExtensionArm subsystem) { + m_ExtensionArm = subsystem; addRequirements(subsystem); } // Called when the command is initially scheduled. @Override public void initialize() { - + m_ExtensionArm.resetEncoders(); + m_ExtensionArm.setZeroed(); } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() { - m_extensionArm.runMotor(-.25); - } + public void execute() {} // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) { - m_extensionArm.stop(); - if (!interrupted) { - m_extensionArm.resetEncoders(); - } - m_extensionArm.setZeroed(); - } + public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return m_extensionArm.LimitSwitch(); + return false; } - } + + diff --git a/src/main/java/frc/robot/subsystems/ExtensionArm.java b/src/main/java/frc/robot/subsystems/ExtensionArm.java index 3bb5af2..81958b7 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionArm.java +++ b/src/main/java/frc/robot/subsystems/ExtensionArm.java @@ -23,9 +23,7 @@ public class ExtensionArm extends SubsystemBase { private final WPI_TalonSRX m_rightMotor; private final MotorControllerGroup m_extensionMotors; - - private final DigitalInput m_limitSwitch; - private boolean isZeroed = false; + private boolean isZeroed = true; /**Extension arm testing constants*/ public static double maxExtensionTicks = 100; // TODO public static double kExtensionDeadband = 0.05; //The % of max extension where it will slow down (works on both ends) @@ -42,8 +40,6 @@ public ExtensionArm() { m_leftMotor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); m_rightMotor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); - m_limitSwitch = new DigitalInput(CAN.limitSwitch); - m_rightMotor.configFactoryDefault(); m_leftMotor.configFactoryDefault(); @@ -63,10 +59,6 @@ public ExtensionArm() { } /**returns if the limit switch has been hit*/ - public boolean LimitSwitch(){ - return m_limitSwitch.get(); - } - public double getPosition(){ return m_leftMotor.getSelectedSensorPosition() * extensionTicksToArmDistance; } @@ -154,7 +146,7 @@ public double rawMotorSpeed(double y) { } public boolean allowedToMovePastEnds(double y) { - if (LimitSwitch() && (y <= 0)) { // if trying to move past the limit switch, return false + if ((getPosition() <= 0) && (y <= 0)) { // if trying to move past the limit switch, return false return false; } else if ((getPosition() >= maxExtensionTicks) && (y >= 0)) { // if trying to move past maxTicks, return false return false; @@ -218,7 +210,6 @@ public double getPercentage() { public void initSendable(SendableBuilder builder){ builder.addDoubleProperty("Position", this::getPosition, null); builder.addDoubleProperty("Dumb Speed", this::getDumbSpeed, this::setDumbSpeed); - builder.addBooleanProperty("Hit Limit Switch", this::LimitSwitch, null); builder.addDoubleProperty("CMax Extension Ticks", this::getMaxExtensionTicks, this::setMaxExtensionTicks); builder.addDoubleProperty("CExtension Deadband", this::getkExtensionDeadband, this::setkExtensionDeadband); builder.addDoubleProperty("CSlow Zone Extension Percentage", this::getPercentage, this::setSlowExtensionEndsDistance);