From 35fc498d756afe37a98bf75f2d1f9aaae9659040 Mon Sep 17 00:00:00 2001
From: Primo4586 <primo4586@gmail.com>
Date: Tue, 16 Jan 2024 22:28:51 +0200
Subject: [PATCH 01/10] intake arm subsystem

---
 src/main/java/frc/robot/Constants.java        | 23 ++++-
 .../robot/subsystems/IntakeArmSubsystem.java  | 88 +++++++++++++++++++
 2 files changed, 110 insertions(+), 1 deletion(-)
 create mode 100644 src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 7432df0..1cecf44 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -28,7 +28,28 @@
 
 public final class Constants {
     public static final double stickDeadband = 0.1;
-
+    
+    public static final class IntakeArm{ //TODO: This must be tuned to specific robot
+        // mm
+        public static final int KMotorID = 0;
+        public static final double CollectMotorSpeed = 0.5;
+        public static final double mmVelocity = 5.0;
+        public static final double mmAcceleration = 10.0;
+        public static final double mmJerk = 50;
+        public static final double KP = 24.0;
+        public static final double KD = 0.1;
+        public static final double KV = 0.12;
+        public static final double KS = 0.25;
+        public static final double PeakForwardVoltage = 11.5;
+        public static final double PeakReverseVoltage = -11.5;
+        public static final double SensorToMechanismRatio = 0;
+        public static final boolean ForwardSoftLimitEnable = true;
+        public static final double ForwardSoftLimitThreshold = 300;
+        public static final boolean ReverseSoftLimitEnable = true;
+        public static final double RevesrseSoftLimitThreshold = 0;  
+        // not mm
+        
+    }
     public static final class Swerve {
         public static final int pigeonID = 10;
         public static final boolean invertGyro = false; // Always ensure Gyro is CCW+ CW-
diff --git a/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java
new file mode 100644
index 0000000..2d09cf2
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java
@@ -0,0 +1,88 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.configs.MotionMagicConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+
+import static frc.robot.Constants.IntakeArm.*;
+
+public class IntakeArmSubsystem extends SubsystemBase {
+  private final CommandXboxController driver = new CommandXboxController(0);
+  private TalonFX m_IntakeArmMotor;
+  private final MotionMagicVoltage motionMagic = new MotionMagicVoltage(0);
+
+  // singleton
+  private static IntakeArmSubsystem instance;
+
+  public static IntakeArmSubsystem getInstance() {
+    if (instance == null) {
+      instance = new IntakeArmSubsystem();
+    }
+    return instance;
+  }
+  /** Creates a new IntakeArmSubsystem. */
+  public IntakeArmSubsystem() {
+    this.m_IntakeArmMotor = new TalonFX(KMotorID);
+    TalonFXConfiguration configs = new TalonFXConfiguration();
+    MotionMagicConfigs mm = new MotionMagicConfigs();
+
+    mm.MotionMagicCruiseVelocity = mmVelocity;
+    mm.MotionMagicAcceleration = mmAcceleration;
+    mm.MotionMagicJerk = mmJerk;
+    configs.MotionMagic = mm;
+
+    configs.Slot0.kP = KP;
+    configs.Slot0.kD = KD;
+    configs.Slot0.kV = KV;
+    configs.Slot0.kS = KS;
+
+    configs.Voltage.PeakForwardVoltage = PeakForwardVoltage;
+    configs.Voltage.PeakReverseVoltage = PeakReverseVoltage;
+    configs.Feedback.SensorToMechanismRatio = SensorToMechanismRatio;
+
+    configs.SoftwareLimitSwitch.ForwardSoftLimitEnable = ForwardSoftLimitEnable;
+    configs.SoftwareLimitSwitch.ForwardSoftLimitThreshold = ForwardSoftLimitThreshold;
+    configs.SoftwareLimitSwitch.ReverseSoftLimitEnable = ReverseSoftLimitEnable;
+    configs.SoftwareLimitSwitch.ReverseSoftLimitThreshold = RevesrseSoftLimitThreshold;
+
+    // gives code to TalonFX
+    StatusCode status = StatusCode.StatusCodeNotInitialized;
+    for (int i = 0; i < 5; i++) {
+      status = m_IntakeArmMotor.getConfigurator().apply(configs);
+      if (status.isOK()) {
+        break;
+      }
+    }
+    if (!status.isOK()) {
+      System.out.println("Turret could not apply configs, error code: " + status.toString());
+    }
+
+    // make sure we start at 0.
+    m_IntakeArmMotor.setPosition(0);
+  }
+
+  // moving the arm
+  public void moveArmTo(double degrees){
+      m_IntakeArmMotor.setControl(motionMagic.withPosition(degrees));
+  }
+  // set incoder idk
+  public void setIncodre (double incoder){
+    m_IntakeArmMotor.setPosition(incoder);
+  }
+  public boolean getSwitch(){
+    return !driver.rightBumper().getAsBoolean(); // TODO change it from rightBumper to idk what 
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

From ac56fc7d0a4f893367515e1ee7d57974d77232cd Mon Sep 17 00:00:00 2001
From: Eilon <eilonhuppert@gmail.com>
Date: Wed, 17 Jan 2024 10:00:29 +0200
Subject: [PATCH 02/10] commands without zero intake

---
 src/main/java/frc/robot/Constants.java        |  1 +
 .../MoveIntakeArmToDegree.java                | 39 +++++++++++++++++
 .../commands/IntakeArmCommands/SetSpeed.java  | 42 +++++++++++++++++++
 .../robot/subsystems/IntakeArmSubsystem.java  | 10 +++++
 4 files changed, 92 insertions(+)
 create mode 100644 src/main/java/frc/robot/commands/IntakeArmCommands/MoveIntakeArmToDegree.java
 create mode 100644 src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 1cecf44..6f2ca05 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -48,6 +48,7 @@ public static final class IntakeArm{ //TODO: This must be tuned to specific robo
         public static final boolean ReverseSoftLimitEnable = true;
         public static final double RevesrseSoftLimitThreshold = 0;  
         // not mm
+        public static final double minimumError = 0;
         
     }
     public static final class Swerve {
diff --git a/src/main/java/frc/robot/commands/IntakeArmCommands/MoveIntakeArmToDegree.java b/src/main/java/frc/robot/commands/IntakeArmCommands/MoveIntakeArmToDegree.java
new file mode 100644
index 0000000..75614c2
--- /dev/null
+++ b/src/main/java/frc/robot/commands/IntakeArmCommands/MoveIntakeArmToDegree.java
@@ -0,0 +1,39 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.IntakeArmCommands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.IntakeArmSubsystem;
+
+public class MoveIntakeArmToDegree extends Command {
+  /** Creates a new MoveIntakeArmTo. */
+  private final IntakeArmSubsystem intakeArm = IntakeArmSubsystem.getInstance();
+  private double degree;
+
+  public MoveIntakeArmToDegree(double degree) {
+    this.degree = degree;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    intakeArm.moveArmTo(degree);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return intakeArm.checkIntakeArmPosion();
+  }
+}
diff --git a/src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java b/src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java
new file mode 100644
index 0000000..25f7f95
--- /dev/null
+++ b/src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java
@@ -0,0 +1,42 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.IntakeArmCommands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.IntakeArmSubsystem;
+
+public class SetSpeed extends Command {
+  /** Creates a new SetSpeed. */
+  private final IntakeArmSubsystem intakeArm = IntakeArmSubsystem.getInstance();
+  private double speed;
+  public SetSpeed(double speed) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.speed = speed;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    IntakeArmSubsystem.getInstance();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    intakeArm.setSpeed(speed);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    intakeArm.setSpeed(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return intakeArm.getSwitch();
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java
index 2d09cf2..4eb0748 100644
--- a/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java
@@ -4,6 +4,7 @@
 
 package frc.robot.subsystems;
 
+import com.ctre.phoenix.motorcontrol.TalonSRXControlMode;
 import com.ctre.phoenix6.StatusCode;
 import com.ctre.phoenix6.configs.MotionMagicConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
@@ -73,13 +74,22 @@ public IntakeArmSubsystem() {
   public void moveArmTo(double degrees){
       m_IntakeArmMotor.setControl(motionMagic.withPosition(degrees));
   }
+  //set speed
+  public void setSpeed(double speed){
+    m_IntakeArmMotor.set(speed);
+  }
   // set incoder idk
   public void setIncodre (double incoder){
     m_IntakeArmMotor.setPosition(incoder);
   }
+  // get if a switch is press
   public boolean getSwitch(){
     return !driver.rightBumper().getAsBoolean(); // TODO change it from rightBumper to idk what 
   }
+  // check if intake arm is in place 
+  public boolean checkIntakeArmPosion(){
+    return Math.abs(m_IntakeArmMotor.getClosedLoopError().getValue()) < minimumError;
+  }
 
   @Override
   public void periodic() {

From 9de091806618d55fa5a58cad337046e9f360ba90 Mon Sep 17 00:00:00 2001
From: Eilon <eilonhuppert@gmail.com>
Date: Wed, 17 Jan 2024 10:07:22 +0200
Subject: [PATCH 03/10] fixing small problems

---
 src/main/java/frc/robot/Constants.java                     | 1 +
 .../IntakeArmCommands/{SetSpeed.java => IntakeZero.java}   | 7 ++++---
 src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java | 1 -
 3 files changed, 5 insertions(+), 4 deletions(-)
 rename src/main/java/frc/robot/commands/IntakeArmCommands/{SetSpeed.java => IntakeZero.java} (87%)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 6f2ca05..4588bc6 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -49,6 +49,7 @@ public static final class IntakeArm{ //TODO: This must be tuned to specific robo
         public static final double RevesrseSoftLimitThreshold = 0;  
         // not mm
         public static final double minimumError = 0;
+        public static final double zeroSpeed = 0;
         
     }
     public static final class Swerve {
diff --git a/src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java b/src/main/java/frc/robot/commands/IntakeArmCommands/IntakeZero.java
similarity index 87%
rename from src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java
rename to src/main/java/frc/robot/commands/IntakeArmCommands/IntakeZero.java
index 25f7f95..6be6e99 100644
--- a/src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java
+++ b/src/main/java/frc/robot/commands/IntakeArmCommands/IntakeZero.java
@@ -6,12 +6,13 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.IntakeArmSubsystem;
+import static frc.robot.Constants.IntakeArm.*;
 
-public class SetSpeed extends Command {
+public class IntakeZero extends Command {
   /** Creates a new SetSpeed. */
   private final IntakeArmSubsystem intakeArm = IntakeArmSubsystem.getInstance();
   private double speed;
-  public SetSpeed(double speed) {
+  public IntakeZero(double speed) {
     // Use addRequirements() here to declare subsystem dependencies.
     this.speed = speed;
   }
@@ -31,7 +32,7 @@ public void execute() {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    intakeArm.setSpeed(0);
+    intakeArm.setSpeed(zeroSpeed);
   }
 
   // Returns true when the command should end.
diff --git a/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java
index 4eb0748..4e76309 100644
--- a/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java
@@ -4,7 +4,6 @@
 
 package frc.robot.subsystems;
 
-import com.ctre.phoenix.motorcontrol.TalonSRXControlMode;
 import com.ctre.phoenix6.StatusCode;
 import com.ctre.phoenix6.configs.MotionMagicConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;

From 5595bcf9577cedf16fb3d4dc6496ca9b596f1335 Mon Sep 17 00:00:00 2001
From: Eilon <eilonhuppert@gmail.com>
Date: Wed, 17 Jan 2024 10:09:12 +0200
Subject: [PATCH 04/10] fixing small problems part 2

---
 .../java/frc/robot/commands/IntakeArmCommands/IntakeZero.java   | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/commands/IntakeArmCommands/IntakeZero.java b/src/main/java/frc/robot/commands/IntakeArmCommands/IntakeZero.java
index 6be6e99..0f5c90d 100644
--- a/src/main/java/frc/robot/commands/IntakeArmCommands/IntakeZero.java
+++ b/src/main/java/frc/robot/commands/IntakeArmCommands/IntakeZero.java
@@ -21,12 +21,12 @@ public IntakeZero(double speed) {
   @Override
   public void initialize() {
     IntakeArmSubsystem.getInstance();
+    intakeArm.setSpeed(speed);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    intakeArm.setSpeed(speed);
   }
 
   // Called once the command ends or is interrupted.

From 8ae45e44ad97e1829ab0e9c0ad4bd036f61707d4 Mon Sep 17 00:00:00 2001
From: Eilon <eilonhuppert@gmail.com>
Date: Wed, 17 Jan 2024 10:13:04 +0200
Subject: [PATCH 05/10] setSpeed command

---
 .../commands/IntakeArmCommands/SetSpeed.java  | 26 +++++++++++++++++++
 1 file changed, 26 insertions(+)
 create mode 100644 src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java

diff --git a/src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java b/src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java
new file mode 100644
index 0000000..e18843b
--- /dev/null
+++ b/src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java
@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.IntakeArmCommands;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.IntakeArmSubsystem;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class SetSpeed extends InstantCommand {
+  private final IntakeArmSubsystem intakeArm = IntakeArmSubsystem.getInstance();
+  private double speed;
+  public SetSpeed(double speed) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.speed = speed;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    intakeArm.setSpeed(speed);
+  }
+}

From 501e70882928bf83dd3611862457c68b3b88a70b Mon Sep 17 00:00:00 2001
From: Primo4586 <primo4586@gmail.com>
Date: Wed, 17 Jan 2024 17:54:48 +0200
Subject: [PATCH 06/10] fixing evrything except switch

---
 src/main/java/frc/robot/Constants.java                 |  5 +++--
 .../robot/commands/IntakeArmCommands/IntakeZero.java   | 10 ++++------
 .../frc/robot/commands/IntakeArmCommands/SetSpeed.java |  5 ++---
 .../java/frc/robot/subsystems/IntakeArmSubsystem.java  |  7 +++----
 4 files changed, 12 insertions(+), 15 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 4588bc6..c8424dc 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -32,7 +32,6 @@ public final class Constants {
     public static final class IntakeArm{ //TODO: This must be tuned to specific robot
         // mm
         public static final int KMotorID = 0;
-        public static final double CollectMotorSpeed = 0.5;
         public static final double mmVelocity = 5.0;
         public static final double mmAcceleration = 10.0;
         public static final double mmJerk = 50;
@@ -49,7 +48,9 @@ public static final class IntakeArm{ //TODO: This must be tuned to specific robo
         public static final double RevesrseSoftLimitThreshold = 0;  
         // not mm
         public static final double minimumError = 0;
-        public static final double zeroSpeed = 0;
+        public static final double startingValue = 0;
+        public static final double zeroEncoder = 0;
+        public static final double intakeArmSpeed = 0;
         
     }
     public static final class Swerve {
diff --git a/src/main/java/frc/robot/commands/IntakeArmCommands/IntakeZero.java b/src/main/java/frc/robot/commands/IntakeArmCommands/IntakeZero.java
index 0f5c90d..24de4be 100644
--- a/src/main/java/frc/robot/commands/IntakeArmCommands/IntakeZero.java
+++ b/src/main/java/frc/robot/commands/IntakeArmCommands/IntakeZero.java
@@ -11,17 +11,14 @@
 public class IntakeZero extends Command {
   /** Creates a new SetSpeed. */
   private final IntakeArmSubsystem intakeArm = IntakeArmSubsystem.getInstance();
-  private double speed;
-  public IntakeZero(double speed) {
+  public IntakeZero() {
     // Use addRequirements() here to declare subsystem dependencies.
-    this.speed = speed;
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    IntakeArmSubsystem.getInstance();
-    intakeArm.setSpeed(speed);
+    intakeArm.setSpeed(intakeArmSpeed);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -32,7 +29,8 @@ public void execute() {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    intakeArm.setSpeed(zeroSpeed);
+    intakeArm.setSpeed(0);
+    intakeArm.setEncoder(zeroEncoder);
   }
 
   // Returns true when the command should end.
diff --git a/src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java b/src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java
index e18843b..1e30b14 100644
--- a/src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java
+++ b/src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java
@@ -6,21 +6,20 @@
 
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.subsystems.IntakeArmSubsystem;
+import static frc.robot.Constants.IntakeArm.*;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class SetSpeed extends InstantCommand {
   private final IntakeArmSubsystem intakeArm = IntakeArmSubsystem.getInstance();
-  private double speed;
   public SetSpeed(double speed) {
     // Use addRequirements() here to declare subsystem dependencies.
-    this.speed = speed;
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    intakeArm.setSpeed(speed);
+    intakeArm.setSpeed(intakeArmSpeed);
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java
index 4e76309..7fc301f 100644
--- a/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java
@@ -15,7 +15,6 @@
 import static frc.robot.Constants.IntakeArm.*;
 
 public class IntakeArmSubsystem extends SubsystemBase {
-  private final CommandXboxController driver = new CommandXboxController(0);
   private TalonFX m_IntakeArmMotor;
   private final MotionMagicVoltage motionMagic = new MotionMagicVoltage(0);
 
@@ -29,7 +28,7 @@ public static IntakeArmSubsystem getInstance() {
     return instance;
   }
   /** Creates a new IntakeArmSubsystem. */
-  public IntakeArmSubsystem() {
+  private IntakeArmSubsystem() {
     this.m_IntakeArmMotor = new TalonFX(KMotorID);
     TalonFXConfiguration configs = new TalonFXConfiguration();
     MotionMagicConfigs mm = new MotionMagicConfigs();
@@ -78,12 +77,12 @@ public void setSpeed(double speed){
     m_IntakeArmMotor.set(speed);
   }
   // set incoder idk
-  public void setIncodre (double incoder){
+  public void setEncoder (double incoder){
     m_IntakeArmMotor.setPosition(incoder);
   }
   // get if a switch is press
   public boolean getSwitch(){
-    return !driver.rightBumper().getAsBoolean(); // TODO change it from rightBumper to idk what 
+    return true;
   }
   // check if intake arm is in place 
   public boolean checkIntakeArmPosion(){

From bd0c62da272a5d6fd90ef4a6475ad4ce60d1d7d6 Mon Sep 17 00:00:00 2001
From: Primo4586 <primo4586@gmail.com>
Date: Wed, 17 Jan 2024 18:02:39 +0200
Subject: [PATCH 07/10] switch

---
 src/main/java/frc/robot/Constants.java                     | 2 ++
 src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java | 7 +++++--
 2 files changed, 7 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index c8424dc..3e50902 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -51,6 +51,8 @@ public static final class IntakeArm{ //TODO: This must be tuned to specific robo
         public static final double startingValue = 0;
         public static final double zeroEncoder = 0;
         public static final double intakeArmSpeed = 0;
+        // switch
+        public static final int switchID = 0;
         
     }
     public static final class Swerve {
diff --git a/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java
index 7fc301f..b398fa7 100644
--- a/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java
@@ -9,6 +9,8 @@
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 
@@ -17,7 +19,7 @@
 public class IntakeArmSubsystem extends SubsystemBase {
   private TalonFX m_IntakeArmMotor;
   private final MotionMagicVoltage motionMagic = new MotionMagicVoltage(0);
-
+  private final DigitalInput intakeSwitch = new DigitalInput(switchID);
   // singleton
   private static IntakeArmSubsystem instance;
 
@@ -52,6 +54,7 @@ private IntakeArmSubsystem() {
     configs.SoftwareLimitSwitch.ReverseSoftLimitEnable = ReverseSoftLimitEnable;
     configs.SoftwareLimitSwitch.ReverseSoftLimitThreshold = RevesrseSoftLimitThreshold;
 
+
     // gives code to TalonFX
     StatusCode status = StatusCode.StatusCodeNotInitialized;
     for (int i = 0; i < 5; i++) {
@@ -82,7 +85,7 @@ public void setEncoder (double incoder){
   }
   // get if a switch is press
   public boolean getSwitch(){
-    return true;
+    return intakeSwitch.get();
   }
   // check if intake arm is in place 
   public boolean checkIntakeArmPosion(){

From 3739c2706d6d3d4b9f10c75dc06000c16359be58 Mon Sep 17 00:00:00 2001
From: Primo4586 <primo4586@gmail.com>
Date: Wed, 17 Jan 2024 18:14:52 +0200
Subject: [PATCH 08/10] fixing small problems part 3

---
 src/main/java/frc/robot/Constants.java                    | 2 +-
 .../frc/robot/commands/IntakeArmCommands/IntakeZero.java  | 2 +-
 .../frc/robot/commands/IntakeArmCommands/SetSpeed.java    | 4 +++-
 .../java/frc/robot/subsystems/IntakeArmSubsystem.java     | 8 ++++----
 4 files changed, 9 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 3e50902..18c62d2 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -49,7 +49,7 @@ public static final class IntakeArm{ //TODO: This must be tuned to specific robo
         // not mm
         public static final double minimumError = 0;
         public static final double startingValue = 0;
-        public static final double zeroEncoder = 0;
+        public static final double zeroEncoderValue = 0;
         public static final double intakeArmSpeed = 0;
         // switch
         public static final int switchID = 0;
diff --git a/src/main/java/frc/robot/commands/IntakeArmCommands/IntakeZero.java b/src/main/java/frc/robot/commands/IntakeArmCommands/IntakeZero.java
index 24de4be..ee0a19f 100644
--- a/src/main/java/frc/robot/commands/IntakeArmCommands/IntakeZero.java
+++ b/src/main/java/frc/robot/commands/IntakeArmCommands/IntakeZero.java
@@ -30,7 +30,7 @@ public void execute() {
   @Override
   public void end(boolean interrupted) {
     intakeArm.setSpeed(0);
-    intakeArm.setEncoder(zeroEncoder);
+    intakeArm.setEncoder(zeroEncoderValue);
   }
 
   // Returns true when the command should end.
diff --git a/src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java b/src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java
index 1e30b14..82daabe 100644
--- a/src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java
+++ b/src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java
@@ -13,13 +13,15 @@
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class SetSpeed extends InstantCommand {
   private final IntakeArmSubsystem intakeArm = IntakeArmSubsystem.getInstance();
+  private double speed;
   public SetSpeed(double speed) {
     // Use addRequirements() here to declare subsystem dependencies.
+    this.speed = speed;
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    intakeArm.setSpeed(intakeArmSpeed);
+    intakeArm.setSpeed(speed);
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java
index b398fa7..3c3aeb0 100644
--- a/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java
@@ -67,8 +67,8 @@ private IntakeArmSubsystem() {
       System.out.println("Turret could not apply configs, error code: " + status.toString());
     }
 
-    // make sure we start at 0.
-    m_IntakeArmMotor.setPosition(0);
+
+    m_IntakeArmMotor.setPosition(startingValue);
   }
 
   // moving the arm
@@ -80,8 +80,8 @@ public void setSpeed(double speed){
     m_IntakeArmMotor.set(speed);
   }
   // set incoder idk
-  public void setEncoder (double incoder){
-    m_IntakeArmMotor.setPosition(incoder);
+  public void setEncoder (double EncoderValue){
+    m_IntakeArmMotor.setPosition(EncoderValue);
   }
   // get if a switch is press
   public boolean getSwitch(){

From 9bf33b4e339d3a126f47a206d9cf50195e84373e Mon Sep 17 00:00:00 2001
From: Primo4586 <primo4586@gmail.com>
Date: Wed, 17 Jan 2024 18:17:24 +0200
Subject: [PATCH 09/10] e

---
 src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java
index 3c3aeb0..198e11d 100644
--- a/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java
@@ -80,8 +80,8 @@ public void setSpeed(double speed){
     m_IntakeArmMotor.set(speed);
   }
   // set incoder idk
-  public void setEncoder (double EncoderValue){
-    m_IntakeArmMotor.setPosition(EncoderValue);
+  public void setEncoder (double encoderValue){
+    m_IntakeArmMotor.setPosition(encoderValue);
   }
   // get if a switch is press
   public boolean getSwitch(){

From 27491ef0fa02a09d3819d0a0f067bc134661c00c Mon Sep 17 00:00:00 2001
From: 0kullyThekoala0 <oricoval@gmail.com>
Date: Wed, 17 Jan 2024 18:22:15 +0200
Subject: [PATCH 10/10] cleanup

---
 .../java/frc/robot/commands/IntakeArmCommands/SetSpeed.java     | 1 -
 src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java      | 2 --
 2 files changed, 3 deletions(-)

diff --git a/src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java b/src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java
index 82daabe..e18843b 100644
--- a/src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java
+++ b/src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java
@@ -6,7 +6,6 @@
 
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.subsystems.IntakeArmSubsystem;
-import static frc.robot.Constants.IntakeArm.*;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
diff --git a/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java
index 3c3aeb0..0a8d2d3 100644
--- a/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java
@@ -12,8 +12,6 @@
 
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-
 import static frc.robot.Constants.IntakeArm.*;
 
 public class IntakeArmSubsystem extends SubsystemBase {