need to add encoder stuff

diff --git a/src/main/java/frc/robot/commands/gpm/IntakeCommand.java b/src/main/java/frc/robot/commands/gpm/IntakeCommand.java
deleted file mode 100644 (file)
index 9ac2258..0000000
--- a/src/main/java/frc/robot/commands/gpm/IntakeCommand.java
+++ /dev/null
@@ -1,7 +0,0 @@
-package frc.robot.commands.gpm;
-
-import edu.wpi.first.wpilibj2.command.Command;
-
-public class IntakeCommand extends Command {
-    
-}

diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java
index 1a83bc9abc031e23fe322b4e55b3dda6f94aa50c..047d874837a3e8a600b108d644a7699820df9bfa 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java
@@ -39,9 +39,9 @@ public class Intake extends SubsystemBase {
     private TalonFX leftMotor; //invert this one
     private double maxVelocity;
     private double maxAcceleration;
-    private DCMotorSim intakeSim;
-
-    private MotionMagicVoltage voltageRequest = new MotionMagicVoltage(0);
+    private final DCMotorSim intakeSim;
+    private double distance;
+    private final MotionMagicVoltage voltageRequest = new MotionMagicVoltage(0);
 
     public Intake() {
         rightMotor = new TalonFX(IntakeConstants.rightID);
@@ -70,7 +70,7 @@ public class Intake extends SubsystemBase {
         var slot0Configs = Config.Slot0;
         //find values later
         //friction, maybe?
-        slot0Configs.kP = 0;
+        slot0Configs.kP = 0.1;
         slot0Configs.kI = 0;
         slot0Configs.kD = 0;
         slot0Configs.kV = 0;
@@ -114,7 +114,7 @@ public class Intake extends SubsystemBase {
 
         // report the position of the extension
         double percentExtended = getPosition() / IntakeConstants.kMaxRotations;
-        double distance = percentExtended/IntakeConstants.gearRatio * 1/IntakeConstants.rackPitch; // in inches
+        distance = percentExtended/IntakeConstants.gearRatio * 1/IntakeConstants.rackPitch; 
         percentExtended = Math.max(0.0, Math.min(1.0, percentExtended));
 
         // robotExtension.setLength(percentExtended * maxExtension);
@@ -152,6 +152,11 @@ public class Intake extends SubsystemBase {
         return position;
     }
 
+    public boolean atMaxExtension(){
+        return distance == IntakeConstants.maxExtension; //  TODO add tolerance for distance
+    }
+
+
     public void spin(double speed) {
         rollerMotor.set(IntakeConstants.speed);
     }
@@ -163,6 +168,7 @@ public class Intake extends SubsystemBase {
 
     public void retract(){
         setPosition(IntakeConstants.startingPoint);
+
         
     }