Created IO and started sim

BitBucketsFRC4183 · Jan 25, 2025 · 41cceea · 41cceea
2 parents 5e0e33f + 98277e0
commit 41cceea
Show file tree

Hide file tree

Showing 7 changed files with 63 additions and 24 deletions.
diff --git a/src/main/java/frc/robot/constants/SingleJointedArmConstants.java b/src/main/java/frc/robot/constants/SingleJointedArmConstants.java
@@ -16,4 +16,8 @@ public class SingleJointedArmConstants {
     private static final double MIN_ANGLE = 0.0;
     public static double SparkkP = 0.0;
     public static double SparkkD = 0.0;
+    public static double gearingRatio = 0.0;
+    public static double armSparkMotorCurrentLimit = 0.0;
+    public static double armSparkEncoderPositionFactor;
+    public static double armSparkEncoderVelocityFactor;
 }
diff --git a/src/main/java/frc/robot/subsystems/SingleJointedArmSubsystem/SingleJointedArmIO.java b/src/main/java/frc/robot/subsystems/SingleJointedArmSubsystem/SingleJointedArmIO.java
@@ -7,6 +7,8 @@ public interface SingleJointedArmIO {
     class ArmIOInputs {
         public double armAngle = 0.0;
         public double armAppliedVoltage = 0.0;
+        public double armCurrentAmps = 0.0;
+        public boolean armConnected;
     }
     public default void setArmMotorVoltage(double volts) {
     }

diff --git a/src/main/java/frc/robot/subsystems/SingleJointedArmSubsystem/SingleJointedArmIOEncoder.java b/src/main/java/frc/robot/subsystems/SingleJointedArmSubsystem/SingleJointedArmIOEncoder.java
@@ -0,0 +1,5 @@
+package frc.robot.subsystems.SingleJointedArmSubsystem;
+
+public class SingleJointedArmIOEncoder {
+
+}
diff --git a/...ems/SingleJointedArmSubsystem/Module.java → ...edArmSubsystem/SingleJointedArmIOSim.java b/...ems/SingleJointedArmSubsystem/Module.java → ...edArmSubsystem/SingleJointedArmIOSim.java
@@ -1,4 +1,4 @@
 package frc.robot.subsystems.SingleJointedArmSubsystem;
 
-public class Module {
+public class SingleJointedArmIOSim {
 }
diff --git a/src/main/java/frc/robot/subsystems/SingleJointedArmSubsystem/SingleJointedArmIOSparkMax.java b/src/main/java/frc/robot/subsystems/SingleJointedArmSubsystem/SingleJointedArmIOSparkMax.java
@@ -0,0 +1,4 @@
+package frc.robot.subsystems.SingleJointedArmSubsystem;
+
+public class SingleJointedArmIOSparkMax {
+}
diff --git a/src/main/java/frc/robot/subsystems/SingleJointedArmSubsystem/SingleJointedArmSparkMax.java b/src/main/java/frc/robot/subsystems/SingleJointedArmSubsystem/SingleJointedArmSparkMax.java
@@ -7,39 +7,38 @@
 import com.revrobotics.spark.config.SparkBaseConfig;
 import com.revrobotics.spark.config.SparkFlexConfig;
 import edu.wpi.first.math.filter.Debouncer;
-import frc.robot.constants.ElevatorConstants;
 import frc.robot.constants.SingleJointedArmConstants;
 import frc.robot.subsystems.DriveSubsystem.SparkOdometryThread;
 
-import java.util.Map;
 import java.util.Queue;
 import java.util.function.DoubleSupplier;
 
 import static frc.robot.constants.DriveConstants.odometryFrequency;
+import static frc.robot.constants.ElevatorConstants.pulleyRadius;
 import static frc.robot.util.SparkUtil.*;
 
 public class SingleJointedArmSparkMax implements SingleJointedArmIO{
-    private final SparkBase armSpark;
-    private final RelativeEncoder armEncoder;
+    private SparkBase armSpark = null;
+    private RelativeEncoder armEncoder = null;
 
-    private final SparkClosedLoopController armController;
+    private SparkClosedLoopController armController = null;
 
-    private final Queue<Double> timestampQueue;
-    private final Queue<Double> elevatorPositionQueue;
+    private Queue<Double> timestampQueue = null;
+    private final Queue<Double> elevatorPositionQueue = null;
 
     private final Debouncer armConnectedDebounce = new Debouncer(0);
 
-    public ArmIOSparkMax(SparkBase armSpark1, SparkBase armSpark2, Queue<Double> timestampQueue, Queue<Double> armPositionQueue){
-        armEncoder = armSpark1.getEncoder();
-        this.armSpark1 = armSpark1;
-        armController = armSpark1.getClosedLoopController();
-        this.timestampQueue = timeStampQueue;
+    public void ArmIOSparkMax(SparkBase armSpark1, SparkBase armSpark2, Queue<Double> timestampQueue, Queue<Double> armPositionQueue){
+        armEncoder = armSpark.getEncoder();
+        this.armSpark = armSpark;
+        armController = armSpark.getClosedLoopController();
+        this.timestampQueue = timestampQueue;
         this.armPositionQueue = armPositionQueue;
 
         var armConfig = new SparkFlexConfig();
         armConfig
                 .idleMode(SparkBaseConfig.IdleMode.kBrake)
-                .smartCurrentLimit(ArmConstants.armSparkMotorCurrentLimit)
+                .smartCurrentLimit(SingleJointedArmConstants.armSparkMotorCurrentLimit)
                 .voltageCompensation(12.0);
         armConfig
                 .encoder
@@ -69,13 +68,31 @@ public ArmIOSparkMax(SparkBase armSpark1, SparkBase armSpark2, Queue<Double> tim
                                 armConfig, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters));
         tryUntil10k(armSpark, 5, () -> armEncoder.setPosition(0.0));
         timestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue();
-        armPositionQueue = SparkOdometryThread.getInstance().registerSignal(elevatorSpark, armEncoder::getPosition);
+        armPositionQueue = SparkOdometryThread.getInstance().registerSignal(armSpark, armEncoder::getPosition);
 
-        public void updateInputs(SingleJointedArmIO.armIOInputs inputs){
+        public void updateInputs (ArmIOInputs inputs){
             sparkStickyFault = false;
             if0k(
-                    elevatorSpark,
-                    new DoubleSupplier[]{armSpark::getAppliedOutput, armSpark1}
+                    armSpark,
+                    new DoubleSupplier[]{armSpark::getAppliedOutput, armSpark::getBusVoltage},
+                    (values) -> inputs.armAppliedVoltage = values[0] * values[1]);
+            ifOk(armSpark, armSpark::getOutputCurrent, (value) -> SingleJointedArmIO.armCurrentAmps = new double[]{value});
+            inputs.armConnected = armConnectedDebounce.calculate(!sparkStickyFault);
+            inputs.loadHeight = (2 * Math.PI * pulleyRadius) / ((armEncoder.getPosition() - inputs.lastEncoderPosition) * SingleJointedArmConstants.gearingRatio);
+
+            inputs.odometryTimestamps =
+                    timestampQueue.stream().mapToDouble((Double value) -> value).toArray();
+            inputs.odometryArmPositionsRad =
+                    armPositionQueue.stream().mapToDouble((Double value) -> value).toArray();
+            timestampQueue.clear();
+            armPositionQueue.clear();
+            inputs.lastEncoderPosition = armEncoder.getPosition();
+        }
+        @Override
+        public void armMotorVoltage(double volts){
+            armSpark.setVoltage(volts);
+        }
+    }
             )
         }
     }

diff --git a/src/main/java/frc/robot/subsystems/SingleJointedArmSubsystem/SingleJointedArmSubsystem.java b/src/main/java/frc/robot/subsystems/SingleJointedArmSubsystem/SingleJointedArmSubsystem.java
@@ -2,27 +2,34 @@
 
 
 
+import com.revrobotics.spark.SparkLowLevel;
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import com.revrobotics.spark.SparkMax;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.RobotContainer;
 import frc.robot.constants.SingleJointedArmConstants;
 
+
+
 public class SingleJointedArmSubsystem extends SubsystemBase {
-    private final PWMSparkMax armMotor = new PWMSparkMax(SingleJointedArmConstants.MotorNumber);
-    private final Encoder armEncoder = new Encoder(SingleJointedArmConstants.EncoderNumber);
+    public static int canID;
+    private final SparkMax armMotor = new SparkMax(canID, SparkLowLevel.MotorType.kBrushless);
+    //private final Encoder armEncoder = new Encoder();
     private final SimpleMotorFeedforward armFeedForward = new SimpleMotorFeedforward(SingleJointedArmConstants.kSVolts, SingleJointedArmConstants.kVVoltsSecondsPerRotation);
     // add soleniod thingy
     private final PIDController armFeedback = new PIDController(SingleJointedArmConstants.kP, SingleJointedArmConstants.kI, SingleJointedArmConstants.kD);
 
     public SingleJointedArmSubsystem() {
-            armMotor.set(0);
-        }
-
+        armFeedback.setTolerance(SingleJointedArmConstants.kArmToleranceRPS);
+        //armEncoder.setDistancePerPulse(SingleJointedArmConstants.kEncoderDistancePerPulse);
+        setDefaultCommand(runOnce(() -> {
+            armMotor.disable();
+        }).andThen(run(() -> {
+        })).withName("Idle"));
     }
 }