From 03c938348fe2fa9dc3214be06e8bfb32eb4d7409 Mon Sep 17 00:00:00 2001 From: Abhishrek05 Date: Tue, 14 Mar 2023 14:26:47 -0600 Subject: [PATCH] artificial soft limits --- .../java/frc4388/robot/RobotContainer.java | 5 +-- .../java/frc4388/robot/subsystems/Arm.java | 32 ++++++++----------- .../java/frc4388/robot/subsystems/Claw.java | 10 +++--- 3 files changed, 22 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c8cffac..054a85b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -23,6 +23,7 @@ import frc4388.robot.subsystems.Arm; import frc4388.robot.subsystems.Claw; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.commands.JoystickRecorder; +import frc4388.robot.commands.PivotCommand; import frc4388.robot.commands.PlaybackChooser; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.utility.controller.XboxController; @@ -145,13 +146,13 @@ public class RobotContainer { new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - .whileTrue(new RunCommand(() -> m_robotArm.m_pivot.set(TalonFXControlMode.Position, 200), m_robotArm)); + .whileTrue(new RunCommand(() -> new PivotCommand(m_robotArm, 135))); new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) .whileTrue(new RunCommand(() -> m_robotArm.m_pivot.set(TalonFXControlMode.Position, 135), m_robotArm)); new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) - .whileTrue(new RunCommand(() -> m_robotArm.m_pivot.set(TalonFXControlMode.Position, 225), m_robotArm)); + .onTrue(new InstantCommand(() -> m_robotClaw.toggle())); new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits())); diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index b68e5df..e20958f 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -18,7 +18,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Arm extends SubsystemBase { private WPI_TalonFX m_tele; - public WPI_TalonFX m_pivot; + public WPI_TalonFX m_pivot; private CANCoder m_pivotEncoder; private boolean m_debug; @@ -32,7 +32,7 @@ public class Arm extends SubsystemBase { m_pivot.configFactoryDefault(); // * Example of deferred code - new DeferredBlock(() -> resetTeleSoftLimit()); + // new DeferredBlock(() -> resetTeleSoftLimit()); // TalonFXConfiguration pivotConfig = new TalonFXConfiguration(); // pivotConfig.slot0.kP = 0.5;//ArmConstants.kP; @@ -84,7 +84,17 @@ public class Arm extends SubsystemBase { } public void setRotVel(double vel) { - m_pivot.set(ControlMode.PercentOutput, .4 * vel); + var degrees = Math.abs(getArmRotation()) - 135; + SmartDashboard.putNumber("arm degrees", degrees); + SmartDashboard.putNumber("arm rot vel", vel); + + if ((degrees < 2 && vel < 0) || (degrees > 110 && vel > 0)) { + m_pivot.set(ControlMode.PercentOutput, 0); + } else if (degrees > 90 && vel > 0) { + m_pivot.set(ControlMode.PercentOutput, .2 * vel); + } else { + m_pivot.set(ControlMode.PercentOutput, .4 * vel); + } } public void setTeleVel(double vel) { @@ -167,21 +177,6 @@ public class Arm extends SubsystemBase { @Override public void periodic() { double degrees = Math.abs(m_pivotEncoder.getAbsolutePosition() - 135); - if (degrees < 2 && resetable) { - var pivot_soft = m_pivot.getSelectedSensorPosition(); - var tele_soft = m_tele.getSelectedSensorPosition(); - - SmartDashboard.putNumber("start pivot", pivot_soft); - SmartDashboard.putNumber("start tele", tele_soft); - - m_pivot.configForwardSoftLimitEnable(soft_limits); - m_pivot.configReverseSoftLimitEnable(soft_limits); - SmartDashboard.putNumber("fwd err", m_pivot.configForwardSoftLimitThreshold(1200 + pivot_soft).value); - SmartDashboard.putNumber("rvs err", m_pivot.configReverseSoftLimitThreshold(pivot_soft).value); - resetable = false; - } else if (degrees > 2) { - resetable = true; - } if (m_tele.isFwdLimitSwitchClosed() == 1 && tele_reset) { var tele_soft = m_tele.getSelectedSensorPosition(); @@ -207,6 +202,7 @@ public class Arm extends SubsystemBase { boolean soft_limits = true; public void killSoftLimits() { + resetTeleSoftLimit(); var pivot_soft = m_pivot.getSelectedSensorPosition(); var tele_soft = m_tele.getSelectedSensorPosition(); diff --git a/src/main/java/frc4388/robot/subsystems/Claw.java b/src/main/java/frc4388/robot/subsystems/Claw.java index 8daa6b5..c4d3bda 100644 --- a/src/main/java/frc4388/robot/subsystems/Claw.java +++ b/src/main/java/frc4388/robot/subsystems/Claw.java @@ -22,9 +22,9 @@ public class Claw extends SubsystemBase { System.out.println("setClaw()"); // m_clawMotor.setPosition(0.5); // m_clawMotor.setRaw(0); - // m_clawMotor.setRaw(m_open ? 0 : 255); + m_clawMotor.setRaw(m_open ? 1000 : 2000); // m_clawMotor.setSpeed(m_open ? -1 : 1); - PWMJNI.setPWMSpeed(m_clawMotor.getHandle(), m_open ? -1 : 1); + // PWMJNI.setPWMSpeed(m_clawMotor.getHandle(), m_open ? -1 : 1); // PWMJNI.setPWMDisabled(0); System.out.println("Claw Pos: " + m_clawMotor.getRaw()); } @@ -39,9 +39,9 @@ public class Claw extends SubsystemBase { } public void disable() { - m_disabled = true; - // PWMJNI.setPWMRaw(m_clawMotor.getHandle(), PWMJNI.getPWMRaw(m_clawMotor.getHandle())); - PWMJNI.setPWMSpeed(m_clawMotor.getHandle(), 0.5); + // m_disabled = true; + // // PWMJNI.setPWMRaw(m_clawMotor.getHandle(), PWMJNI.getPWMRaw(m_clawMotor.getHandle())); + // PWMJNI.setPWMSpeed(m_clawMotor.getHandle(), 0.5); // PWMJNI.setPWMDisabled(m_clawMotor.getHandle()); }