Revert "[Refactor] change superstructure into robot container"

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

@@ -17,11 +17,11 @@
 import frc.robot.auto.basics.AutoActions;
 import frc.robot.commands.*;
 import frc.robot.display.Display;
-//import frc.robot.subsystems.Superstructure;
+import frc.robot.subsystems.Superstructure;
 import frc.robot.subsystems.apriltagvision.AprilTagVision;
 import frc.robot.subsystems.apriltagvision.AprilTagVisionIONorthstar;
 import frc.robot.subsystems.beambreak.BeambreakIOReal;
-import frc.robot.subsystems.elevator.ElevatorIOTalonFX;
+import frc.robot.subsystems.elevator.ElevatorIOReal;
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import frc.robot.subsystems.endeffector.EndEffectorIOReal;
 import frc.robot.subsystems.endeffector.EndEffectorSubsystem;
@@ -61,10 +61,10 @@ public class RobotContainer {
             new AprilTagVisionIONorthstar(this::getAprilTagLayoutType, 1));
     Swerve swerve = Swerve.getInstance();
     Display display = Display.getInstance();
-    ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem(new ElevatorIOTalonFX());
+    ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem(new ElevatorIOReal());
     EndEffectorSubsystem endEffectorSubsystem = new EndEffectorSubsystem(new EndEffectorIOReal(), new BeambreakIOReal(ENDEFFECTOR_MIDDLE_BEAMBREAK_ID), new BeambreakIOReal(ENDEFFECTOR_EDGE_BEAMBREAK_ID));
 
-    //Superstructure superstructure =  new Superstructure(endEffectorSubsystem);
+    Superstructure superstructure =  new Superstructure(endEffectorSubsystem);
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -132,11 +132,17 @@ private void configureOperatorBindings(CommandXboxController operatorController)
     private void configureTesterBindings(CommandXboxController controller) {
         //test of endeffector state machine
         new Trigger(controller.leftBumper())
-                .onTrue(endEffectorSubsystem.setWantedSuperStateCommand(EndEffectorSubsystem.WantedState.FUNNEL_INTAKE));
-        new Trigger(controller.rightTrigger())
-                .onTrue(endEffectorSubsystem.setWantedSuperStateCommand(EndEffectorSubsystem.WantedState.SHOOT));
+                .onTrue(superstructure.setWantedSuperStateCommand(Superstructure.WantedSuperState.INTAKE_CORAL_FUNNEL));
+        new Trigger(controller.rightBumper())
+                .onTrue(superstructure.setWantedSuperStateCommand(Superstructure.WantedSuperState.SHOOT_CORAL));
         new Trigger(controller.start())
-                .onTrue(endEffectorSubsystem.setWantedSuperStateCommand(EndEffectorSubsystem.WantedState.IDLE));
+                .onTrue(superstructure.setWantedSuperStateCommand(Superstructure.WantedSuperState.STOPPED));
+        //test of elevator heights
+        controller.a().onTrue(Commands.runOnce(() -> elevatorSubsystem.setPosition(L1_EXTENSION_METERS.get()),elevatorSubsystem).until(() ->elevatorSubsystem.isAtSetpoint(L1_EXTENSION_METERS.get())));
+        controller.b().onTrue(Commands.runOnce(() -> elevatorSubsystem.setPosition(L2_EXTENSION_METERS.get()),elevatorSubsystem).until(() ->elevatorSubsystem.isAtSetpoint(L2_EXTENSION_METERS.get())));
+        controller.x().onTrue(Commands.runOnce(() -> elevatorSubsystem.setPosition(L3_EXTENSION_METERS.get()),elevatorSubsystem).until(() ->elevatorSubsystem.isAtSetpoint(L3_EXTENSION_METERS.get())));
+        controller.y().onTrue(Commands.runOnce(() -> elevatorSubsystem.setPosition(L4_EXTENSION_METERS.get()),elevatorSubsystem).until(() ->elevatorSubsystem.isAtSetpoint(L4_EXTENSION_METERS.get())));
+        controller.povDown().onTrue(new ElevatorZeroingCommand(elevatorSubsystem));
     }
 
     /**

src/main/java/frc/robot/commands/ElevatorZeroingCommand.java

@@ -0,0 +1,39 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Robot;
+import frc.robot.subsystems.elevator.*;
+import edu.wpi.first.math.MathUtil;
+import frc.robot.RobotConstants;
+
+
+public class ElevatorZeroingCommand extends Command {
+    private ElevatorSubsystem elevator;
+
+    public ElevatorZeroingCommand(ElevatorSubsystem elevator) {
+        this.elevator = elevator;
+    }
+
+    @Override
+    public void initialize() {
+
+    }
+
+    @Override
+    public void execute(){
+        elevator.setVoltage(-1);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return Math.abs(elevator.getLeaderCurrent()) > RobotConstants.ElevatorConstants.ELEVATOR_ZEROING_CURRENT.get();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        elevator.resetPosition();
+        elevator.setVoltage(0);
+    }
+
+
+}

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

@@ -1,46 +1,37 @@
 package frc.robot.subsystems.elevator;
 
-import edu.wpi.first.units.*;
 import org.littletonrobotics.junction.AutoLog;
 
-import static edu.wpi.first.units.Units.*;
-
 public interface ElevatorIO {
-    void updateInputs(ElevatorIOInputs inputs);
-
-    void setElevatorDirectVoltage(double volts);
-
-    void setElevatorTarget(double meters);
+    @AutoLog
+    class ElevatorIOInputs {
+        public double positionMeters = 0.0;
+        public double velocityMetersPerSec = 0.0;
+        public double motionMagicVelocityTarget = 0.0;
+        public double motionMagicPositionTarget = 0.0;
+        public double setpointMeters = 0.0;
+        public double[] appliedVolts = new double[] {}; // {leader, follower}
+        public double[] statorCurrentAmps = new double[] {}; // {leader, follower}
+        public double[] supplyCurrentAmps = new double[] {}; // {leader, follower}
+        public double[] tempCelsius = new double[] {}; // {leader, follower}
+        // public double acceleration = 0.0; may add later
+    }
 
-    void resetElevatorPosition();
+    public default void updateInputs(ElevatorIOInputs inputs) {}
 
-    double getElevatorVelocity();
+    public default void setVoltage(double voltage) {}
 
-    double getElevatorPosition();
+    public default void setPosition(double meters) {}
 
-    boolean isNearExtension(double expected);
+    public default void stop() {
+        setVoltage(0.0);
+    }
 
-    boolean isCurrentMax(double expected);
+    public default void resetEncoder(double position) {}
 
-    @AutoLog
-    class ElevatorIOInputs {
-        public Measure<AngularVelocityUnit> leftElevatorVelocity = RadiansPerSecond.zero();
-        public Measure<AngleUnit> leftElevatorPosition = Radians.zero();
-        public Measure<VoltageUnit> leftElevatorAppliedVoltage = Volts.zero();
-        public Measure<CurrentUnit> leftElevatorSupplyCurrent = Amps.zero();
-
-        public Measure<AngularVelocityUnit> rightElevatorVelocity = RadiansPerSecond.zero();
-        public Measure<AngleUnit> rightElevatorPosition = Radians.zero();
-        public Measure<VoltageUnit> rightElevatorAppliedVoltage = Volts.zero();
-        public Measure<CurrentUnit> rightElevatorSupplyCurrent = Amps.zero();
-
-        public Measure<AngularVelocityUnit> targetElevatorVelocity = RadiansPerSecond.zero();
-
-        public double ElevatorKP = frc.robot.RobotConstants.ElevatorConstants.ElevatorGainsClass.ELEVATOR_KP.get();
-        public double ElevatorKI = frc.robot.RobotConstants.ElevatorConstants.ElevatorGainsClass.ELEVATOR_KI.get();
-        public double ElevatorKD = frc.robot.RobotConstants.ElevatorConstants.ElevatorGainsClass.ELEVATOR_KD.get();
-        public double ElevatorKA = frc.robot.RobotConstants.ElevatorConstants.ElevatorGainsClass.ELEVATOR_KA.get();
-        public double ElevatorKV = frc.robot.RobotConstants.ElevatorConstants.ElevatorGainsClass.ELEVATOR_KV.get();
-        public double ElevatorKS = frc.robot.RobotConstants.ElevatorConstants.ElevatorGainsClass.ELEVATOR_KS.get();
+    public default void resetEncoder() {
+        resetEncoder(0.0);
     }
+
+    public default void resetPosition() {}
 }

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

@@ -0,0 +1,186 @@
+package frc.robot.subsystems.elevator;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MotionMagicConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfigurator;
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.units.measure.*;
+import frc.robot.RobotConstants;
+
+import static frc.robot.RobotConstants.*;
+import static frc.robot.RobotConstants.ElevatorConstants.*;
+import static frc.robot.RobotConstants.ElevatorGainsClass.*;
+
+public class ElevatorIOReal implements ElevatorIO {
+    /* Hardware */
+    private final TalonFX leader;
+    private final TalonFX follower;
+
+    /* Configurators */
+    private TalonFXConfigurator leaderConfigurator;
+    private TalonFXConfigurator followerConfigurator;
+
+    /* Configs */
+    private final CurrentLimitsConfigs currentLimitsConfigs;
+    private final MotorOutputConfigs leaderMotorConfigs;
+    private final MotorOutputConfigs followerMotorConfigs;
+    private final Slot0Configs slot0Configs;
+    private final MotionMagicConfigs motionMagicConfigs;
+
+    private final MotionMagicVoltage motionRequest = new MotionMagicVoltage(0.0).withEnableFOC(true);
+    private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true);
+
+    private final StatusSignal<Voltage> voltageLeft;
+    private final StatusSignal<Voltage> voltageRight;
+    private final StatusSignal<Current> statorLeft;
+    private final StatusSignal<Current> statorRight;
+    private final StatusSignal<Current> supplyLeft;
+    private final StatusSignal<Current> supplyRight;
+    private final StatusSignal<Temperature> tempLeft;
+    private final StatusSignal<Temperature> tempRight;
+
+    public ElevatorIOReal() {
+        this.leader = new TalonFX(LEFT_ELEVATOR_MOTOR_ID, CANIVORE_CAN_BUS_NAME);
+        this.follower = new TalonFX(RIGHT_ELEVATOR_MOTOR_ID, CANIVORE_CAN_BUS_NAME);
+
+        this.leaderConfigurator = leader.getConfigurator();
+        this.followerConfigurator = follower.getConfigurator();
+
+        currentLimitsConfigs = new CurrentLimitsConfigs();
+        currentLimitsConfigs.StatorCurrentLimitEnable = true;
+        currentLimitsConfigs.SupplyCurrentLimitEnable = true;
+        currentLimitsConfigs.StatorCurrentLimit = 80.0;
+        currentLimitsConfigs.SupplyCurrentLimit = 30.0;
+
+        leaderMotorConfigs = new MotorOutputConfigs();
+        leaderMotorConfigs.NeutralMode = NeutralModeValue.Brake;
+        leaderMotorConfigs.Inverted = InvertedValue.Clockwise_Positive;
+        followerMotorConfigs = new MotorOutputConfigs();
+        followerMotorConfigs.NeutralMode = NeutralModeValue.Brake;
+
+        motionMagicConfigs = new MotionMagicConfigs();
+        motionMagicConfigs.MotionMagicAcceleration = motionAcceleration.get();
+        motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocity.get();
+        motionMagicConfigs.MotionMagicJerk = motionJerk.get();
+
+        slot0Configs = new Slot0Configs();
+        slot0Configs.kA = ELEVATOR_KA.get();
+        slot0Configs.kS = ELEVATOR_KS.get();
+        slot0Configs.kV = ELEVATOR_KV.get();
+        slot0Configs.kP = ELEVATOR_KP.get();
+        slot0Configs.kI = ELEVATOR_KI.get();
+        slot0Configs.kD = ELEVATOR_KD.get();
+
+        leader.setPosition(0.0);
+        follower.setPosition(0.0);
+
+        leaderConfigurator.apply(currentLimitsConfigs);
+        leaderConfigurator.apply(leaderMotorConfigs);
+        leaderConfigurator.apply(slot0Configs);
+        leaderConfigurator.apply(motionMagicConfigs);
+        followerConfigurator.apply(currentLimitsConfigs);
+        followerConfigurator.apply(followerMotorConfigs);
+        followerConfigurator.apply(slot0Configs);
+        followerConfigurator.apply(motionMagicConfigs);
+
+        voltageLeft = leader.getSupplyVoltage();
+        voltageRight = follower.getSupplyVoltage();
+        statorLeft = leader.getStatorCurrent();
+        statorRight = follower.getStatorCurrent();
+        supplyLeft = leader.getSupplyCurrent();
+        supplyRight = follower.getSupplyCurrent();
+        tempLeft = leader.getDeviceTemp();
+        tempRight = follower.getDeviceTemp();
+
+        follower.setControl(new Follower(leader.getDeviceID(), true));
+    }
+
+    @Override
+    public void updateInputs(ElevatorIOInputs inputs) {
+        BaseStatusSignal.refreshAll(
+                voltageLeft, voltageRight,
+                statorLeft, statorRight,
+                supplyLeft, supplyRight,
+                tempLeft, tempRight
+        );
+
+        inputs.positionMeters = getHeight();
+        inputs.velocityMetersPerSec = getVelocity();
+        inputs.motionMagicVelocityTarget = rotationsToMeters(leader.getClosedLoopReferenceSlope().getValue());
+        inputs.motionMagicPositionTarget = rotationsToMeters(leader.getClosedLoopReference().getValue());
+        inputs.appliedVolts = new double[] { voltageLeft.getValueAsDouble(), voltageRight.getValueAsDouble() };
+        inputs.statorCurrentAmps = new double[] { statorLeft.getValueAsDouble(), statorRight.getValueAsDouble() };
+        inputs.supplyCurrentAmps = new double[] { supplyLeft.getValueAsDouble(), supplyRight.getValueAsDouble() };
+        inputs.tempCelsius = new double[] { tempLeft.getValueAsDouble(), tempRight.getValueAsDouble() };
+
+        if (RobotConstants.TUNING) {
+            slot0Configs.kA = ELEVATOR_KA.get();
+            slot0Configs.kS = ELEVATOR_KS.get();
+            slot0Configs.kV = ELEVATOR_KV.get();
+            slot0Configs.kP = ELEVATOR_KP.get();
+            slot0Configs.kI = ELEVATOR_KI.get();
+            slot0Configs.kD = ELEVATOR_KD.get();
+            slot0Configs.kG = ELEVATOR_KG.get();
+
+            motionMagicConfigs.MotionMagicAcceleration = motionAcceleration.get();
+            motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocity.get();
+            motionMagicConfigs.MotionMagicJerk = motionJerk.get();
+
+            leaderConfigurator.apply(slot0Configs);
+            followerConfigurator.apply(slot0Configs);
+            leaderConfigurator.apply(motionMagicConfigs);
+            followerConfigurator.apply(motionMagicConfigs);
+        }
+    }
+
+    @Override
+    public void setPosition(double heightMeters) {
+        leader.setControl(motionRequest.withPosition(metersToRotations(heightMeters)));
+    }
+
+    @Override
+    public void setVoltage(double voltage) {
+        leader.setControl(voltageOut.withOutput(voltage));
+    }
+
+    @Override
+    public void resetEncoder(final double position) {
+        leader.setPosition(position);
+        follower.setPosition(position);
+    }
+
+
+
+    @Override
+    public void resetPosition(){
+        leader.setPosition(0.0);
+        follower.setPosition(0.0);
+    }
+
+
+    private double getHeight() {
+        return rotationsToMeters(leader.getPosition().getValueAsDouble());
+    }
+
+    private double getVelocity() {
+        return rotationsToMeters(leader.getVelocity().getValueAsDouble());
+    }
+
+    private double metersToRotations(double heightMeters) {
+        return (heightMeters / (Math.PI * ELEVATOR_SPOOL_DIAMETER)) * ELEVATOR_GEAR_RATIO;
+    }
+
+    private double rotationsToMeters(double rotations) {
+        return rotations * (Math.PI * ELEVATOR_SPOOL_DIAMETER) / ELEVATOR_GEAR_RATIO;
+    }
+
+}