intake

simgui-ds.json

@@ -0,0 +1,92 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ]
+}

src/main/java/frc/robot/Constants.java → src/main/java/frc/robot/GlobalConstants.java

@@ -19,7 +19,7 @@
 import frc.robot.pioneersLib.misc.VisionUtil.CameraResolution;
 import frc.robot.subsystems.Drive.Drive.SysIdMode;
 
-public final class Constants {
+public final class GlobalConstants {
 
     public enum RobotMode {
         REAL,

src/main/java/frc/robot/Robot.java

@@ -23,7 +23,7 @@ public class Robot extends LoggedRobot {
     @Override
     public void robotInit() {
 
-        switch (Constants.ROBOT_MODE) {
+        switch (GlobalConstants.ROBOT_MODE) {
             case REAL:
                 Logger.addDataReceiver(new NT4Publisher());
                 Logger.addDataReceiver(new WPILOGWriter());

src/main/java/frc/robot/subsystems/Drive/Drive.java

@@ -21,16 +21,16 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
-import frc.robot.Constants;
+import frc.robot.GlobalConstants;
 import frc.robot.pioneersLib.misc.LocalADStarAK;
 import frc.robot.pioneersLib.subsystem.Subsystem;
 
 import org.littletonrobotics.junction.Logger;
 
 import static edu.wpi.first.units.Units.*;
-import static frc.robot.Constants.Controllers.*;
-import static frc.robot.Constants.*;
-import static frc.robot.Constants.Drive.*;
+import static frc.robot.GlobalConstants.Controllers.*;
+import static frc.robot.GlobalConstants.*;
+import static frc.robot.GlobalConstants.Drive.*;
 import static frc.robot.subsystems.Drive.TunerConstants.kSpeedAt12Volts;
 
 public class Drive extends Subsystem<DriveStates> {
@@ -106,8 +106,8 @@ public void runState() {
             DriverStation.getAlliance().ifPresent(allianceColor -> {
                 driveIO.getDrive().setOperatorPerspectiveForward(
                         allianceColor == Alliance.Red
-                                ? Constants.Drive.RED_ALLIANCE_PERSPECTIVE_ROTATION
-                                : Constants.Drive.BLUE_ALLIANCE_PERSPECTIVE_ROTATION);
+                                ? GlobalConstants.Drive.RED_ALLIANCE_PERSPECTIVE_ROTATION
+                                : GlobalConstants.Drive.BLUE_ALLIANCE_PERSPECTIVE_ROTATION);
                 robotMirrored = true;
             });
         }
@@ -262,7 +262,7 @@ public enum SysIdMode {
 
     @Override
     public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
-        switch (Constants.Drive.SYS_ID_MODE) {
+        switch (GlobalConstants.Drive.SYS_ID_MODE) {
             case ROTATION:
                 return sysIdRoutineRotation.quasistatic(direction);
             case TRANSLATION:
@@ -276,7 +276,7 @@ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
 
     @Override
     public Command sysIdDynamic(SysIdRoutine.Direction direction) {
-        switch (Constants.Drive.SYS_ID_MODE) {
+        switch (GlobalConstants.Drive.SYS_ID_MODE) {
             case ROTATION:
                 return sysIdRoutineRotation.dynamic(direction);
             case TRANSLATION:

src/main/java/frc/robot/subsystems/Drive/DriveIOSim.java

@@ -4,7 +4,7 @@
 
 import edu.wpi.first.wpilibj.Notifier;
 import edu.wpi.first.wpilibj.RobotController;
-import frc.robot.Constants;
+import frc.robot.GlobalConstants;
 
 /**
  * This class represents the simulated input/output for the drive subsystem.
@@ -35,6 +35,6 @@ private void startSimThread() {
             /* use the measured time delta, get battery voltage from WPILib */
             getDrive().updateSimState(deltaTime, RobotController.getBatteryVoltage());
         });
-        simNotifier.startPeriodic(Constants.Drive.SIM_UPDATE_TIME);
+        simNotifier.startPeriodic(GlobalConstants.Drive.SIM_UPDATE_TIME);
     }
 }

src/main/java/frc/robot/subsystems/Drive/DriveStates.java

@@ -3,8 +3,8 @@
 import frc.robot.pioneersLib.subsystem.SubsystemStates;
 
 import static edu.wpi.first.units.Units.*;
-import static frc.robot.Constants.Controllers.*;
-import static frc.robot.Constants.Drive.*;
+import static frc.robot.GlobalConstants.Controllers.*;
+import static frc.robot.GlobalConstants.Drive.*;
 import static frc.robot.subsystems.Drive.TunerConstants.*;
 
 /**

src/main/java/frc/robot/subsystems/Intake/Constants.java

@@ -0,0 +1,29 @@
+package frc.robot.subsystems.Intake;
+
+import com.pathplanner.lib.config.PIDConstants;
+
+public final class Constants {
+    public final static double INTAKING_SPEED = 60;
+    public final static double OUTTAKING_SPEED = -60;
+    public static final double PASSING_SPEED = 30; //might be negative idk
+    public final static double IDLE_SPEED = 0;
+
+    public final static double INTAKING_PIVOT = 70;
+    public final static double OUTTAKING_PIVOT = 70;
+    public final static double PASSING_PIVOT = 0;
+    public final static double IDLE_PIVOT = 0;
+
+    public final static int NUM_PIVOT_MOTORS = 1;
+    public final static double PIVOT_GEARING = 5;
+    public final static double PIVOT_MOI = 0.31654227;
+    public final static double PIVOT_ARM_LENGTH = 26.199558;
+    public final static double MIN_PIVOT_ANGLE = 0;
+    public final static double MAX_PIVOT_ANGLE = 70;
+    public final static double STARTING_PIVOT_ANGLE = 70;
+    public final static PIDConstants PIVOT_PID_CONSTANTS = new PIDConstants(0, 0, 0);
+
+    public final static int NUM_WHEEL_MOTORS = 1;
+    public final static double WHEEL_MOTOR_MOI = 0.0001;
+    public final static double WHEEL_MOTOR_GEARING = 3;
+    public final static PIDConstants WHEEL_PID_CONSTANTS = new PIDConstants(0, 0, 0);
+}

src/main/java/frc/robot/subsystems/Intake/Intake.java

@@ -0,0 +1,36 @@
+package frc.robot.subsystems.Intake;
+
+import static frc.robot.GlobalConstants.ROBOT_MODE;
+
+import frc.robot.pioneersLib.subsystem.Subsystem;
+import frc.robot.subsystems.Intake.IntakeIO.IntakeIOInputs;
+
+public class Intake extends Subsystem<IntakeStates>{
+    IntakeIO io;
+    IntakeIOInputs inputs;
+    private static Intake instance;
+
+    private Intake(IntakeIO io) {
+        super("Intake", IntakeStates.IDLE);
+        this.io = io;
+        inputs = new IntakeIOInputs();
+    }
+
+    @Override
+    protected void runState() {
+        io.setPivotSetpoint(getState().getPivotSetpoint());
+        io.setWheelSpeed(getState().getWheelSpeedSetpoint());
+        io.updateInputs(inputs);
+    }
+
+    public static Intake getInstance() {
+        if (instance == null) {
+            IntakeIO intakeIO = switch(ROBOT_MODE) {
+                case SIM -> new IntakeIOSim();
+                default -> new IntakeIOSim();
+            };
+            instance = new Intake(intakeIO);
+        }
+        return instance;
+    }
+}

src/main/java/frc/robot/subsystems/Intake/IntakeIO.java

@@ -0,0 +1,21 @@
+package frc.robot.subsystems.Intake;
+
+public interface IntakeIO {
+    public static class IntakeIOInputs {
+		// Pivot
+		public double pivotPosition;
+		public double pivotSetpoint;
+
+		// Spinners
+		public double wheelSpeed;
+		public double wheelSpeedSetpoint;
+	}
+
+    public void updateInputs(IntakeIOInputs input);
+
+    public void setPivotSetpoint(double pivotSetpoint);
+
+    public void setWheelSpeed(double wheelSpeed);
+
+
+}

src/main/java/frc/robot/subsystems/Intake/IntakeIOSim.java

@@ -0,0 +1,54 @@
+package frc.robot.subsystems.Intake;
+
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+
+public class IntakeIOSim implements IntakeIO {
+    SingleJointedArmSim pivotSim;
+    DCMotorSim wheelMotorSim;
+    PIDController pivotController;
+    PIDController wheelSpeedController;
+
+    double wheelSpeedSetpoint;
+    double pivotPositionSetpoint;
+
+    IntakeIOSim() {
+        pivotSim = new SingleJointedArmSim(DCMotor.getKrakenX60(Constants.NUM_PIVOT_MOTORS), Constants.PIVOT_GEARING, Constants.PIVOT_MOI, Constants.PIVOT_ARM_LENGTH, Units.degreesToRadians(Constants.MIN_PIVOT_ANGLE), Units.degreesToRadians(Constants.MAX_PIVOT_ANGLE), false, Units.degreesToRadians(Constants.STARTING_PIVOT_ANGLE));
+        wheelMotorSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(Constants.NUM_WHEEL_MOTORS), Constants.WHEEL_MOTOR_MOI, Constants.WHEEL_MOTOR_GEARING), DCMotor.getKrakenX60(Constants.NUM_WHEEL_MOTORS));
+        pivotController = new PIDController(0, 0, 0);
+        wheelSpeedController = new PIDController(0, 0, 0);
+
+        wheelSpeedSetpoint = 0;
+        pivotPositionSetpoint = 0;
+    }
+
+    @Override
+    public void updateInputs(IntakeIOInputs input) {
+        input.wheelSpeed = Units.radiansToDegrees(wheelMotorSim.getAngularVelocityRadPerSec());
+        input.wheelSpeedSetpoint = wheelSpeedSetpoint;
+        input.pivotPosition = Units.radiansToDegrees(pivotSim.getAngleRads());
+        input.pivotSetpoint = pivotPositionSetpoint;
+
+        pivotSim.update(0.02);
+        wheelMotorSim.update(0.02);
+    }
+
+    @Override
+    public void setPivotSetpoint(double pivotSetpoint) {
+        this.pivotPositionSetpoint = pivotSetpoint;
+        pivotSim.setInputVoltage(pivotController.calculate(Units.radiansToDegrees(pivotSim.getAngleRads()), pivotSetpoint));
+    }
+
+    @Override
+    public void setWheelSpeed(double wheelSpeedSetpoint) {
+        this.wheelSpeedSetpoint = wheelSpeedSetpoint;
+        wheelMotorSim.setInputVoltage(wheelSpeedController.calculate(wheelMotorSim.getAngularVelocityRadPerSec(), wheelSpeedSetpoint));
+    }
+
+
+}

src/main/java/frc/robot/subsystems/Intake/IntakeStates.java

@@ -0,0 +1,33 @@
+package frc.robot.subsystems.Intake;
+
+import frc.robot.pioneersLib.subsystem.SubsystemStates;
+
+public enum IntakeStates implements SubsystemStates {
+    INTAKING(Constants.INTAKING_SPEED, Constants.INTAKING_PIVOT, "INTAKING"),
+    OUTTAKING(Constants.OUTTAKING_SPEED, Constants.OUTTAKING_PIVOT, "OUTTAKING"),
+    PASSING(Constants.PASSING_SPEED, Constants.PASSING_PIVOT, "PASSING"),
+    IDLE(Constants.IDLE_SPEED, Constants.IDLE_PIVOT, "IDLE");
+
+    IntakeStates(Double wheelSpeed, double pivotSetpoint, String stateString) {
+        this.wheelSpeed = wheelSpeed;
+        this.pivotSetpoint = pivotSetpoint;
+        this.stateString = stateString;
+    }
+
+    private double wheelSpeed;
+    private double pivotSetpoint;
+    private String stateString;
+
+    public double getWheelSpeedSetpoint() {
+        return wheelSpeed;
+    }
+
+    public double getPivotSetpoint() {
+        return pivotSetpoint;
+    }
+
+    @Override
+    public String getStateString() {
+        return stateString;
+    }
+}