ef working, start of post-placing

FRC-4509-MechBulls · Oct 17, 2023 · d4cf6c7 · d4cf6c7
1 parent b12f711
commit d4cf6c7
Show file tree

Hide file tree

Showing 4 changed files with 51 additions and 36 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -160,9 +160,9 @@ public static class ArmConstants {
   //          0.047,0.046,0.043,0.042,0.04,0.0375,0.035,0.03,0.025,0.02,0.01,0};
   }
 
-  public static class EfConstants {
-    public static int EF_UPPER_PORT = 14;
-    public static int EF_LOWER_PORT = 13;
+  public static class EfConstants { //end effector motor ids
+    public static int EF_UPPER_PORT = 12;
+    public static int EF_LOWER_PORT = 14;
   }
 
 

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -14,6 +14,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.subsystems.EndEffectorSub;
 import frc.robot.subsystems.StateControllerSub;
 import frc.robot.subsystems.arm.Arm;
 import frc.robot.subsystems.arm.StageOneSub;
@@ -40,8 +41,9 @@ public class RobotContainer {
 
 
   Arm arm = new Arm();
+  EndEffectorSub endEffectorSub = new EndEffectorSub();
 
-  StateControllerSub stateController = new StateControllerSub(arm);
+  StateControllerSub stateController = new StateControllerSub(arm,endEffectorSub);
 
   Command stageOneToHolding = new InstantCommand(()->arm.setStageOneAngle(Rotation2d.fromDegrees(90)));
   Command stageTwoToHolding = new InstantCommand(()->arm.setStageTwoAngle(Rotation2d.fromDegrees(15)));

diff --git a/src/main/java/frc/robot/subsystems/EndEffectorSub.java b/src/main/java/frc/robot/subsystems/EndEffectorSub.java
@@ -10,31 +10,31 @@
 
 
 public class EndEffectorSub extends SubsystemBase {
-    private final TalonSRX endEffectorLower;
-    private final TalonSRX endEffectorUpper;
+    private final TalonSRX lower;
+    private final TalonSRX upper;
 
     public EndEffectorSub() {
-        endEffectorLower = new TalonSRX(Constants.EfConstants.EF_LOWER_PORT);
-        endEffectorUpper= new TalonSRX(Constants.EfConstants.EF_UPPER_PORT);
+        lower = new TalonSRX(Constants.EfConstants.EF_LOWER_PORT);
+        upper = new TalonSRX(Constants.EfConstants.EF_UPPER_PORT);
 
-        endEffectorUpper.configFactoryDefault();
-        endEffectorUpper.configFactoryDefault();
+        upper.configFactoryDefault();
+        upper.configFactoryDefault();
 
-        endEffectorUpper.setNeutralMode(NeutralMode.Coast);
-        endEffectorLower.setNeutralMode(NeutralMode.Coast);
+        upper.setNeutralMode(NeutralMode.Coast);
+        lower.setNeutralMode(NeutralMode.Coast);
 
 
-        endEffectorUpper.setInverted(true);
-        endEffectorLower.setInverted(false);
+        upper.setInverted(true);
+        lower.setInverted(false);
 
-        endEffectorLower.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(
+        lower.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(
                 true, // enabled
                 10, // Limit (amp)
                 10, // Trigger Threshold (amp)
                 0)); // Trigger Threshold Time(s)
 
 
-        endEffectorUpper.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(
+        upper.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(
                 true, // enabled
                 10, // Limit (amp)
                 10, // Trigger Threshold (amp)
@@ -44,45 +44,45 @@ public EndEffectorSub() {
     }
 
     public void intakeCone() {
-        endEffectorUpper.set(TalonSRXControlMode.PercentOutput, -1);
-        endEffectorLower.set(TalonSRXControlMode.PercentOutput, 1);
+        upper.set(TalonSRXControlMode.PercentOutput, -1);
+        lower.set(TalonSRXControlMode.PercentOutput, 1);
     }
 
     public void intakeCube() {
-        endEffectorUpper.set(TalonSRXControlMode.PercentOutput, 0);
-        endEffectorLower.set(TalonSRXControlMode.PercentOutput, -1);
+        upper.set(TalonSRXControlMode.PercentOutput, 0);
+        lower.set(TalonSRXControlMode.PercentOutput, -1);
     }
 
     public void holdCone() {
 
-        endEffectorUpper.set(TalonSRXControlMode.PercentOutput, -0.1);
-        endEffectorLower.set(TalonSRXControlMode.PercentOutput, 0.1);
+        upper.set(TalonSRXControlMode.PercentOutput, -0.1);
+        lower.set(TalonSRXControlMode.PercentOutput, 0.1);
     }
 
     public void holdCube() {
 
-        endEffectorUpper.set(TalonSRXControlMode.PercentOutput, 0);
-        endEffectorLower.set(TalonSRXControlMode.PercentOutput, 0);
+        upper.set(TalonSRXControlMode.PercentOutput, 0);
+        lower.set(TalonSRXControlMode.PercentOutput, 0);
     }
 
     public void placeCone() {
-        endEffectorUpper.set(TalonSRXControlMode.PercentOutput, 1);
-        endEffectorLower.set(TalonSRXControlMode.PercentOutput, -.1);
+        upper.set(TalonSRXControlMode.PercentOutput, 1);
+        lower.set(TalonSRXControlMode.PercentOutput, -.1);
     }
 
     public void placeCubeBottom() {
-        endEffectorUpper.set(TalonSRXControlMode.PercentOutput, 1);
-        endEffectorLower.set(TalonSRXControlMode.PercentOutput, -0.3);
+        upper.set(TalonSRXControlMode.PercentOutput, 1);
+        lower.set(TalonSRXControlMode.PercentOutput, -0.3);
     }
     public void placeCubeTop() {
 
-        endEffectorUpper.set(TalonSRXControlMode.PercentOutput, -1);
-        endEffectorLower.set(TalonSRXControlMode.PercentOutput, -0.3);
+        upper.set(TalonSRXControlMode.PercentOutput, -1);
+        lower.set(TalonSRXControlMode.PercentOutput, -0.3);
     }
 
     public void stopMotors() {
-        endEffectorUpper.set(TalonSRXControlMode.PercentOutput, 0);
-        endEffectorLower.set(TalonSRXControlMode.PercentOutput, 0);
+        upper.set(TalonSRXControlMode.PercentOutput, 0);
+        lower.set(TalonSRXControlMode.PercentOutput, 0);
     }
 
 }
diff --git a/src/main/java/frc/robot/subsystems/StateControllerSub.java b/src/main/java/frc/robot/subsystems/StateControllerSub.java
@@ -9,8 +9,9 @@
 public class StateControllerSub extends SubsystemBase {
 
     Arm arm;
+    EndEffectorSub ef;
 
-    public enum AgArmMode {PLACING,HOLDING,INTAKING};
+    public enum AgArmMode {PLACING,POST_PLACING,HOLDING,INTAKING};
     public enum ItemIsFallen {FALLEN_CONE,NOT_FALLEN};
     public enum ItemType {CUBE,CONE}
     public enum PlacementLevel {LEVEL1,LEVEL2,LEVEL3};
@@ -21,8 +22,9 @@ public enum PlacementLevel {LEVEL1,LEVEL2,LEVEL3};
 
 
 
-    public StateControllerSub(Arm arm) {
+    public StateControllerSub(Arm arm, EndEffectorSub ef) {
         this.arm = arm;
+        this.ef = ef;
     }
 
 
@@ -44,6 +46,10 @@ public void periodic(){
         if (oldState.armMode != desiredState.armMode) {
             switch (desiredState.armMode) {
                 case HOLDING:
+                    switch (desiredState.itemType){
+                        case CONE:ef.holdCone(); break;
+                        case CUBE:ef.holdCube(); break;
+                    }
                     switch (oldState.armMode) {
                         case PLACING:
                             if (desiredState.itemType == ItemType.CONE) {
@@ -102,13 +108,21 @@ public void periodic(){
                                     ArmCommands.intakeConeUpright(arm).schedule();
                                     break;
                             }
+                            ef.intakeCone();
                             terminate();
                         } else if (desiredState.itemType == ItemType.CUBE) {
                             // cube gangsta mode
                             // your logic here
                         }
                     }
                     break;
+
+                case POST_PLACING:
+                    switch (desiredState.itemType){
+                        case CONE: ef.placeCone(); break;
+                        case CUBE: break;
+                    }
+                    break;
             }
         }
 
@@ -155,8 +169,7 @@ public void setItemConeUpright() {
         desiredState.itemIsFallen = ItemIsFallen.NOT_FALLEN;
     }
 
-
-
+
     public void setItemCubeFallen(){
         desiredState.itemType = ItemType.CUBE;
         desiredState.itemIsFallen = ItemIsFallen.NOT_FALLEN;