Merge branch 'mechanism-constants-update' into collision-fixing

src/main/java/frc/robot/coralIntake/CoralIntake.java

@@ -33,8 +33,8 @@ public static class CoralIntakeConfig extends Config {
         @Getter private double velocityKs = 14;
 
         /* Sim Configs */
-        @Getter private double intakeX = 1.0; // 0.5; // relative to elbow at 0 degrees
-        @Getter private double intakeY = 1.1; // relative to elbow at 0 degrees
+        @Getter private double intakeX = 0.8; // relative to elbow at 0 degrees
+        @Getter private double intakeY = 1.3; // relative to elbow at 0 degrees
         @Getter private double wheelDiameter = 5.0;
 
         public CoralIntakeConfig() {

src/main/java/frc/robot/elbow/Elbow.java

@@ -31,6 +31,7 @@ public static class ElbowConfig extends Config {
         @Getter private final double home = 0;
         @Getter private final double coralIntake = 35.5;
         @Getter private final double floorIntake = 0;
+        @Getter private final double corralFloorIntake = 87.8;
         @Getter private final double l1Coral = 87.8;
         @Getter private final double l2Algae = 20;
         @Getter private final double l3Algae = 20;
@@ -52,8 +53,8 @@ public static class ElbowConfig extends Config {
         // Removed implementation of tree map
 
         /* Sim properties */
-        @Getter private double elbowX = 0.6 + 0.4; // 1.0;
-        @Getter private double elbowY = 0.6;
+        @Getter private double elbowX = 0.8; // 1.0;
+        @Getter private double elbowY = 0.8;
         @Getter @Setter private double simRatio = 1;
         @Getter private double length = 0.4;
         @Getter private double startingAngle = 180 - 90;
@@ -210,17 +211,13 @@ public ElbowSim(TalonFXSimState elbowMotorSim, Mechanism2d mech) {
                                     config.simRatio,
                                     config.length,
                                     -90,
-                                    // 180 - 45 +
-                                    // Units.rotationsToDegrees(config.getMinRotations()),
                                     180 + 90,
-                                    // 180 - 45 +
-                                    // Units.rotationsToDegrees(config.getMaxRotations()),
                                     config.getStartingAngle())
                             .setColor(new Color8Bit(Color.kAqua))
                             .setMount(Robot.getShoulder().getSim(), true),
                     mech,
                     elbowMotorSim,
-                    "2" + config.getName()); // added 2 to the name to create it second
+                    "3" + config.getName()); // added 3 to the name to create it third
         }
     }
 }

src/main/java/frc/robot/elevator/Elevator.java

@@ -21,17 +21,17 @@ public class Elevator extends Mechanism {
 
     public static class ElevatorConfig extends Config {
         /* Elevator constants in rotations */
-        @Getter private double maxRotations = 50; // 29.8;
+        @Getter private double maxRotations = 110.6304660319; // 29.8;
         @Getter private double minRotations = 0;
 
         /* Elevator positions in rotations */
         // TODO: Find elevator positions
-        @Getter @Setter private double fullExtend = maxRotations;
+        @Getter @Setter private double fullExtend = maxRotations * .999;
         @Getter private double home = minRotations;
-        @Getter private final double l2 = 24.4168;
-        @Getter private final double l3 = 35;
-        @Getter private final double l4 = maxRotations;
-        @Getter private final double barge = maxRotations;
+        @Getter private final double l2 = 15.412;
+        @Getter private final double l3 = 24.4168;
+        @Getter private final double l4 = fullExtend;
+        @Getter private final double barge = fullExtend;
 
         @Getter private double tolerance = 0.95;
         @Getter private double elevatorUpHeight = 5;
@@ -44,14 +44,14 @@ public static class ElevatorConfig extends Config {
         @Getter private final double torqueCurrentLimit = 100;
 
         /* Sim properties */
-        @Getter private double kElevatorGearing = 2.9; // 5;
+        @Getter private double kElevatorGearing = 3.62722; // 5;
         @Getter private double kCarriageMass = 1;
         @Getter private double kElevatorDrumRadiusMeters = Units.inchesToMeters(0.955 / 2);
         @Getter private double initialX = 0.8;
         @Getter private double initialY = 0.35;
-        @Getter private double angle = 90; // 180.0 - 72.0;
-        @Getter private double staticLength = 55;
-        @Getter private double movingLength = 40;
+        @Getter private double angle = 90;
+        @Getter private double staticLength = 50;
+        @Getter private double movingLength = 50;
 
         public ElevatorConfig() {
             super("Elevator", 40, Rio.CANIVORE);
@@ -210,7 +210,8 @@ public ElevatorSim(TalonFXSimState elevatorMotorSim, Mechanism2d mech) {
                                     config.kElevatorDrumRadiusMeters)
                             .setAngle(config.angle)
                             .setMovingLength(config.getMovingLength())
-                            .setStaticLength(config.getStaticLength()),
+                            .setStaticLength(config.getStaticLength())
+                            .setMaxHeight(30.5),
                     mech,
                     elevatorMotorSim,
                     "1" + config.getName()); // added 1 to the name to create it first

src/main/java/frc/robot/shoulder/Shoulder.java

@@ -31,16 +31,15 @@ public static class ShoulderConfig extends Config {
         @Getter private final double score = 100 - 65;
         @Getter private final double climbHome = 100 - 3;
         @Getter private final double home = 100 - 1;
-        @Getter private final double coralIntake = 44;
-        @Getter private final double floorIntake = 99;
-        @Getter private final double ninetyDegrees = 100 - 39;
-        @Getter private final double l1Coral = 63;
-        @Getter private final double l2Algae = 90;
-        @Getter private final double l3Algae = 90;
-        @Getter private final double l2Coral = 90;
-        @Getter private final double l3Coral = 90;
-        @Getter private final double l4Coral = 57;
-        @Getter private final double barge = 57;
+        @Getter private final double coralIntake = 91;
+        @Getter private final double floorIntake = -70;
+        @Getter private final double l1Coral = -80;
+        @Getter private final double l2Algae = -40;
+        @Getter private final double l3Algae = -40;
+        @Getter private final double l2Coral = -40;
+        @Getter private final double l3Coral = -40;
+        @Getter private final double l4Coral = -88.9;
+        @Getter private final double barge = -88.9;
         @Getter @Setter private double tuneShoulder = 0;
 
         /* Shoulder config settings */
@@ -58,7 +57,7 @@ public static class ShoulderConfig extends Config {
 
         /* Sim properties */
         @Getter private double shoulderX = 0.8;
-        @Getter private double shoulderY = 0.8;
+        @Getter private double shoulderY = 1.1;
 
         @Getter @Setter private double simRatio = 1;
 
@@ -73,7 +72,7 @@ public ShoulderConfig() {
             configSupplyCurrentLimit(currentLimit, true);
             configForwardTorqueCurrentLimit(torqueCurrentLimit);
             configReverseTorqueCurrentLimit(torqueCurrentLimit);
-            configMinMaxRotations(0, 11.57142857);
+            configMinMaxRotations(-7.714285714, 7.714285714);
             configReverseSoftLimit(getMinRotations(), true);
             configForwardSoftLimit(getMaxRotations(), true);
             configNeutralBrakeMode(true);
@@ -215,13 +214,13 @@ public ShoulderSim(TalonFXSimState shoulderMotorSim, Mechanism2d mech) {
                                     config.shoulderY,
                                     config.simRatio,
                                     config.length,
-                                    -45,
-                                    270 - 45,
-                                    -45)
+                                    -270,
+                                    360 - 90,
+                                    -90)
                             .setMount(Robot.getElevator().getSim(), true),
                     mech,
                     shoulderMotorSim,
-                    "3" + config.getName()); // added 3 to the name to create it third
+                    "2" + config.getName()); // added 2 to the name to create it second
         }
     }
 }

src/main/java/frc/robot/twist/Twist.java

@@ -63,8 +63,8 @@ public static class TwistConfig extends Config {
         // Removed implementation of tree map
 
         /* Sim properties */
-        @Getter private double twistX = 0.7;
-        @Getter private double twistY = 1.2;
+        @Getter private double twistX = 0.525;
+        @Getter private double twistY = 1.35;
         @Getter @Setter private double simRatio = 15.429; // TODO: Set this to actual twist ratio
         @Getter private double coralLength = 0.225;
         @Getter private double algaeLength = 0.125;

src/main/java/frc/spectrumLib/sim/LinearConfig.java

@@ -77,6 +77,11 @@ public LinearConfig setMovingLength(double lengthInches) {
         return this;
     }
 
+    public LinearConfig setMaxHeight(double lengthInches) {
+        this.maxHeight = Units.inchesToMeters(lengthInches);
+        return this;
+    }
+
     public LinearConfig setMount(LinearSim sim) {
         if (sim != null) {
             mounted = true;