Aidan's elevator implementaion into arm wow ur so cool

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

@@ -15,6 +15,7 @@
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
@@ -25,10 +26,12 @@
 import frc.robot.commands.ArmCommands.ManualArmCommand;
 import frc.robot.commands.BaseDriveCommand;
 import frc.robot.commands.ArmCommands.BendCommand;
+import frc.robot.commands.CloseClawCommand;
 import frc.robot.commands.ElevatorCommands.ElevatorGoToOriginCommand;
 import frc.robot.commands.ElevatorCommands.ElevatorSetPointCommand;
 import frc.robot.commands.ElevatorCommands.ManualElevatorCommand;
 import frc.robot.commands.ElevatorCommands.ResetElevatorEncoderCommand;
+import frc.robot.commands.OpenClawCommand;
 import frc.robot.constants.Constants;
 import frc.robot.constants.ElevatorConstants;
 import frc.robot.constants.DriveConstants;
@@ -47,10 +50,7 @@
 import frc.robot.subsystems.AlgaeIntakeSubsystem.IntakeIOSparkMax;
 import frc.robot.subsystems.AlgaeIntakeSubsystem.IntakeIOSim;
 import frc.robot.subsystems.LEDSubsytem.LEDSubsystem;
-import frc.robot.subsystems.SingleJointedArmSubsystem.SingleJointedArmIOEncoder;
-import frc.robot.subsystems.SingleJointedArmSubsystem.SingleJointedArmIOSim;
-import frc.robot.subsystems.SingleJointedArmSubsystem.SingleJointedArmIOTalonFX;
-import frc.robot.subsystems.SingleJointedArmSubsystem.SingleJointedArmSubsystem;
+import frc.robot.subsystems.SingleJointedArmSubsystem.*;
 import frc.robot.subsystems.VisionSubsystem.VisionIO;
 import frc.robot.subsystems.VisionSubsystem.VisionIOPhotonVision;
 import frc.robot.subsystems.VisionSubsystem.VisionIOPhotonVisionSim;
@@ -62,8 +62,6 @@
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
 
-import java.util.function.DoubleSupplier;
-
 public class RobotContainer {
   // Subsystems
   public final DriveSubsystem drive;
@@ -113,8 +111,7 @@ public RobotContainer() {
         ledSubsystem =
                 new LEDSubsystem(); //TODO
         singleJointedArmSubsystem =
-                new SingleJointedArmSubsystem(new SingleJointedArmIOTalonFX()) {
-                }; //TODO
+                new SingleJointedArmSubsystem(new SingleJointedArmIOTalonFX(), new ArmEncoderIO() {}); //TODO
         visionSubsystem =
                 new VisionSubsystem(new VisionIOPhotonVision()); //TODO
         break;
@@ -143,7 +140,7 @@ public RobotContainer() {
         ledSubsystem =
                 new LEDSubsystem(); //TODO
         singleJointedArmSubsystem =
-                new SingleJointedArmSubsystem(new SingleJointedArmIOSim()); //TODO
+                new SingleJointedArmSubsystem(new SingleJointedArmIOSim(), new ArmEncoderIO() {}); //TODO
         visionSubsystem =
                 new VisionSubsystem(new VisionIOPhotonVisionSim(driveSimulation::getSimulatedDriveTrainPose)); //TODO
         break;
@@ -167,7 +164,7 @@ public RobotContainer() {
         ledSubsystem =
                 new LEDSubsystem(); //TODO
         singleJointedArmSubsystem =
-                new SingleJointedArmSubsystem(new SingleJointedArmIOSim()); //TODO
+                new SingleJointedArmSubsystem(new SingleJointedArmIO() {}, new ArmEncoderIO() {}); //TODO
         visionSubsystem =
                 new VisionSubsystem(new VisionIO() {}); //TODO
         break;

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

@@ -53,7 +53,7 @@ public void execute() {
         // Convert to field relative speeds & send command
         boolean isFlipped =
                 DriverStation.getAlliance().isPresent()
-                        && DriverStation.getAlliance().get() == Alliance.Red;
+                        && DriverStation.getAlliance().get() == DriverStation.Alliance.Red;
         drive.runVelocity(
                 ChassisSpeeds.fromFieldRelativeSpeeds(
                         linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),

src/main/java/frc/robot/constants/SingleJointedArmConstants.java

@@ -6,19 +6,27 @@ public class SingleJointedArmConstants {
     public static final int frontLeftDriveCanId = 1;
     public static final int MotorNumber = 0;
     public static final int EncoderNumber = 1;
-    public static final double kP = 3;
-    public static final double kD = 2;
-    public static final double kI = 0;
-    public static final double kV= 0.39;
-    public static final double kG = 1.696;
-    public static final double kS = 0;
+    public static final double kPSim = 3;
+    public static final double kDSim = 2;
+    public static final double kISim = 0;
+    public static final double kVSim= 0.39;
+    public static final double kGSim = 1.696;
+    public static final double kSSim = 0;
+    public static double kASim = 0;
+    public static double kS = 0;
+    public static double kV = 1;
+    public static double kG = 0;
+    public static double kA = 0;
+    public static double kP = 0;
+    public static double kI = 0;
+    public static double kD = 0;
     public static final double kArmToleranceRPS = 0;
     public static final double kEncoderDistancePerPulse = 0;
     public static final double MAX_ANGLE = Math.PI;
     public static final double MIN_ANGLE = -Math.PI;
     public static double SparkkP = 1.0;
     public static double SparkkD = 1.0;
-    public static double gearingRatio = 20.0;
+    public static double gearingRatio = 180.0;
     public static double armLength = 0.3;
     public static DCMotor armGearbox = DCMotor.getNEO(1);
     public static int armSparkMotorCurrentLimit = 1;
@@ -34,4 +42,6 @@ public class SingleJointedArmConstants {
     public static int arm1TalonID;
     public static int arm2TalonID;
 
+    public static int fullRange = 4;
+    public static int expectedZero = 2;
 }

src/main/java/frc/robot/subsystems/ElevatorSubsystem/ElevatorEncoderIO.java

@@ -1,7 +1,7 @@
 package frc.robot.subsystems.ElevatorSubsystem;
 
 import org.littletonrobotics.junction.AutoLog;
-import com.revrobotics.RelativeEncoder;
+
 public interface ElevatorEncoderIO {
     @AutoLog
     class ElevatorEncoderIOInputs {

src/main/java/frc/robot/subsystems/SingleJointedArmSubsystem/ArmEncoderIO.java

@@ -0,0 +1,18 @@
+package frc.robot.subsystems.SingleJointedArmSubsystem;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface ArmEncoderIO {
+    @AutoLog
+    class ArmEncoderIOInputs {
+        double unfilteredArmAngle = 0.0;
+        double armAngle = 0.0;
+        double encoderPositionRads = 0;
+        double encoderPositionRots = 0;
+        double encoderVelocityRads = 0;
+        double encoderVelocityRots = 0;
+    }
+    public default void updateInputs(ArmEncoderIO.ArmEncoderIOInputs inputs) {}
+    public default void resetEncoderPositionWithArmAngle() {}
+
+}

src/main/java/frc/robot/subsystems/SingleJointedArmSubsystem/ArmEncoderIOThroughbore.java

@@ -0,0 +1,34 @@
+package frc.robot.subsystems.SingleJointedArmSubsystem;
+
+import edu.wpi.first.math.filter.LinearFilter;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.constants.ElevatorConstants;
+import frc.robot.constants.SingleJointedArmConstants;
+import frc.robot.subsystems.ElevatorSubsystem.ElevatorEncoderIO;
+
+public class ArmEncoderIOThroughbore implements ArmEncoderIO{
+    private final DutyCycleEncoder armDCEncoder;
+    LinearFilter armFilter = LinearFilter.singlePoleIIR(0.1, 0.02);
+
+    public ArmEncoderIOThroughbore () {
+        armDCEncoder = new DutyCycleEncoder(0, SingleJointedArmConstants.fullRange, SingleJointedArmConstants.expectedZero);
+    }
+
+    @Override
+    public void updateInputs(ArmEncoderIO.ArmEncoderIOInputs inputs) {
+        inputs.encoderPositionRots = armDCEncoder.get();
+        inputs.encoderPositionRads =  Units.rotationsToRadians(inputs.encoderPositionRots);
+        inputs.encoderVelocityRots = armDCEncoder.get() * armDCEncoder.getFrequency();
+        inputs.encoderVelocityRads = Units.rotationsToRadians(inputs.encoderVelocityRads);
+
+        inputs.unfilteredArmAngle = inputs.encoderPositionRads / SingleJointedArmConstants.gearingRatio;
+        inputs.armAngle = armFilter.calculate(inputs.unfilteredArmAngle);
+    }
+    @Override
+    public void resetEncoderPositionWithArmAngle() {
+        armDCEncoder.close();
+    }
+}

src/main/java/frc/robot/subsystems/SingleJointedArmSubsystem/SingleJointedArmIO.java

@@ -1,11 +1,8 @@
 package frc.robot.subsystems.SingleJointedArmSubsystem;
 
-import frc.robot.subsystems.ElevatorSubsystem.ElevatorIO;
 import org.littletonrobotics.junction.AutoLog;
 
 public interface SingleJointedArmIO {
-    SingleJointedArmIO.ArmIOInputs ArmIOInputs = new SingleJointedArmIO.ArmIOInputs();
-
     @AutoLog
     class ArmIOInputs {
         public double armAngle;

src/main/java/frc/robot/subsystems/SingleJointedArmSubsystem/SingleJointedArmIOTalonFX.java

@@ -123,17 +123,17 @@ public SingleJointedArmIOTalonFX() {
         ParentDevice.optimizeBusUtilizationForAll(armTalon1, armTalon2);
 
     }
-    public void updateInputs(ArmIOInputs inputs) {
+    public void updateInputs(SingleJointedArmIO.ArmIOInputs inputs) {
         // Refresh all signals
         var arm1Status =
                 BaseStatusSignal.refreshAll(arm1Position, arm1Velocity, arm1AppliedVolts, arm1Current);
         var arm2Status =
                 BaseStatusSignal.refreshAll(arm2Position, arm2Velocity, arm2AppliedVolts, arm2Current);
 
         // Update drive inputs
-       inputs.arm1Connected = arm1ConnectedDebounce.calculate(arm1Status.isOK());
-       inputs.arm1PositionRad = Units.rotationsToRadians(arm1Position.getValueAsDouble());
-       inputs.arm1VelocityRadPerSec = Units.rotationsToRadians(arm1Velocity.getValueAsDouble());
+        inputs.arm1Connected = arm1ConnectedDebounce.calculate(arm1Status.isOK());
+        inputs.arm1PositionRad = Units.rotationsToRadians(arm1Position.getValueAsDouble());
+        inputs.arm1VelocityRadPerSec = Units.rotationsToRadians(arm1Velocity.getValueAsDouble());
         inputs.arm1AppliedVolts = arm1AppliedVolts.getValueAsDouble();
         inputs.arm1CurrentAmps = arm1Current.getValueAsDouble();
 

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

@@ -4,59 +4,70 @@
 
 import com.revrobotics.spark.SparkLowLevel;
 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 com.revrobotics.spark.SparkMax;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.commands.ArmCommands.ArmHoverCommand;
+import frc.robot.constants.Constants;
+import frc.robot.constants.ElevatorConstants;
 import frc.robot.constants.SingleJointedArmConstants;
 import frc.robot.subsystems.ElevatorSubsystem.ElevatorEncoderIO;
 import frc.robot.subsystems.ElevatorSubsystem.ElevatorEncoderIOInputsAutoLogged;
 import frc.robot.subsystems.ElevatorSubsystem.ElevatorIO;
 import frc.robot.subsystems.ElevatorSubsystem.ElevatorIOInputsAutoLogged;
-
-import java.beans.Encoder;
-
-import static frc.robot.subsystems.SingleJointedArmSubsystem.SingleJointedArmIO.ArmIOInputs;
+import frc.robot.subsystems.SingleJointedArmSubsystem.SingleJointedArmIO;
 
 
 public class SingleJointedArmSubsystem extends SubsystemBase {
-    private final SingleJointedArmIO singleJointedArmIO;
-    private final SingleJointedArmIOEncoder singleJointedArmIOEncoder;
+    private SingleJointedArmIO singleJointedArmIO;
+    public ArmFeedforward armFeedForward;
+    public final ProfiledPIDController armFeedback;
 
+    private final ArmEncoderIO armEncoderIO;
     private final ArmIOInputsAutoLogged armIOInputs;
-//    private final SingleJointedArmIOEncoderInputs encoderIOInputs;
-
-    public double hoverAngle = 6969; //TODO!!!!!!
-    public final ArmFeedforward armFeedForward = new ArmFeedforward(SingleJointedArmConstants.kS, SingleJointedArmConstants.kG, SingleJointedArmConstants.kV, 0);
-    public final ProfiledPIDController armFeedback = new ProfiledPIDController(SingleJointedArmConstants.kP, SingleJointedArmConstants.kI, SingleJointedArmConstants.kD, new TrapezoidProfile.Constraints(SingleJointedArmConstants.maxVelocity,SingleJointedArmConstants.maxAcceleration));
+    private ArmEncoderIOInputsAutoLogged armEncoderIOInputs = new ArmEncoderIOInputsAutoLogged();
+    public double hoverAngle = 6969;
+
+    public SingleJointedArmSubsystem(SingleJointedArmIO singleJointedArmIO, ArmEncoderIO armIOEncoder) {
+        if (Constants.currentMode == Constants.Mode.SIM) {
+            this.armFeedForward = new ArmFeedforward(SingleJointedArmConstants.kSSim, SingleJointedArmConstants.kGSim, SingleJointedArmConstants.kVSim, SingleJointedArmConstants.kASim);
+            this.armFeedback = new ProfiledPIDController(SingleJointedArmConstants.kPSim, SingleJointedArmConstants.kISim, SingleJointedArmConstants.kDSim, new TrapezoidProfile.Constraints(SingleJointedArmConstants.maxVelocity, SingleJointedArmConstants.maxAcceleration));
+        } else {
+            this.armFeedForward = new ArmFeedforward(SingleJointedArmConstants.kS, SingleJointedArmConstants.kG, SingleJointedArmConstants.kV, SingleJointedArmConstants.kA);
+            this.armFeedback = new ProfiledPIDController(SingleJointedArmConstants.kP, SingleJointedArmConstants.kI, SingleJointedArmConstants.kD, new TrapezoidProfile.Constraints(SingleJointedArmConstants.maxVelocity, SingleJointedArmConstants.maxAcceleration));
+        }
 
-    public SingleJointedArmSubsystem(SingleJointedArmIO singleJointedArmIO, SingleJointedArmIOEncoder singleJointedArmIOEncoder) {
         this.singleJointedArmIO = singleJointedArmIO;
-        this.singleJointedArmIOEncoder = singleJointedArmIOEncoder;
-
+        this.armEncoderIO = armIOEncoder;
         this.armIOInputs = new ArmIOInputsAutoLogged();
-//        this.encoderIOInputs = new SingleJointedArmIOEncoderInputs();
+        this.armEncoderIOInputs =  new ArmEncoderIOInputsAutoLogged();
 
+//        this.encoderIOInputs = new SingleJointedArmIOEncoderInputs();
         armFeedback.setTolerance(SingleJointedArmConstants.kArmToleranceRPS);
         //armEncoder.setDistancePerPulse(SingleJointedArmConstants.kEncoderDistancePerPulse);
     }
 
 
     @Override
     public void periodic(){
-        singleJointedArmIO.updateInputs(ArmIOInputs);
+        singleJointedArmIO.updateInputs(armIOInputs);
+        armEncoderIO.updateInputs(armEncoderIOInputs);
     }
 
+    public void resetArmAngleEncoderValue() {
+        armEncoderIO.resetEncoderPositionWithArmAngle();
+    }
     public double getCurrentAngle(){
-
-        return ArmIOInputs.armAngle;
+        return armEncoderIOInputs.armAngle;
+    }
+    public double getArmSpeedRads() {
+        return armEncoderIOInputs.encoderVelocityRads;
     }
-
     public void setArmVoltage(double volts){
         singleJointedArmIO.setArmMotorVoltage(volts);
     }
 }
-//    public double getCurrentVelocity();