Merge pull request #3 from FRC-7525/Elevator

Elevator

.github/scripts/constants.py

@@ -3,7 +3,7 @@
 import sys
 
 # List of files to excuse (constants and things we didnt make and stuff we wont use)
-excused_files = ["Constants.java", "Drive.java", "TunerConstants.java", "BuildConstants.java", "LocalADStarAK.java", "VisionUtil.java", "SwerveModule.java", "VisionIOSim.java"]
+excused_files = ["GlobalConstants.java", "Drive.java", "TunerConstants.java", "BuildConstants.java", "LocalADStarAK.java", "VisionUtil.java", "SwerveModule.java", "VisionIOSim.java", "ElevatorConstants.java", "IntakeConstants.java"]
 
 # Not really dirs becasue the full ones didnt work
 excused_dirs = [

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

@@ -0,0 +1,46 @@
+package frc.robot.subsystems.Elevator;
+
+import static frc.robot.GlobalConstants.ROBOT_MODE;
+import static frc.robot.subsystems.Elevator.ElevatorConstants.SUBSYSTEM_NAME;
+
+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(SUBSYSTEM_NAME, 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() {
+        if (!io.isZeroed()) {
+            io.zero();
+        } else {
+            io.setHeightGoalpoint(getState().getTargetHeight());
+            io.runElevator();
+        }
+
+        io.updateInputs(inputs);
+        Logger.processInputs(ElevatorConstants.SUBSYSTEM_NAME, inputs);
+    }
+
+}

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

@@ -0,0 +1,70 @@
+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 static edu.wpi.first.units.Units.MetersPerSecond;
+
+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.LinearVelocity;
+import edu.wpi.first.units.measure.Mass;
+import frc.robot.pioneersLib.controlConstants.FFConstants;
+
+public final class ElevatorConstants {
+    public static final String SUBSYSTEM_NAME = "Elevator";
+
+    public static final int LEFT_MOTOR_CANID = 12;
+    public static final int RIGHT_MOTOR_CANID = 13;
+
+    public static final Distance POSITION_TOLERANCE = Meters.of(.1);
+    public static final LinearVelocity VELOCITY_TOLERANCE = MetersPerSecond.of(.1);
+    public static final Constraints TRAPEZOID_PROFILE_CONSTRAINTS = new TrapezoidProfile.Constraints(1, .5);
+    public static final LinearVelocity ZEROING_VELOCITY = MetersPerSecond.of(0.25);
+    public static final Current ZEROING_CURRENT_LIMIT = Amps.of(10.0);
+
+    public static final Distance HIGH_POSITION_HEIGHT = Meters.of(2.5146);
+    public static final Distance MID_POSITION_HEIGHT = Meters.of(1.54305); //mid is all the way down according to nick
+    public static final Distance IDLE_POSITION_HEIGHT = Meters.of(1.54305);
+    public static final Distance METERS_PER_ROTATION = Meters.of(1); // random value lol
+
+    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 = false;
+        public static final Distance STARTING_HEIGHT = Meters.of(1.54305);
+        //idk these values
+        public static final PIDConstants PROFILLED_PID_CONSTANTS = new PIDConstants(2, 0, 0, 0);
+
+        public static final FFConstants FF_CONSTANTS = new FFConstants(.2, .2, 0, 0);
+
+    }
+
+    public static class Real {
+            public static final int LIMIT_SWITCH_DIO = 1;
+
+
+            public static final boolean LEFT_INVERTED = false;
+            public static final NeutralModeValue LEFT_NEUTRAL_MODE = NeutralModeValue.Brake;
+            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.Brake;
+            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,32 @@
+package frc.robot.subsystems.Elevator;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface ElevatorIO {
+
+    @AutoLog
+    public static class ElevatorIOInputs {
+        public String stateString;
+        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 boolean elevatorZeroed;
+    }
+
+    public void setHeightGoalpoint(double height);
+
+    public void updateInputs(ElevatorIOInputs inputs);
+
+    public void runElevator();
+
+    public void zero();
+
+    public boolean isZeroed();
+
+    public boolean nearTarget();
+}

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

@@ -0,0 +1,141 @@
+package frc.robot.subsystems.Elevator;
+
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static frc.robot.GlobalConstants.ROBOT_MODE;
+import static frc.robot.subsystems.Elevator.ElevatorConstants.Real.*;
+import static frc.robot.subsystems.Elevator.ElevatorConstants.*;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfigurator;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.GlobalConstants.RobotMode;
+
+
+public class ElevatorIOReal implements ElevatorIO {
+
+    private TalonFX leftMotor;
+    private TalonFXConfigurator leftConfigurator;
+    private TalonFXConfiguration leftConfigurations;
+
+    private TalonFX rightMotor;
+    private TalonFXConfigurator rightConfigurator;
+    private TalonFXConfiguration rightConfigurations;
+
+    private ProfiledPIDController pidController;
+    private ElevatorFeedforward ffcontroller;
+    private DigitalInput limitSwitch;
+
+    private double metersPerRotation;
+    private double leftMotorVoltage;
+    private double rightMotorVoltage;
+    private boolean leftMotorZeroed;
+    private boolean rightMotorZeroed;
+
+    public ElevatorIOReal() {
+        leftMotor = new TalonFX(LEFT_MOTOR_CANID);
+        rightMotor = new TalonFX(RIGHT_MOTOR_CANID);
+        limitSwitch = new DigitalInput(LIMIT_SWITCH_DIO);
+
+        leftMotorVoltage = 0;
+        rightMotorVoltage = 0;
+        leftMotorZeroed = false;
+        rightMotorZeroed = false;
+        metersPerRotation = METERS_PER_ROTATION.magnitude();
+
+        //Motor configs
+        leftConfigurator = leftMotor.getConfigurator();
+        rightConfigurator = rightMotor.getConfigurator();
+
+        leftConfigurations = new TalonFXConfiguration();
+        rightConfigurations = new TalonFXConfiguration();
+
+        leftConfigurations.MotorOutput.Inverted = LEFT_INVERTED ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive;
+        leftConfigurations.MotorOutput.NeutralMode = LEFT_NEUTRAL_MODE;
+        leftConfigurations.CurrentLimits.StatorCurrentLimitEnable = LEFT_STRATOR_CURRENT_LIMIT_ENABLED;
+        leftConfigurations.CurrentLimits.StatorCurrentLimit = LEFT_STRATOR_CURRENT_LIMIT.magnitude();
+        leftConfigurator.apply(leftConfigurations);
+
+        rightConfigurations.MotorOutput.Inverted = RIGHT_INVERTED ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive;
+        rightConfigurations.MotorOutput.NeutralMode = RIGHT_NEUTRAL_MODE;
+        rightConfigurations.CurrentLimits.StatorCurrentLimitEnable = RIGHT_STRATOR_CURRENT_LIMIT_ENABLED;
+        rightConfigurations.CurrentLimits.StatorCurrentLimit = RIGHT_STRATOR_CURRENT_LIMIT.magnitude();
+        rightConfigurator.apply(rightConfigurations);
+
+        //PID and FF controller setup
+        pidController = new ProfiledPIDController(PROFILLED_PID_CONSTANTS.kP, PROFILLED_PID_CONSTANTS.kI,
+        PROFILLED_PID_CONSTANTS.kD, ElevatorConstants.TRAPEZOID_PROFILE_CONSTRAINTS);
+        pidController.setTolerance(ElevatorConstants.POSITION_TOLERANCE.magnitude(), ElevatorConstants.VELOCITY_TOLERANCE.magnitude());
+        pidController.setIZone(PROFILLED_PID_CONSTANTS.iZone);
+
+        ffcontroller = new ElevatorFeedforward(FF_CONSTANTS.kS,
+        FF_CONSTANTS.kG, FF_CONSTANTS.kV,
+        FF_CONSTANTS.kA);
+
+        if (ROBOT_MODE == RobotMode.TESTING) {
+            SmartDashboard.putData("Elevator PID controller", pidController);
+        }
+    }
+
+    @Override
+    public void setHeightGoalpoint(double height) {
+        pidController.setGoal(height);
+    }
+
+    @Override
+    public void updateInputs(ElevatorIOInputs inputs) {
+        inputs.currentElevatorHeight = leftMotor.getPosition().getValueAsDouble() * metersPerRotation;
+        inputs.elevatorHeightSetpoint = pidController.getSetpoint().position;
+        inputs.elevatorHeightGoalpoint = pidController.getGoal().position;
+        inputs.elevatorVelocity = leftMotor.getVelocity().getValueAsDouble();
+        inputs.elevatorVelocitySetpoint = pidController.getSetpoint().velocity;
+        inputs.elevatorHeightGoalpoint = pidController.getGoal().velocity;
+        inputs.leftMotorVoltInput = leftMotorVoltage;
+        inputs.rightMotorVoltInput = rightMotorVoltage;
+    }
+
+    @Override
+    public void runElevator() {
+        leftMotorVoltage = pidController.calculate(leftMotor.getPosition().getValueAsDouble() * metersPerRotation) +
+        ffcontroller.calculate(MetersPerSecond.of(pidController.getSetpoint().velocity)).magnitude();
+        rightMotorVoltage = pidController.calculate(rightMotor.getPosition().getValueAsDouble() * metersPerRotation) +
+        ffcontroller.calculate(MetersPerSecond.of(pidController.getSetpoint().velocity)).magnitude();
+        leftMotor.setVoltage(leftMotorVoltage);
+        rightMotor.setVoltage(rightMotorVoltage);
+    }
+
+    @Override
+    public boolean nearTarget() {
+        return pidController.atGoal();
+    }
+
+    @Override
+    public void zero() {
+        double leftZeroingSpeed = -ElevatorConstants.ZEROING_VELOCITY.magnitude();
+        double rightZeroingSpeed = -ElevatorConstants.ZEROING_VELOCITY.magnitude();
+
+        if (rightMotor.getStatorCurrent().getValueAsDouble() > ElevatorConstants.ZEROING_CURRENT_LIMIT.magnitude() || !limitSwitch.get()) {
+            rightZeroingSpeed = 0;
+            if (!rightMotorZeroed) rightMotor.setPosition(0); rightMotorZeroed = true;
+        }
+
+        if (leftMotor.getStatorCurrent().getValueAsDouble() > ElevatorConstants.ZEROING_CURRENT_LIMIT.magnitude() || !limitSwitch.get()) {
+            leftZeroingSpeed = 0;
+            if (!leftMotorZeroed) leftMotor.setPosition(0); leftMotorZeroed = true;
+        }
+
+        rightMotor.set(rightZeroingSpeed);
+        leftMotor.set(leftZeroingSpeed);
+    }
+
+    @Override
+    public boolean isZeroed() {
+        return leftMotorZeroed && rightMotorZeroed;
+    }
+
+}