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 (file)
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 8e09939748e1b92da9aea198ebfc2d2aa7a61426..3611a9cdc71266e62d532a3487b7401bce2b7dac 100644 (file)
--- 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 51b0a3a01d3f1a37d542ed25afefca3f6216509b..6d6cc22a329b26a94091f22f8263d486e6985179 100644 (file)
--- 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() {