artificial soft limits

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()));

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();
         

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;
+		// m_disabled = true;
-		// PWMJNI.setPWMRaw(m_clawMotor.getHandle(), PWMJNI.getPWMRaw(m_clawMotor.getHandle()));
+		// // PWMJNI.setPWMRaw(m_clawMotor.getHandle(), PWMJNI.getPWMRaw(m_clawMotor.getHandle()));
-		PWMJNI.setPWMSpeed(m_clawMotor.getHandle(), 0.5);
+		// PWMJNI.setPWMSpeed(m_clawMotor.getHandle(), 0.5);
 		// PWMJNI.setPWMDisabled(m_clawMotor.getHandle());
 	}