brake enable toggle, different gain outside of braking range

Author: IsaacThoman

src/main/java/frc/robot/Constants.java

@@ -72,10 +72,24 @@ public static class EFCConstants{
 
   public static class ArmConstants{
 
-    public static final double armkP = 0.8;
+    public static final double armMaxPower = 0.6;
+
+    public static final double armkP = 0.6;
     public static final double armkI = 0.1;
     public static final double armkD = 0.0;
-    public static final double armIZone = 0.15;
+
+
+    public static final double armkPBrakeless = 0.3;
+    public static final double armkIBrakeless = 0.1;
+    public static final double armkDBrakeless = 0.0;
+
+    public static final double brakeMinAngle = Units.degreesToRadians(-5);
+    public static final double brakeMaxAngle = Units.degreesToRadians(50);
+
+
+    public static final double armIZone = Units.degreesToRadians(10);
+
+    public static final double brakeEngageError = Units.degreesToRadians(5);
 
 
     public static final double armGearRatio = (40.0/14) * 80.0;

src/main/java/frc/robot/RobotContainer.java

@@ -88,7 +88,7 @@ private void configureBindings() {
     operator.povUp().onTrue(new InstantCommand(stateController::sourcePressed));
     operator.povRight().onTrue(new InstantCommand(stateController::ampPressed));
     operator.povDown().onTrue(new InstantCommand(stateController::speakerPressed));
-    operator.povLeft().onTrue(new InstantCommand(stateController::trapPressed));
+    //operator.povLeft().onTrue(new InstantCommand(stateController::trapPressed));
 
    // operator.leftTrigger(0.5).onTrue(new InstantCommand(stateController::shootPressed));
     driver.leftTrigger(0.5).onTrue(new InstantCommand(stateController::shootPressed));

src/main/java/frc/robot/StateControllerSub.java

@@ -310,6 +310,7 @@ public boolean alignWhenClose() {
         if(armState == ArmState.INTAKE){
             return noteAlignAngleDiff;
         }
+
         return MBUtils.angleDiffRad(angleToObjective(objective),robotPose.getRotation().getRadians());
     }
     double noteAlignAngleDiff = 0;

src/main/java/frc/robot/subsystems/ArmSubsystem.java

@@ -34,7 +34,7 @@ public ArmSubsystem(StateControllerSub stateControllerSub, PneumaticControlSub p
       //  armDutyCycle.setDistancePerRotation(Math.PI);
 
         pidController.setIZone(armIZone);
-        pidController.setIntegratorRange(0,0.1);
+        pidController.setIntegratorRange(-0.15,0.15);
 
 
         this.stateControllerSub = stateControllerSub;
@@ -111,9 +111,7 @@ public ArmSubsystem(StateControllerSub stateControllerSub, PneumaticControlSub p
 
     }
 
-    double lastP = armkP;
-    double lastI = armkI;
-    double lastD = armkD;
+
 
     double lastZeroingTimestamp = 0;
 
@@ -167,25 +165,39 @@ public void periodic() {
         SmartDashboard.putNumber("armError",getArmError());
 
 
-        double currentP = SmartDashboard.getNumber("armTuningP",0);
-        double currentI = SmartDashboard.getNumber("armTuningI",0);
-        double currentD = SmartDashboard.getNumber("armTuningD",0);
-        if((currentP!=lastP || currentI!=lastI || currentD !=lastD)){ // && stateControllerSub.tuningMode()
-            lastP = currentP;
-            lastI = currentI;
-            lastD = currentD;
 
-            armMaster.getPIDController().setP(currentP);
-            armMaster.getPIDController().setI(currentI);
-            armMaster.getPIDController().setD(currentD);
+        if((stateControllerSub.tuningMode())){ //
+
+            double currentP = SmartDashboard.getNumber("armTuningP",0);
+            double currentI = SmartDashboard.getNumber("armTuningI",0);
+            double currentD = SmartDashboard.getNumber("armTuningD",0);
+
+        //    armMaster.getPIDController().setP(currentP);
+        //    armMaster.getPIDController().setI(currentI);
+        //    armMaster.getPIDController().setD(currentD);
 
             pidController.setP(currentP);
             pidController.setI(currentI);
             pidController.setD(currentD);
 
+        }else{
+
+            if(inRangeOfBrake()){
+                pidController.setP(armkP);
+                pidController.setI(armkI);
+                pidController.setD(armkD);
+
+            }else{
+                pidController.setP(armkPBrakeless);
+                pidController.setI(armkIBrakeless);
+                pidController.setD(armkDBrakeless);
+
+            }
+
+
         }
 
-        double pidOut = MBUtils.clamp(pidController.calculate(getRIODutyCycleRad()),0.5);
+        double pidOut = MBUtils.clamp(pidController.calculate(getRIODutyCycleRad()),armMaxPower);
 
         if(armDutyCycle.isConnected())
             armMaster.set(pidOut);
@@ -199,7 +211,11 @@ public void periodic() {
     //    SmartDashboard.putNumber("armRIO-PWM rad",getRIODutyCycleRad());
     //    SmartDashboard.putNumber("armRIO-PWM raw",armDutyCycle.getAbsolutePosition());
 
-        pneumaticControlSub.setBrakeSolenoid(Math.abs(getArmError())<0.05 && getArmAngle() > 0&& getArmAngle() < Units.degreesToRadians(50) );
+        pneumaticControlSub.setBrakeSolenoid(Math.abs(getArmError())<brakeEngageError &&  inRangeOfBrake());
+    }
+
+    public boolean inRangeOfBrake(){
+        return getArmAngle() > brakeMinAngle&& getArmAngle() < brakeMaxAngle;
     }
 
     public double getArmError(){

src/main/java/frc/robot/subsystems/PneumaticControlSub.java

@@ -17,9 +17,9 @@ public PneumaticControlSub(){
         SmartDashboard.putBoolean("enableCompressor",false);
 
         climbSolenoid = pcm.makeSolenoid(6);
-        brakeSolenoid = pcm.makeSolenoid(7);
+        brakeSolenoid = pcm.makeSolenoid(5);
 
-       SmartDashboard.putBoolean("setBrakeSolenoid",false);
+       SmartDashboard.putBoolean("enableBrakes",true);
 
 
     }
@@ -49,7 +49,9 @@ public void setClimbSolenoid(boolean state){
     }
 
     public void setBrakeSolenoid(boolean state){
-        SmartDashboard.putBoolean("brakeSolenoid",state);
+        SmartDashboard.putBoolean("brakeSolenoidClosed",state);
+        if(!SmartDashboard.getBoolean("enableBrakes",true))
+            state = false;
         brakeSolenoid.set(state);
     }