korean sweet and spuicy puffs

BitBucketsFRC4183 · Feb 9, 2025 · 0244a68 · 0244a68
1 parent bce4163
commit 0244a68
Show file tree

Hide file tree

Showing 9 changed files with 75 additions and 48 deletions.
diff --git a/simgui-ds.json b/simgui-ds.json
@@ -9,6 +9,11 @@
       "visible": true
     }
   },
+  "System Joysticks": {
+    "window": {
+      "visible": false
+    }
+  },
   "keyboardJoysticks": [
     {
       "axisConfig": [
@@ -31,7 +36,7 @@
         }
       ],
       "axisCount": 3,
-      "buttonCount": 9,
+      "buttonCount": 10,
       "buttonKeys": [
         90,
         88,
@@ -42,19 +47,21 @@
         85,
         89,
         84,
+        77,
+        -1,
         -1,
         -1
       ],
       "povConfig": [
         {
-          "key0": 328,
+          "key0": 49,
           "key135": 323,
           "key180": 322,
           "key225": 321,
           "key270": 324,
           "key315": 327,
-          "key45": 329,
-          "key90": 326
+          "key45": 46,
+          "key90": 47
         }
       ],
       "povCount": 1
@@ -72,10 +79,10 @@
       "axisCount": 2,
       "buttonCount": 4,
       "buttonKeys": [
-        77,
-        44,
-        46,
-        47
+        -1,
+        -1,
+        -1,
+        -1
       ],
       "povCount": 0
     },
@@ -109,6 +116,7 @@
     }
   ],
   "robotJoysticks": [
+    {},
     {
       "guid": "Keyboard0"
     }

diff --git a/src/main/java/frc/robot/OperatorInput.java b/src/main/java/frc/robot/OperatorInput.java
@@ -29,8 +29,8 @@ public class OperatorInput {
 
   final Trigger IntakeOn = driver.a();
 
-  final Trigger armbendup = driver.povUp();
-  final Trigger armbenddown = driver.povDown();
+  final Trigger armbendup = operatorControl.povUp();
+  final Trigger armbenddown = operatorControl.povDown();
   final Trigger xNotDesired =
       driver
           .axisGreaterThan(XboxController.Axis.kLeftX.value, 0.1)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -45,6 +45,9 @@
 import frc.robot.subsystems.ElevatorSubsystem.*;
 import frc.robot.subsystems.GroundIntakeSubsystem.GroundIntakeSubsystem;
 import frc.robot.subsystems.LEDSubsytem.LEDSubsystem;
+import frc.robot.subsystems.SingleJointedArmSubsystem.SingleJointedArmIO;
+import frc.robot.subsystems.SingleJointedArmSubsystem.SingleJointedArmIOSim;
+import frc.robot.subsystems.SingleJointedArmSubsystem.SingleJointedArmSparkMax;
 import frc.robot.subsystems.SingleJointedArmSubsystem.SingleJointedArmSubsystem;
 import frc.robot.subsystems.VisionSubsystem.VisionIO;
 import frc.robot.subsystems.VisionSubsystem.VisionIOPhotonVision;
@@ -112,7 +115,8 @@ public RobotContainer() {
         ledSubsystem =
                 new LEDSubsystem(); //TODO
         singleJointedArmSubsystem =
-                new SingleJointedArmSubsystem(); //TODO
+                new SingleJointedArmSubsystem(new SingleJointedArmSparkMax()) {
+                }; //TODO
         visionSubsystem =
                 new VisionSubsystem(new VisionIOPhotonVision()); //TODO
         break;
@@ -146,7 +150,7 @@ public RobotContainer() {
         ledSubsystem =
                 new LEDSubsystem(); //TODO
         singleJointedArmSubsystem =
-                new SingleJointedArmSubsystem(); //TODO
+                new SingleJointedArmSubsystem(new SingleJointedArmIOSim()); //TODO
         visionSubsystem =
                 new VisionSubsystem(new VisionIOPhotonVisionSim(driveSimulation::getSimulatedDriveTrainPose)); //TODO
         break;
@@ -171,7 +175,7 @@ public RobotContainer() {
         ledSubsystem =
                 new LEDSubsystem(); //TODO
         singleJointedArmSubsystem =
-                new SingleJointedArmSubsystem(); //TODO
+                new SingleJointedArmSubsystem(new SingleJointedArmIOSim()); //TODO
         visionSubsystem =
                 new VisionSubsystem(new VisionIO() {}); //TODO
         break;
@@ -211,7 +215,7 @@ void loadCommands() {
 
     operatorInput.elevatorGoToOrigin.onTrue(new ElevatorGoToOriginCommand(elevatorSubsystem));
 
-    operatorInput.armbendup.whileTrue(new BendCommand(singleJointedArmSubsystem, SingleJointedArmConstants.MAX_ANGLE));
+    operatorInput.armbendup.whileTrue(new BendCommand(singleJointedArmSubsystem, 45));
     operatorInput.armbenddown.whileTrue(new BendCommand(singleJointedArmSubsystem, SingleJointedArmConstants.MIN_ANGLE));
     operatorInput.resetEncoder.onTrue(new ResetElevatorEncoderCommand(elevatorSubsystem));
 

diff --git a/src/main/java/frc/robot/commands/BendCommand.java b/src/main/java/frc/robot/commands/BendCommand.java
@@ -19,16 +19,16 @@ public BendCommand(SingleJointedArmSubsystem subsystem, double targetAngle) {
     @Override
     public void initialize() {
         singleJointedArmSubsystem.armFeedback.setGoal(targetAngle);
-        Logger.recordOutput("SingleJointedArmSubsystem/target_Angle", targetAngle);
+        Logger.recordOutput("ArmSubsystem/target_Angle", targetAngle);
 
     }
 
     @Override
     public void execute() {
         double voltsPID = singleJointedArmSubsystem.armFeedback.calculate(singleJointedArmSubsystem.getCurrentAngle());
-        double calculatedVolts = singleJointedArmSubsystem.armFeedForward.calculate(singleJointedArmSubsystem.getCurrentAngle(), singleJointedArmSubsystem.armFeedback.getSetpoint().velocity) + voltsPID;
+        double calculatedVolts = singleJointedArmSubsystem.armFeedForward.calculate(singleJointedArmSubsystem.armFeedback.getSetpoint().position, singleJointedArmSubsystem.armFeedback.getSetpoint().velocity) + voltsPID;
 
-        Logger.recordOutput("SingleJointedArmSubsystem/target_Voltage", calculatedVolts);
+        Logger.recordOutput("SingleJointedArmSubsystem/target_voltage", calculatedVolts);
         Logger.recordOutput("SingleJointedArmSubsystem/desired_position", singleJointedArmSubsystem.armFeedback.getSetpoint().position);
 
         this.singleJointedArmSubsystem.setArmVoltage(calculatedVolts);
@@ -37,7 +37,7 @@ public void execute() {
     @Override
     public void end(boolean interrupted) {
         if (interrupted) {
-            singleJointedArmSubsystem.setArmVoltage(SingleJointedArmConstants.kG);
+            singleJointedArmSubsystem.setArmVoltage(SingleJointedArmConstants.kG * Math.cos(singleJointedArmSubsystem.getCurrentAngle()));
         }
     }
 }
diff --git a/src/main/java/frc/robot/constants/SingleJointedArmConstants.java b/src/main/java/frc/robot/constants/SingleJointedArmConstants.java
@@ -6,28 +6,25 @@
 public class SingleJointedArmConstants {
     public static final int MotorNumber = 0;
     public static final int EncoderNumber = 1;
-    public static final double kP = 0;
-    public static final double kD = 0;
-    public static final double kI = 0;
-    public static final double kV= 0;
-    public static final double kG = 0;
-    public static final double kS = 0;
-    public static final double armMotorReduction = 0;
-    public static final double kSVolts = 0;
-    public static final double kVVoltsSecondsPerRotation = 0;
+    public static final double kP = 1;
+    public static final double kD = 1;
+    public static final double kI = 1;
+    public static final double kV= 10;
+    public static final double kG = 1;
+    public static final double kS = 1;
     public static final double kArmToleranceRPS = 0;
     public static final double kEncoderDistancePerPulse = 0;
-    public static final double MAX_ANGLE = 90.0;
+    public static final double MAX_ANGLE = 180.0;
     public 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 armLength = 0.0;
+    public static double SparkkP = 1.0;
+    public static double SparkkD = 1.0;
+    public static double gearingRatio = 20.0;
+    public static double armLength = 0.3;
     public static DCMotor armGearbox = DCMotor.getNEO(1);
-    public static int armSparkMotorCurrentLimit = 0;
+    public static int armSparkMotorCurrentLimit = 1;
     public static double armSparkEncoderPositionFactor;
     public static double armSparkEncoderVelocityFactor;
-    public static double maxVelocity;
-    public static double maxAcceleration;
-
+    public static double maxVelocity = 15;
+    public static double maxAcceleration = 15;
+    public static double mass = 5;
 }
diff --git a/src/main/java/frc/robot/subsystems/SingleJointedArmSubsystem/SingleJointedArmIO.java b/src/main/java/frc/robot/subsystems/SingleJointedArmSubsystem/SingleJointedArmIO.java
@@ -8,7 +8,7 @@ public interface SingleJointedArmIO {
     class ArmIOInputs {
         public double armAngle = 0.0;
         public double armAppliedVoltage = 0.0;
-        public double armCurrentAmps = 0.0;
+        public double[] armCurrentAmps = new double[] {};
         public boolean armConnected;
         public double[] odometryTimestamps;
         public double[] odometryArmPositionsRad;

diff --git a/src/main/java/frc/robot/subsystems/SingleJointedArmSubsystem/SingleJointedArmIOSim.java b/src/main/java/frc/robot/subsystems/SingleJointedArmSubsystem/SingleJointedArmIOSim.java
@@ -6,18 +6,34 @@
 import edu.wpi.first.wpilibj.simulation.LinearSystemSim;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
 import frc.robot.constants.SingleJointedArmConstants;
+import frc.robot.subsystems.ElevatorSubsystem.ElevatorIO;
+import org.littletonrobotics.junction.Logger;
 
 public class SingleJointedArmIOSim implements SingleJointedArmIO {
     private static final double LOOP_PERIOD_SECS = 0.02;
     public static final SingleJointedArmSim armMotorSim = new SingleJointedArmSim(
             SingleJointedArmConstants.armGearbox,
             SingleJointedArmConstants.gearingRatio,
-            0,
+            SingleJointedArmSim.estimateMOI(SingleJointedArmConstants.armLength, SingleJointedArmConstants.mass),
             SingleJointedArmConstants.armLength,
             SingleJointedArmConstants.MIN_ANGLE,
             SingleJointedArmConstants.MAX_ANGLE,
-            true, 0, 0
+            true, 0, 0.01,0
             );
 
+    @Override
+    public void updateInputs(SingleJointedArmIO.ArmIOInputs inputs) {
+        armMotorSim.update(LOOP_PERIOD_SECS);
+        inputs.armAngle = armMotorSim.getAngleRads();
+        inputs.armCurrentAmps = new double[]{Math.abs(armMotorSim.getCurrentDrawAmps())};
+        Logger.recordOutput("ArmSubsystem/armAngle", inputs.armAngle);
+        Logger.recordOutput("ArmSubsystem/armCurrentAmps", inputs.armCurrentAmps);
+
+    }
+    @Override
+    public void setArmMotorVoltage(double volts) {
+        armMotorSim.setInputVoltage(volts);
+        Logger.recordOutput("ArmSubsystem/appliedVolts", volts);
+    }
 
 }
diff --git a/src/main/java/frc/robot/subsystems/SingleJointedArmSubsystem/SingleJointedArmSparkMax.java b/src/main/java/frc/robot/subsystems/SingleJointedArmSubsystem/SingleJointedArmSparkMax.java
@@ -77,7 +77,7 @@ public void updateInputs(SingleJointedArmIO.ArmIOInputs inputs) {
                     armSpark,
                     new DoubleSupplier[]{armSpark::getAppliedOutput, armSpark::getBusVoltage},
                     (values) -> inputs.armAppliedVoltage = values[0] * values[1]);
-            ifOk(armSpark, armSpark::getOutputCurrent, (value) -> inputs.armCurrentAmps = value);
+            ifOk(armSpark, armSpark::getOutputCurrent, (value) -> inputs.armCurrentAmps = new double[]{value});
             inputs.armConnected = armConnectedDebounce.calculate(!sparkStickyFault);
 
             inputs.odometryTimestamps =

diff --git a/src/main/java/frc/robot/subsystems/SingleJointedArmSubsystem/SingleJointedArmSubsystem.java b/src/main/java/frc/robot/subsystems/SingleJointedArmSubsystem/SingleJointedArmSubsystem.java
@@ -15,36 +15,38 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.RobotContainer;
 import frc.robot.constants.SingleJointedArmConstants;
-
-
+import frc.robot.subsystems.ElevatorSubsystem.ElevatorEncoderIOInputsAutoLogged;
+import frc.robot.subsystems.ElevatorSubsystem.ElevatorIOInputsAutoLogged;
 
 
 public class SingleJointedArmSubsystem extends SubsystemBase {
     public static int canID;
-    public SingleJointedArmIO singleJointedArm;
+    public SingleJointedArmIO singleJointedArmIO;
     private final ArmIOInputsAutoLogged armIOInputs;
     private final SparkMax armMotor = new SparkMax(canID, SparkLowLevel.MotorType.kBrushless);
     //private final Encoder armEncoder = new Encoder();
-    public final ArmFeedforward armFeedForward = new ArmFeedforward(SingleJointedArmConstants.kS, SingleJointedArmConstants.kG, SingleJointedArmConstants.kV );
-    // add soleniod thingy
+    public final ArmFeedforward armFeedForward = new ArmFeedforward(SingleJointedArmConstants.kS, SingleJointedArmConstants.kG, SingleJointedArmConstants.kV);
     public final ProfiledPIDController armFeedback = new ProfiledPIDController(SingleJointedArmConstants.kP, SingleJointedArmConstants.kI, SingleJointedArmConstants.kD, new TrapezoidProfile.Constraints(SingleJointedArmConstants.maxVelocity,SingleJointedArmConstants.maxAcceleration));
 
-    public SingleJointedArmSubsystem() {
+    public SingleJointedArmSubsystem(SingleJointedArmIO singleJointedArmIO) {
+        this.singleJointedArmIO = singleJointedArmIO;
         this.armIOInputs = new ArmIOInputsAutoLogged();
+
         armFeedback.setTolerance(SingleJointedArmConstants.kArmToleranceRPS);
         //armEncoder.setDistancePerPulse(SingleJointedArmConstants.kEncoderDistancePerPulse);
 
     }
     @Override
     public void periodic(){
-        singleJointedArm.updateInputs(armIOInputs);
+        singleJointedArmIO.updateInputs(armIOInputs);
     }
 
     public double getCurrentAngle(){
         return armIOInputs.armAngle;
     }
+
     public void setArmVoltage(double volts){
-        singleJointedArm.setArmMotorVoltage(volts);
+        singleJointedArmIO.setArmMotorVoltage(volts);
     }
 }