From: Saara21 <113394225+Saara21@users.noreply.github.com>
Date: Sat, 7 Feb 2026 20:02:21 +0000 (-0800)
Subject: update
X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=738236841307db017837884298f09b352ceea2cd;p=FRC2026.git

update
---

diff --git a/src/main/java/frc/robot/commands/gpm/IntakeCommand.java b/src/main/java/frc/robot/commands/gpm/IntakeCommand.java
new file mode 100644
index 0000000..9ac2258
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/IntakeCommand.java
@@ -0,0 +1,7 @@
+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/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java
index 8e09939..3611a9c 100644
--- a/src/main/java/frc/robot/constants/IntakeConstants.java
+++ b/src/main/java/frc/robot/constants/IntakeConstants.java
@@ -9,12 +9,20 @@ public class IntakeConstants {
     public static final int leftID = 2;
     public static final int rollerID = 3;
 
+    //Motor speed
+    public static final double speed = 0.2;
+    public static final double gearRatio = 3;
+    public static final double radius = 0.5;
+
+     public static final double statorLimitAmps = 50.0;
+    public static final double supplyLimitAmps = 40.0;
+
     // Intake positions
 
     public static final double maxExtension = 12; // in inches: convert to rotations later
     public static final double startingPoint = 0;
 
+    public static final double rackPitch = 10;
     // for simulation
     public static final double kMaxRotations = 37.5;
-    public static final double kMaxVisualLength = 0.75;
 }
diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java
index 51b0a3a..6d6cc22 100644
--- a/src/main/java/frc/robot/subsystems/Intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java
@@ -8,8 +8,11 @@ import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.MotorAlignmentValue;
 import com.ctre.phoenix6.signals.MotorArrangementValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.fasterxml.jackson.databind.ser.std.InetAddressSerializer;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
@@ -30,6 +33,9 @@ public class Intake extends SubsystemBase {
     private TalonFX rollerMotor; 
     private TalonFX rightMotor; //leader
     private TalonFX leftMotor; //invert this one
+    private double maxVelocity;
+    private double maxAcceleration;
+    private DCMotorSim intakeSim;
 
     private MotionMagicVoltage voltageRequest = new MotionMagicVoltage(0);
 
@@ -37,6 +43,7 @@ public class Intake extends SubsystemBase {
         rightMotor = new TalonFX(IntakeConstants.rightID);
         leftMotor = new TalonFX(IntakeConstants.leftID);
         rollerMotor = new TalonFX(IntakeConstants.rollerID); 
+        DCMotor dcmotor = DCMotor.getKrakenX44(2);
 
         // right motor configs
         TalonFXConfiguration Config = new TalonFXConfiguration();
@@ -49,6 +56,17 @@ public class Intake extends SubsystemBase {
         slot0Configs.kV = 0;
         slot0Configs.kA = 0;
 
+
+        var currentLimits = Config.CurrentLimits;
+        currentLimits.StatorCurrentLimitEnable = true;
+        currentLimits.StatorCurrentLimit = IntakeConstants.statorLimitAmps;
+        currentLimits.SupplyCurrentLimitEnable = true;
+        currentLimits.SupplyCurrentLimit = IntakeConstants.supplyLimitAmps;
+
+        var motionMagicConfigs = Config.MotionMagic;
+
+        motionMagicConfigs.MotionMagicCruiseVelocity =  IntakeConstants.gearRatio * maxVelocity/IntakeConstants.radius/Math.PI/2;
+        motionMagicConfigs.MotionMagicAcceleration = IntakeConstants.gearRatio * maxAcceleration/IntakeConstants.radius/Math.PI/2;
         rightMotor.getConfigurator().apply(new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake));
         rightMotor.getConfigurator().apply(Config);
 
@@ -76,7 +94,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
         percentExtended = Math.max(0.0, Math.min(1.0, percentExtended));
 
         // robotExtension.setLength(percentExtended * maxExtension);
@@ -85,6 +103,17 @@ public class Intake extends SubsystemBase {
 
     public void simulationPeriodic(){
         // simulate the motor activity
+        double voltage = rightMotor.getMotorVoltage().getValueAsDouble();
+        intakeSim.setInputVoltage(voltage);
+        intakeSim.update(0.02);
+        // rackPitch in teeth/inch
+        double mechanicalRotation = intakeSim.getAngularPositionRotations();
+        double motorRotation = mechanicalRotation * IntakeConstants.gearRatio; 
+        //convert motor rotation to distance
+      
+
+
+
     }
 
     /**
@@ -99,7 +128,7 @@ public class Intake extends SubsystemBase {
     }
 
     /**
-     * gets the position in rotations
+     * gets motor position in inches
      */
     public double getPosition(){
         double position = rightMotor.getPosition().getValueAsDouble();
@@ -107,7 +136,7 @@ public class Intake extends SubsystemBase {
     }
 
     public void spin(double speed) {
-        rollerMotor.set(0.2);
+        rollerMotor.set(IntakeConstants.speed);
     }
 
     public void extend() {