Elevator wrist subsystem

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

@@ -7,20 +7,56 @@ public final class Constants {
     /**
      * Stick Deadband
      */
-    public static final double stickDeadband = 0.1;
+    public static final double STICK_DEADBAND = 0.1;
     /**
      * Driver ID
      */
-    public static final int driverID = 0;
+    public static final int DRIVER_ID = 0;
     /**
      * Operator ID
      */
-    public static final int operatorID = 1;
+    public static final int OPERATOR_ID = 1;
 
     /**
      * Motor CAN id's.
      */
     public static final class Motors {
+        /**
+         * Class for elevator and wrist motor constants
+         */
+        public static final class ElevatorWrist {
+            public static final int TALON_ID = -1;
+            public static final int NEO_ID = -1;
+        }
+    }
+
+    /**
+     * Class for elevator and wrist constants
+     */
+    public static final class ElevatorWristConstants {
+        public static final int ELEVATOR_ENC_CHANNEL_A = -1;
+        public static final int ELEVATOR_ENC_CHANNEL_B = -1;
+        public static final int TOP_LIMIT_SWITCH_PORT = -1;
+        public static final int BOTTOM_LIMIT_SWITCH_PORT = -1;
+
+        public static final double ELEVATOR_KP = 0;
+        public static final double ELEVATOR_KI = 0;
+        public static final double ELEVATOR_KD = 0;
+        public static final double ELEVATOR_MAX_VELOCITY = 0;
+        public static final double ELEVATOR_MAX_ACCELERATION = 0;
+        public static final double ELEVATOR_KS = 0;
+        public static final double ELEVATOR_KG = 0;
+        public static final double ELEVATOR_KV = 0;
+
+
+        public static final double WRIST_KP = 0;
+        public static final double WRIST_KI = 0;
+        public static final double WRIST_KD = 0;
+        public static final double WRIST_MAX_VELOCITY = 0;
+        public static final double WRIST_MAX_ACCELERATION = 0;
+        public static final double WRIST_KS = 0;
+        public static final double WRIST_KG = 0;
+        public static final double WRIST_KV = 0;
     }
 
     /**

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

@@ -21,8 +21,8 @@
  */
 public class RobotContainer {
     /* Controllers */
-    private final CommandXboxController driver = new CommandXboxController(Constants.driverID);
-    private final CommandXboxController operator = new CommandXboxController(Constants.operatorID);
+    private final CommandXboxController driver = new CommandXboxController(Constants.DRIVER_ID);
+    private final CommandXboxController operator = new CommandXboxController(Constants.OPERATOR_ID);
 
     // Initialize AutoChooser Sendable
     private final SendableChooser<String> autoChooser = new SendableChooser<>();

src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWrist.java

@@ -0,0 +1,63 @@
+package frc.robot.subsystems.elevator_wrist;
+
+import org.littletonrobotics.junction.Logger;
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import frc.robot.Constants;
+
+/**
+ * Elevator and Wrist Subsystem
+ */
+public class ElevatorWrist implements Subsystem {
+    public ElevatorWristIO io;
+    public ElevatorWristInputsAutoLogged inputs = new ElevatorWristInputsAutoLogged();
+
+
+    ProfiledPIDController elevatorPIDController = new ProfiledPIDController(
+        Constants.ElevatorWristConstants.ELEVATOR_KP, Constants.ElevatorWristConstants.ELEVATOR_KI,
+        Constants.ElevatorWristConstants.ELEVATOR_KD,
+        new TrapezoidProfile.Constraints(Constants.ElevatorWristConstants.ELEVATOR_MAX_VELOCITY,
+            Constants.ElevatorWristConstants.ELEVATOR_MAX_ACCELERATION));
+
+    ProfiledPIDController wristPIDController =
+        new ProfiledPIDController(Constants.ElevatorWristConstants.WRIST_KP,
+            Constants.ElevatorWristConstants.WRIST_KI, Constants.ElevatorWristConstants.WRIST_KD,
+            new TrapezoidProfile.Constraints(Constants.ElevatorWristConstants.WRIST_MAX_VELOCITY,
+                Constants.ElevatorWristConstants.WRIST_MAX_ACCELERATION));
+
+
+    private ElevatorFeedforward elevatorFeedForward = new ElevatorFeedforward(
+        Constants.ElevatorWristConstants.ELEVATOR_KS, Constants.ElevatorWristConstants.ELEVATOR_KG,
+        Constants.ElevatorWristConstants.ELEVATOR_KV);
+
+    private ArmFeedforward wristFeedForward =
+        new ArmFeedforward(Constants.ElevatorWristConstants.WRIST_KS,
+            Constants.ElevatorWristConstants.WRIST_KG, Constants.ElevatorWristConstants.WRIST_KV);
+
+    public ElevatorWrist(ElevatorWristIO io) {
+        this.io = io;
+    }
+
+    @Override
+    public void periodic() {
+
+        io.updateInputs(inputs);
+        Logger.processInputs("ElevatorWrist", inputs);
+
+        double elevatorPIDValue =
+            elevatorPIDController.calculate(inputs.elevatorRelativeEncRawValue);
+        double wristPIDValue = wristPIDController.calculate(inputs.wristAbsoluteEncRawValue);
+
+        double elevatorFeedForwardValue =
+            elevatorFeedForward.calculate(0, 0, wristPIDController.getPeriod());
+
+        double wristFeedForwardValue =
+            wristFeedForward.calculate(0, 0, wristPIDController.getPeriod());
+
+        io.setElevatorVoltage(elevatorFeedForwardValue + elevatorPIDValue);
+        io.setWristVoltage(wristFeedForwardValue + wristPIDValue);
+    }
+}

src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWristIO.java

@@ -0,0 +1,28 @@
+package frc.robot.subsystems.elevator_wrist;
+
+import org.littletonrobotics.junction.AutoLog;
+
+/**
+ * Elevator and wrist IO class
+ */
+public interface ElevatorWristIO {
+    /**
+     * Elevator and wrist inputs
+     */
+    @AutoLog
+
+    public static class ElevatorWristInputs {
+        public double elevatorRelativeEncRawValue;
+        public boolean topLimitSwitch;
+        public boolean bottomLimitSwitch;
+
+        public double wristAbsoluteEncRawValue;
+    }
+
+    public default void updateInputs(ElevatorWristInputs inputs) {}
+
+    public default void setElevatorVoltage(double voltage) {}
+
+    public default void setWristVoltage(double voltage) {}
+
+}

src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWristReal.java

@@ -0,0 +1,70 @@
+package frc.robot.subsystems.elevator_wrist;
+
+
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.SparkAbsoluteEncoder.Type;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.Encoder;
+import frc.robot.Constants;
+
+/**
+ * Elevator and wrist real robot class
+ */
+public class ElevatorWristReal implements ElevatorWristIO {
+    public final TalonFX elevatorMotor = new TalonFX(Constants.Motors.ElevatorWrist.TALON_ID);
+
+    public final Encoder elevatorRelativeEnc =
+        new Encoder(Constants.ElevatorWristConstants.ELEVATOR_ENC_CHANNEL_A,
+            Constants.ElevatorWristConstants.ELEVATOR_ENC_CHANNEL_B);
+
+    public final CANSparkMax wristMotor =
+        new CANSparkMax(Constants.Motors.ElevatorWrist.NEO_ID, MotorType.kBrushless);
+    public final DigitalInput topLimitSwitch =
+        new DigitalInput(Constants.ElevatorWristConstants.TOP_LIMIT_SWITCH_PORT);
+    public final DigitalInput bottomLimitSwitch =
+        new DigitalInput(Constants.ElevatorWristConstants.BOTTOM_LIMIT_SWITCH_PORT);
+
+    public final AbsoluteEncoder wristAbsoluteEnc;
+
+    private VoltageOut voltage = new VoltageOut(0);
+
+    /**
+     * Constructor for elevator wrist real class
+     */
+    public ElevatorWristReal() {
+        wristAbsoluteEnc = wristMotor.getAbsoluteEncoder(Type.kDutyCycle);
+
+        elevatorMotor.setNeutralMode(NeutralModeValue.Brake);
+        elevatorMotor.setInverted(false);
+
+        wristMotor.setIdleMode(IdleMode.kBrake);
+        wristMotor.setInverted(false);
+
+    }
+
+
+    @Override
+    public void updateInputs(ElevatorWristInputs inputs) {
+        inputs.topLimitSwitch = topLimitSwitch.get();
+        inputs.bottomLimitSwitch = bottomLimitSwitch.get();
+        inputs.elevatorRelativeEncRawValue = elevatorRelativeEnc.get();
+        inputs.wristAbsoluteEncRawValue = wristAbsoluteEnc.getPosition();
+    }
+
+    @Override
+    public void setElevatorVoltage(double v) {
+        elevatorMotor.setControl(voltage.withOutput(v));
+    }
+
+    @Override
+    public void setWristVoltage(double v) {
+        wristMotor.setVoltage(v);
+    }
+
+}