feat: more sim functionality

flamingchickens1540 · Jan 5, 2024 · cb2bb71 · cb2bb71
1 parent e9f2276
commit cb2bb71
Show file tree

Hide file tree

Showing 5 changed files with 14 additions and 25 deletions.
diff --git a/src/main/java/org/team1540/advantagekitdemo/BuildConstants.java b/src/main/java/org/team1540/advantagekitdemo/BuildConstants.java
@@ -7,12 +7,12 @@ public final class BuildConstants {
     public static final String MAVEN_GROUP = "";
     public static final String MAVEN_NAME = "advantageKitDemo";
     public static final String VERSION = "unspecified";
-    public static final int GIT_REVISION = 2;
-    public static final String GIT_SHA = "2a08f6b65c19ec91dfce3fb905ed68b2ab4608ec";
-    public static final String GIT_DATE = "2024-01-04 09:13:53 PST";
+    public static final int GIT_REVISION = 3;
+    public static final String GIT_SHA = "e9f2276b07371e1c617fbb77f12dcd6de3aeaf5c";
+    public static final String GIT_DATE = "2024-01-04 16:56:28 PST";
     public static final String GIT_BRANCH = "main";
-    public static final String BUILD_DATE = "2024-01-04 16:52:55 PST";
-    public static final long BUILD_UNIX_TIME = 1704415975607L;
+    public static final String BUILD_DATE = "2024-01-04 20:09:59 PST";
+    public static final long BUILD_UNIX_TIME = 1704427799024L;
     public static final int DIRTY = 1;
 
     private BuildConstants(){}

diff --git a/src/main/java/org/team1540/advantagekitdemo/Constants.java b/src/main/java/org/team1540/advantagekitdemo/Constants.java
@@ -1,16 +1,3 @@
-// Copyright 2021-2023 FRC 6328
-// http://github.com/Mechanical-Advantage
-//
-// This program is free software; you can redistribute it and/or
-// modify it under the terms of the GNU General Public License
-// version 3 as published by the Free Software Foundation or
-// available in the root directory of this project.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU General Public License for more details.
-
 package org.team1540.advantagekitdemo;
 
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;

diff --git a/src/main/java/org/team1540/advantagekitdemo/commands/WristManualCommand.java b/src/main/java/org/team1540/advantagekitdemo/commands/WristManualCommand.java
@@ -1,7 +1,10 @@
 package org.team1540.advantagekitdemo.commands;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+
+import org.team1540.advantagekitdemo.Constants;
 import org.team1540.advantagekitdemo.subsystems.intake.Intake;
 
 public class WristManualCommand extends Command {
@@ -16,7 +19,7 @@ public WristManualCommand(Intake intake, CommandXboxController controller) {
 
     @Override
     public void execute() {
-        intake.setWristPercent(controller.getRightX());
+        intake.setWristPercent(MathUtil.applyDeadband(controller.getRightY(), Constants.DEADZONE_RADIUS));
     }
 
     @Override

diff --git a/src/main/java/org/team1540/advantagekitdemo/subsystems/intake/IntakeIOSim.java b/src/main/java/org/team1540/advantagekitdemo/subsystems/intake/IntakeIOSim.java
@@ -25,7 +25,7 @@ public class IntakeIOSim implements IntakeIO {
     @Override
     public void updateInputs(IntakeIOInputs inputs) {
         wristSim.update(0.02);
-        inputs.wristPosition = Rotation2d.fromDegrees(wristSim.getAngleRads());
+        inputs.wristPosition = Rotation2d.fromRadians(wristSim.getAngleRads());
         inputs.wristAppliedVolts = wristVoltage;
         inputs.wristCurrentAmps = wristSim.getCurrentDrawAmps();
         inputs.intakeAppliedVolts = intakeVoltage;

diff --git a/src/main/java/org/team1540/advantagekitdemo/util/SuperstructureVisualizer.java b/src/main/java/org/team1540/advantagekitdemo/util/SuperstructureVisualizer.java
@@ -20,26 +20,25 @@ public static void periodic() {
     public static void setElevatorPosition(double positionMeters) {
         if (positionMeters <= ElevatorConstants.STAGE_1_HEIGHT_METERS) {
             elevatorStage1 = new Pose3d();
-            elevatorCarriage = new Pose3d(0.0, positionMeters, 0.0, new Rotation3d());
         } else {
-            elevatorCarriage = new Pose3d(0.0, ElevatorConstants.STAGE_1_HEIGHT_METERS, 0.0, new Rotation3d());
             elevatorStage1 =
                     new Pose3d(
                             0.0,
-                            positionMeters - ElevatorConstants.STAGE_1_HEIGHT_METERS,
                             0.0,
+                            positionMeters - ElevatorConstants.STAGE_1_HEIGHT_METERS,
                             new Rotation3d()
                     );
         }
+        elevatorCarriage = new Pose3d(0.0, 0.0, positionMeters, new Rotation3d());
         elevatorPosition = positionMeters;
     }
 
     public static void setWristPosition(Rotation2d wristPosition) {
         wrist = new Pose3d(
                 0.0,
-                elevatorPosition,
                 0.0,
-                new Rotation3d(0, wristPosition.getDegrees(), 0)
+                elevatorPosition,
+                new Rotation3d(0, wristPosition.getRadians(), 0)
         );
     }
 }