Elevator sim

important-files/advantage-scope/cad/Robot_UNKNOWN/config.json

@@ -16,37 +16,16 @@
   ],
   "position": [
     0.05,
-    0.1,
-    0.12
-  ],
-  "cameras": [
-    {
-      "name": "Object detection",
-      "rotations": [
-        {
-          "axis": "x",
-          "degrees": 0
-        },
-        {
-          "axis": "y",
-          "degrees": -32
-        },
-        {
-          "axis": "z",
-          "degrees": 180
-        }
-      ],
-      "position": [-0.293, -0.293, 0.2],
-      "resolution": [1600, 1200],
-      "fov": 75
-    }
+    0.13,
+    0.125
   ],
+  "cameras": [],
   "components": [
     {
       "zeroedRotations": [
         {
           "axis": "x",
-          "degrees": 110
+          "degrees": 113
         },
         {
           "axis": "y",
@@ -58,11 +37,15 @@
         }
       ],
       "zeroedPosition": [
-        -0.628,
-        0.413,
-        0.777
+        -0.42,
+        1.145,
+        -0.12
       ]
-    }
+    },
+    {
+      "zeroedRotations": [
+    0.1,
+    0.12
   ],
-  "name": "CLASSIFIED"
+  "name": "Classified"
 }

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

@@ -10,6 +10,7 @@
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.subsystems.elevator.ElevatorConstants;
 import frc.robot.swerve.SwerveConstants;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
@@ -77,6 +78,7 @@ public void robotInit() {
         SignalLogger.enableAutoLogging(true);
 
         SwerveConstants.initConstants(true, Robot.isReal());
+        ElevatorConstants.initConstants();
         robotContainer = RobotContainer.getInstance();
         compressor.enableDigital();
     }

src/main/java/frc/robot/RobotContainer.java

@@ -1,8 +1,6 @@
 package frc.robot;
 
-import edu.wpi.first.units.Units;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.button.CommandGenericHID;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.subsystems.conveyor.Conveyor;
 import frc.robot.subsystems.conveyor.ConveyorConstants;
@@ -21,21 +19,31 @@ public class RobotContainer {
     private final Conveyor conveyor;
     private final SwerveDrive swerveDrive;
     private final CommandXboxController xboxController = new CommandXboxController(0);
-    private final CommandGenericHID keyBoardControl = new CommandGenericHID(0);
 
     /** The container for the robot. Contains subsystems, OI devices, and commands. */
     private RobotContainer() {
         ElevatorIO elevatorIO;
-        ConveyorIO conveyorIO;
-        ConveyorConstants.initialize(Constants.CURRENT_MODE);
+        ExampleSubsystemIO exampleSubsystemIO;
         switch (Constants.CURRENT_MODE) {
             case REAL:
                 elevatorIO = new ElevatorIOReal();
-                conveyorIO = new ConveyorIOSim();
-                break;
-            case SIM:
-            case REPLAY:
-            default:
+                exampleSubsystemIO = new ExampleSubsystemIOReal();
+                ConveyorIO conveyorIO;
+                ConveyorConstants.initialize(Constants.CURRENT_MODE);
+                switch (Constants.CURRENT_MODE) {
+                    case REAL:
+                        elevatorIO = new ElevatorIOReal();
+                        conveyorIO = new ConveyorIOSim();
+                        break;
+                    case SIM:
+                    case REPLAY:
+                    default:
+                        exampleSubsystemIO = new ExampleSubsystemIOSim();
+                        elevatorIO = new ElevatorIOSim();
+                        break;
+                }
+                ExampleSubsystem.initialize(exampleSubsystemIO);
+                Elevator.initialize(elevatorIO);
                 conveyorIO = new ConveyorIOSim();
                 elevatorIO = new ElevatorIOSim();
                 break;
@@ -48,6 +56,7 @@ private RobotContainer() {
         swerveDrive = SwerveDrive.getInstance();
         elevator = Elevator.getInstance();
         conveyor = Conveyor.getInstance();
+
         // Configure the button bindings and default commands
         configureDefaultCommands();
         configureButtonBindings();
@@ -70,15 +79,7 @@ private void configureDefaultCommands() {
                         () -> true));
     }
 
-    private void configureButtonBindings() {
-        //        xboxController.a().onTrue(elevator.setHeight(2));
-        keyBoardControl
-                .button(1)
-                .onTrue(conveyor.setVelocity(() -> Units.RotationsPerSecond.of(50).mutableCopy()));
-        keyBoardControl
-                .button(2)
-                .onTrue(conveyor.setVelocity(() -> Units.RotationsPerSecond.of(0).mutableCopy()));
-    }
+    private void configureButtonBindings() {}
 
     /**
      * Use this to pass the autonomous command to the main {@link Robot} class.

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

@@ -3,6 +3,9 @@
 import static frc.robot.subsystems.elevator.ElevatorConstants.MECHANISM_HEIGHT;
 import static frc.robot.subsystems.elevator.ElevatorConstants.MECHANISM_WIDTH;
 
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.units.Distance;
 import edu.wpi.first.units.MutableMeasure;
 import edu.wpi.first.units.Units;
@@ -24,9 +27,12 @@ public class Elevator extends SubsystemBase {
     @AutoLogOutput
     private final Mechanism2d mechanism2d = new Mechanism2d(MECHANISM_WIDTH, MECHANISM_HEIGHT);
 
+    @AutoLogOutput private Pose3d elevatorPose = new Pose3d(0, 0, 0, new Rotation3d());
+    @AutoLogOutput private Pose3d carriagePose = new Pose3d(0, 0, 0, new Rotation3d());
+
     private final MechanismRoot2d root = mechanism2d.getRoot("Elevator", 0, 0);
     private final MechanismLigament2d elevator =
-            root.append(new MechanismLigament2d("Elevator", 0, 0));
+            root.append(new MechanismLigament2d("Elevator", 0, 90));
 
     private Elevator(ElevatorIO io) {
         this.io = io;
@@ -40,32 +46,34 @@ public static void initialize(ElevatorIO io) {
         INSTANCE = new Elevator(io);
     }
 
-    public void setPower(double power) {
-        io.setPower(power);
-        inputs.controlMode = ElevatorIO.ControlMode.PERCENT_OUTPUT;
-    }
-
-    public void setHeight(MutableMeasure<Distance> height) {
-        io.setHeight(height);
-        inputs.controlMode = ElevatorIO.ControlMode.POSITION;
-    }
-
     public MutableMeasure<Distance> getCurrentHeight() {
-        return inputs.currentHeight;
+        return inputs.carriageHeight;
     }
 
     public MutableMeasure<Distance> getHeightSetpoint() {
         return inputs.heightSetpoint;
     }
 
-    public Command setHeight(double height) {
-        return run(() -> this.setHeight(height));
+    public Command setHeight(MutableMeasure<Distance> height) {
+        return runOnce(() -> inputs.heightSetpoint = height)
+                .andThen(run(() -> io.setHeight(height)));
     }
 
     @Override
     public void periodic() {
         io.updateInputs(inputs);
         elevator.setLength(getCurrentHeight().in(Units.Meters));
         Logger.processInputs(this.getClass().getSimpleName(), inputs);
+
+        Logger.recordOutput(
+                "elevatorPose",
+                new Pose3d(
+                        new Translation3d(0, 0, inputs.gripperHeight.in(Units.Meters)),
+                        new Rotation3d(0, 0, 0)));
+        Logger.recordOutput(
+                "carriagePose",
+                new Pose3d(
+                        new Translation3d(0, 0, inputs.carriageHeight.in(Units.Meters)),
+                        new Rotation3d(0, 0, 0)));
     }
 }

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

@@ -1,11 +1,13 @@
 package frc.robot.subsystems.elevator;
 
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.units.*;
+import frc.robot.Constants;
 import lib.webconstants.LoggedTunableNumber;
 
 public class ElevatorConstants { // TODO: check real values
-    public static final double MECHANISM_WIDTH = 0; // [m]
-    public static final double MECHANISM_HEIGHT = 0; // [m]
+    public static final double MECHANISM_WIDTH = 0.8; // [m]
+    public static final double MECHANISM_HEIGHT = 2; // [m]
     public static final double GEAR_RATIO = 12.0;
     public static final double DRUM_RADIUS = 0.02; // [m]
 
@@ -16,7 +18,29 @@ public class ElevatorConstants { // TODO: check real values
     public static final MutableMeasure<Distance> MAX_HEIGHT =
             Units.Meters.of(0).mutableCopy(); // [m]
 
-    public static final LoggedTunableNumber KP = new LoggedTunableNumber("kp", 0);
-    public static final LoggedTunableNumber KI = new LoggedTunableNumber("ki", 0);
-    public static final LoggedTunableNumber KD = new LoggedTunableNumber("kd", 0);
+    public static final double MAX_VELOCITY = 3;
+    public static final double MAX_ACCELERATION = 7;
+
+    public static final Measure<Distance> GRIPPER_HEIGHT = Units.Meters.of(0.3); // [m]
+
+    public static final TrapezoidProfile.Constraints TRAPEZOID_PROFILE =
+            new TrapezoidProfile.Constraints(MAX_VELOCITY, MAX_ACCELERATION);
+
+    public static final LoggedTunableNumber KP = new LoggedTunableNumber("kp");
+    public static final LoggedTunableNumber KI = new LoggedTunableNumber("ki");
+    public static final LoggedTunableNumber KD = new LoggedTunableNumber("kd");
+
+    public static void initConstants() {
+        switch (Constants.CURRENT_MODE) {
+            case REAL:
+                KP.initDefault(0.8);
+                KI.initDefault(0.0);
+                KD.initDefault(0.0);
+            case SIM:
+            case REPLAY:
+                KP.initDefault(43.0);
+                KI.initDefault(0.0003);
+                KD.initDefault(0.05);
+        }
+    }
 }

src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java

@@ -4,7 +4,7 @@
 
 import com.ctre.phoenix6.controls.DutyCycleOut;
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
-import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.units.Distance;
 import edu.wpi.first.units.MutableMeasure;
 import edu.wpi.first.wpilibj.Timer;
@@ -17,35 +17,44 @@ public class ElevatorIOSim implements ElevatorIO {
     private final DutyCycleOut powerRequest = new DutyCycleOut(0);
 
     public ElevatorIOSim() {
-        motor = new TalonFXSim(2, ElevatorConstants.GEAR_RATIO, 0.5, 1);
-
-        motor.setController(
-                new PIDController(
+        motor =
+                new TalonFXSim(
+                        2,
+                        ElevatorConstants.GEAR_RATIO,
+                        0.000_01,
+                        ElevatorConstants.GEAR_RATIO
+                                * (2 * Math.PI * ElevatorConstants.DRUM_RADIUS));
+
+        motor.setProfiledController(
+                new ProfiledPIDController(
                         ElevatorConstants.KP.get(),
                         ElevatorConstants.KI.get(),
                         ElevatorConstants.KD.get(),
-                        0.02));
+                        ElevatorConstants.TRAPEZOID_PROFILE));
+
+        motor = new TalonFXSim(2, ElevatorConstants.GEAR_RATIO, 0.5, 1);
     }
 
     @Override
     public void setPower(double power) {
-        powerRequest.withOutput(power);
-        motor.setControl(powerRequest);
+        motor.setControl(powerRequest.withOutput(power));
     }
 
     @Override
     public void setHeight(MutableMeasure<Distance> height) {
-        positionRequest.withPosition(height.in(Meters));
-        motor.setControl(positionRequest);
+        motor.setControl(positionRequest.withPosition(height.in(Meters)));
     }
 
     @Override
     public void updateInputs(ElevatorInputs inputs) {
         motor.update(Timer.getFPGATimestamp());
-        inputs.currentHeight =
-                Meters.of(
-                                lib.units.Units.rpsToMetersPerSecond(
-                                        motor.getPosition(), ElevatorConstants.DRUM_RADIUS))
-                        .mutableCopy();
+
+        inputs.carriageHeight = Meters.of(motor.getPosition()).mutableCopy();
+        inputs.gripperHeight.mut_replace(
+                inputs.carriageHeight.gt(ElevatorConstants.GRIPPER_HEIGHT)
+                        ? inputs.carriageHeight
+                                .mutableCopy()
+                                .mut_minus(ElevatorConstants.GRIPPER_HEIGHT)
+                        : Meters.zero());
     }
 }

src/main/java/frc/robot/subsystems/elevator/ElevatorInputs.java

@@ -9,7 +9,8 @@
 @AutoLog
 public class ElevatorInputs {
     public double power = 0;
-    public MutableMeasure<Distance> currentHeight = Units.Meters.of(0).mutableCopy();
+    public MutableMeasure<Distance> carriageHeight = Units.Meters.of(0).mutableCopy();
+    public MutableMeasure<Distance> gripperHeight = Units.Meters.of(0).mutableCopy();
     public MutableMeasure<Distance> heightSetpoint = Units.Meters.of(0).mutableCopy();
     public MutableMeasure<Velocity<Velocity<Distance>>> acceleration =
             Units.MetersPerSecondPerSecond.of(0).mutableCopy();