Elevator

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,14 +19,16 @@
 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,
         TESTING,
-        SIM
+        SIM,
+        REPLAY
     }
     public static final RobotMode ROBOT_MODE = "Crash".equals(System.getenv("CI_NAME")) ? RobotMode.SIM : RobotMode.SIM;
+    public static final double SIM_DELTA_TIME = 0.02;
 
     public static class Controllers {
         public static final XboxController DRIVER_CONTROLLER = new XboxController(0);

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());
@@ -36,6 +36,9 @@ public void robotInit() {
             case TESTING:
                 Logger.addDataReceiver(new NT4Publisher());
                 break;
+            case REPLAY:
+                Logger.addDataReceiver(new NT4Publisher());
+                break;
         }
 
 		Logger.start();

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> {
@@ -56,6 +56,7 @@ private Drive() {
             case REAL -> new DriveIOReal();
             case SIM -> new DriveIOSim();
             case TESTING -> new DriveIOReal();
+            case REPLAY -> new DriveIOSim();
         };
 
         // Setup Path Planner
@@ -106,8 +107,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 +263,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 +277,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/Elevator/Elevator.java

@@ -0,0 +1,41 @@
+package frc.robot.subsystems.Elevator;
+
+import static frc.robot.GlobalConstants.ROBOT_MODE;
+
+import org.littletonrobotics.junction.Logger;
+import frc.robot.pioneersLib.subsystem.Subsystem;
+
+public class Elevator extends Subsystem<ElevatorStates> {
+    private static Elevator instance;
+
+    private ElevatorIO io;
+    private ElevatorIOInputsAutoLogged inputs;
+
+    private Elevator() {
+        super("Elevator", ElevatorStates.IDLE);
+        this.io = switch (ROBOT_MODE) {
+            case SIM -> new ElevatorIOSim();
+            case REAL -> new ElevatorIOReal();
+            case TESTING -> new ElevatorIOReal();
+            case REPLAY -> new ElevatorIOSim();
+        };
+        inputs = new ElevatorIOInputsAutoLogged();
+    }
+
+    public static Elevator getInstance() {
+        if (instance == null) {
+            instance = new Elevator();
+        }
+        return instance;
+    }
+
+    @Override
+    protected void runState() {
+        io.setHeightGoalpoint(getState().getTargetHeight());
+        io.runElevator();
+
+        io.updateInputs(inputs);
+        Logger.processInputs("Elevator", inputs);
+    }
+
+}

src/main/java/frc/robot/subsystems/Elevator/ElevatorConstants.java

@@ -0,0 +1,62 @@
+package frc.robot.subsystems.Elevator;
+
+import static edu.wpi.first.units.Units.Amps;
+import static edu.wpi.first.units.Units.Kilogram;
+import static edu.wpi.first.units.Units.Meters;
+
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.pathplanner.lib.config.PIDConstants;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.Mass;
+import frc.robot.pioneersLib.controlConstants.FFConstants;
+
+public final class ElevatorConstants {
+
+    public static final double POSITION_TOLERANCE = 0;
+    public static final double VELOCITY_TOLERANCE = 0;
+    public static final Constraints TRAPEZOID_PROFILE_CONSTRAINTS = new TrapezoidProfile.Constraints(1, .5);
+
+    //more random values
+    public static final Distance HIGH_POSITION_HEIGHT = Meters.of(1.4);
+    public static final Distance MID_POSITION_HEIGHT = Meters.of(.5);
+    public static final Distance IDLE_POSITION_HEIGHT = Meters.of(0);
+
+    public static class Sim {
+        public static final DCMotor GEARBOX = DCMotor.getKrakenX60(2);
+        public static final double GEARING = 9;
+        public static final Mass CARRIAGE_MASS = Kilogram.of(28.44);
+        public static final Distance DRUM_RADIUS = Meters.of(.5); // Random value cuz mech is bum
+        public static final Distance MIN_HEIGHT = Meters.of(1.54305);
+        public static final Distance MAX_HEIGHT = Meters.of(2.60985);
+        public static final boolean SIMULATE_GRAVITY = true;
+        public static final Distance STARTING_HEIGHT = Meters.of(1.54305);
+        //idk these values
+        public static final PIDConstants PROFILLED_PID_CONSTANTS = new PIDConstants(0, 0, 0, 0);
+
+        public static final FFConstants FF_CONSTANTS = new FFConstants(0, 0, 0, 0);
+
+    }
+
+    public static class Real {
+            public static final int LEFT_MOTOR_CANID = 12;
+            public static final int RIGHT_MOTOR_CANID = 13;
+
+            public static final boolean LEFT_INVERTED = false;
+            public static final NeutralModeValue LEFT_NEUTRAL_MODE = NeutralModeValue.Coast;
+            public static final boolean LEFT_STRATOR_CURRENT_LIMIT_ENABLED = true;
+            public static final Current LEFT_STRATOR_CURRENT_LIMIT = Amps.of(30);
+
+            public static final boolean RIGHT_INVERTED = false;
+            public static final NeutralModeValue RIGHT_NEUTRAL_MODE = NeutralModeValue.Coast;
+            public static final boolean RIGHT_STRATOR_CURRENT_LIMIT_ENABLED = true;
+            public static final Current RIGHT_STRATOR_CURRENT_LIMIT = Amps.of(30);
+
+            public static final PIDConstants PROFILLED_PID_CONSTANTS = new PIDConstants(0, 0, 0, 0);
+            public static final FFConstants FF_CONSTANTS = new FFConstants(0, 0, 0, 0);
+    }       
+}

src/main/java/frc/robot/subsystems/Elevator/ElevatorIO.java

@@ -0,0 +1,26 @@
+package frc.robot.subsystems.Elevator;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface ElevatorIO {
+
+    @AutoLog
+    public static class ElevatorIOInputs {
+        public double currentElevatorHeight;
+        public double elevatorHeightSetpoint;
+        public double elevatorHeightGoalpoint;
+        public double elevatorVelocity;
+        public double elevatorVelocitySetpoint;
+        public double elevatorVelocityGoalpoint;
+        public double leftMotorVoltInput;
+        public double rightMotorVoltInput;
+    }
+
+    public void setHeightGoalpoint(double height);
+
+    public void updateInputs(ElevatorIOInputs inputs);
+
+    public void runElevator();
+
+    public boolean nearTarget();
+}