added flapsubsystem

vikings204 · Mar 12, 2024 · ab191d2 · ab191d2
1 parent 1d76ee3
commit ab191d2
Show file tree

Hide file tree

Showing 3 changed files with 74 additions and 0 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -2,6 +2,8 @@
 
 import edu.wpi.first.math.geometry.*;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.wpilibj.Servo;
+
 import com.revrobotics.CANSparkBase.IdleMode;
 
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
@@ -182,6 +184,12 @@ public static final class Vision {
         public static final Translation2d SPEAKER_RED = new Translation2d(inchesToMeters(652.73) - TARGET_OFFSET, inchesToMeters(218.42));
         public static final Translation2d SPEAKER_BLUE = new Translation2d(TARGET_OFFSET, inchesToMeters(218.42));
     }
+    public static final class Flap{
+        public static final int FLAP_PWM_CHANNEL =1;
+        public static final double AMP_POSITION = 0.5;
+        public static final double SOURCE_POSITION = 0.75;
+         public static final double CLOSED_POSITION = 0.0;
+    }
 }
 
 

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -26,6 +26,7 @@ public class RobotContainer {
     public final ShooterSubsystem Shooter = new ShooterSubsystem(LED);
     public final LinearActuatorSubsystem LinearActuator = new LinearActuatorSubsystem();
     public final PoseEstimationSubsystem PoseEstimation = new PoseEstimationSubsystem(Swerve::getYaw, Swerve::getPositions);
+    public final FlapSubSystem Flap = new FlapSubSystem();
 
     private final ShootSpeakerCommand ShootSpeakerCMD = new ShootSpeakerCommand(Swerve, PoseEstimation, Shooter);
     private final ShootSpeakerPoselessCommand ShootSpeakerPoselessCMD = new ShootSpeakerPoselessCommand(Swerve, PoseEstimation, Shooter);
@@ -122,6 +123,8 @@ private void configureButtonBindings() { // MOVE POOP FUNCTION TO ANOTHER BUTTON
 
         //new JoystickButton(OPERATOR, 1).whileTrue(ShootSpeakerCMD);
         new JoystickButton(OPERATOR, 1).whileTrue(ShootSpeakerPoselessCMD);
+
+        new JoystickButton(OPERATOR, 3).onTrue(new RunCommand(()->Flap.setFlapSource(true)));
     }
 
     public Command getAutonomousCommand() {

diff --git a/src/main/java/frc/robot/subsystems/FlapSubSystem.java b/src/main/java/frc/robot/subsystems/FlapSubSystem.java
@@ -0,0 +1,63 @@
+package frc.robot.subsystems;
+
+//import com.revrobotics.*;
+import edu.wpi.first.wpilibj.Servo;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.Flap;
+public class FlapSubSystem extends SubsystemBase {
+    private final Servo flapServo = new Servo(Flap.FLAP_PWM_CHANNEL);
+  boolean open;
+  boolean amp;
+  boolean source;
+
+    public FlapSubSystem(){
+           open = false;
+           amp = false;
+           source = false;
+    }
+
+    public void setFlapSource(boolean actuate){
+        if (actuate&& open == false){
+            flapServo.set(Flap.SOURCE_POSITION);
+            open = true;
+            source = true;
+            amp = false;
+        }
+        else if (actuate && open == true && amp == true){
+            flapServo.set(Flap.SOURCE_POSITION);
+            source = true;
+            amp = false;
+        }
+        else if (actuate && open == true && source == true){
+            flapServo.set(Flap.CLOSED_POSITION);
+            source = false;
+            open = false;
+            amp = false;
+        }
+        else{}
+
+    }
+        public void setFlapAmp(boolean actuate){
+        if (actuate&& open == false){
+            flapServo.set(Flap.AMP_POSITION);
+            open = true;
+            source = false;
+            amp = true;
+        }
+        else if (actuate && open == true && source == true){
+            flapServo.set(Flap.AMP_POSITION);
+            source = false;
+            amp = true;
+        }
+        else if (actuate && open == true && amp == true){
+            flapServo.set(Flap.CLOSED_POSITION);
+            source = false;
+            open = false;
+            amp = false;
+        }
+        else{}
+
+    }
+
+
+}