From ab191d2fef2aa2919c9f36b65a77bf21bc7812e6 Mon Sep 17 00:00:00 2001
From: Joe Coyne <jcoyne@eccrsd.us>
Date: Tue, 12 Mar 2024 13:59:04 -0400
Subject: [PATCH] added flapsubsystem

---
 src/main/java/frc/robot/Constants.java        |  8 +++
 src/main/java/frc/robot/RobotContainer.java   |  3 +
 .../frc/robot/subsystems/FlapSubSystem.java   | 63 +++++++++++++++++++
 3 files changed, 74 insertions(+)
 create mode 100644 src/main/java/frc/robot/subsystems/FlapSubSystem.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index cb4ca65..e07ce63 100644
--- 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
index 1fa0f83..2a679d6 100644
--- 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
new file mode 100644
index 0000000..3d21dc4
--- /dev/null
+++ 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{}
+
+    }
+
+
+}