Merge pull request #52 from WHS-FRC-3467/fix-sim-mech

Fix sim mech

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

@@ -27,7 +27,8 @@ public class Arm extends GenericMotionProfiledSubsystem<Arm.State> {
     public enum State implements TargetState {
         // HOMING(0.0, 0.0, ProfileType.MM_POSITION),
         STOW(() -> Units.degreesToRotations(124.0), ProfileType.MM_POSITION),
-        CORAL_INTAKE(() -> 0.42, ProfileType.MM_POSITION),
+        // CORAL_INTAKE(() -> 0.42, ProfileType.MM_POSITION),
+        CORAL_INTAKE(() -> 0.405, ProfileType.MM_POSITION),
         LEVEL_1(() -> Units.degreesToRotations(120.0), ProfileType.MM_POSITION),
         LEVEL_2(() -> Units.degreesToRotations(105.0), ProfileType.MM_POSITION),
         LEVEL_3(() -> Units.degreesToRotations(105.0), ProfileType.MM_POSITION),

src/main/java/frc/robot/subsystems/Arm/ArmConstants.java

@@ -8,6 +8,7 @@
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.util.Units;
 import frc.robot.Ports;
+import frc.robot.Robot;
 import frc.robot.subsystems.GenericMotionProfiledSubsystem.GenericMotionProfiledSubsystemConstants;
 import frc.robot.subsystems.GenericMotionProfiledSubsystem.GenericMotionProfiledSubsystemConstants.simType;
 
@@ -58,6 +59,7 @@ public final class ArmConstants {
         kSubSysConstants.kMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
         kSubSysConstants.kMotorConfig.MotorOutput.Inverted =
             InvertedValue.CounterClockwise_Positive;
+
         kSubSysConstants.kMotorConfig.Voltage.PeakForwardVoltage = 12.0;
         kSubSysConstants.kMotorConfig.Voltage.PeakReverseVoltage = -12.0;
 
@@ -85,7 +87,7 @@ public final class ArmConstants {
         kSubSysConstants.kMotorConfig.MotionMagic.MotionMagicJerk = 0;
 
         /* SIM system profile constants */
-        kSubSysConstants.kSimMotorConfig.Slot0.kP = 700;
+        kSubSysConstants.kSimMotorConfig.Slot0.kP = 1400;
         kSubSysConstants.kSimMotorConfig.Slot0.kI = 0;
         kSubSysConstants.kSimMotorConfig.Slot0.kD = 100;
         kSubSysConstants.kSimMotorConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine;
@@ -97,6 +99,13 @@ public final class ArmConstants {
         kSubSysConstants.kSimMotorConfig.MotionMagic.MotionMagicAcceleration = 50;
         kSubSysConstants.kSimMotorConfig.MotionMagic.MotionMagicJerk = 0;
 
+        if (Robot.isSimulation()) {
+            kSubSysConstants.kEncoderConfig.MagnetSensor.MagnetOffset = 0.0;
+            kSubSysConstants.kMotorConfig.MotorOutput.Inverted =
+                InvertedValue.Clockwise_Positive;
+
+        }
+
         // Simulation Type
         kSubSysConstants.SimType = simType.ARM;
 
@@ -105,13 +114,15 @@ public final class ArmConstants {
 
         // Arm Simulation
         kSubSysConstants.kArmSimConfig.kIsComboSim = true;
-        kSubSysConstants.kArmSimConfig.kArmMass = 8.0; // Kilograms
-        kSubSysConstants.kArmSimConfig.kArmLength = Units.inchesToMeters(30);
-        kSubSysConstants.kArmSimConfig.kDefaultArmSetpointDegrees = 75.0;
-        kSubSysConstants.kArmSimConfig.kMinAngleDegrees = -75.0;
-        kSubSysConstants.kArmSimConfig.kMaxAngleDegrees = 255.0;
-        kSubSysConstants.kArmSimConfig.kArmReduction =
-            54.4; // RotorToSensorRatio * SensorToMechanismRatio
-        kSubSysConstants.kArmSimConfig.kSensorReduction = 7.04; // SensorToMechanismRatio
+        kSubSysConstants.kArmSimConfig.kArmMass = Units.lbsToKilograms(11); // Kilograms
+        kSubSysConstants.kArmSimConfig.kArmLength = Units.inchesToMeters(14);
+        kSubSysConstants.kArmSimConfig.kDefaultArmSetpointDegrees =
+            Units.rotationsToDegrees(-0.405);
+        kSubSysConstants.kArmSimConfig.kMinAngleDegrees = Units.rotationsToDegrees(-.405);
+        kSubSysConstants.kArmSimConfig.kMaxAngleDegrees = 0;
+        kSubSysConstants.kArmSimConfig.kArmReduction = (9 / 1) * (48 / 22) * (70 / 22); // RotorToSensorRatio
+                                                                                        // *
+                                                                                        // SensorToMechanismRatio
+        kSubSysConstants.kArmSimConfig.kSensorReduction = 1; // SensorToMechanismRatio
     }
 }

src/main/java/frc/robot/subsystems/Elevator/ElevatorConstants.java

@@ -9,6 +9,7 @@
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.util.Units;
 import frc.robot.Ports;
+import frc.robot.Robot;
 import frc.robot.subsystems.GenericMotionProfiledSubsystem.GenericMotionProfiledSubsystemConstants;
 import frc.robot.subsystems.GenericMotionProfiledSubsystem.GenericMotionProfiledSubsystemConstants.simType;
 
@@ -97,22 +98,28 @@ public final class ElevatorConstants {
         // Simulation Type
         kSubSysConstants.SimType = simType.ELEVATOR;
 
+        if (Robot.isSimulation()) {
+            kSubSysConstants.kMotorConfig.MotorOutput.Inverted =
+                InvertedValue.CounterClockwise_Positive;
+
+        }
+
         // Motor simulation
         kSubSysConstants.kMotorSimConfig.simMotorModelSupplier = () -> DCMotor.getKrakenX60Foc(2);
 
         // Elevator Simulation
         kSubSysConstants.kElevSimConfig.kIsComboSim = true;
         // Elevator Simulation
         kSubSysConstants.kElevSimConfig.kDefaultSetpoint = 0.0; // Meters
-        kSubSysConstants.kElevSimConfig.kCarriageMass = Units.lbsToKilograms(25); // Kilograms
-        kSubSysConstants.kElevSimConfig.kElevatorDrumRadius = Units.inchesToMeters(1.75); // Meters
+        kSubSysConstants.kElevSimConfig.kCarriageMass = Units.lbsToKilograms(21.5); // Kilograms
+        kSubSysConstants.kElevSimConfig.kElevatorDrumRadius = Units.inchesToMeters(1.756 / 2); // Meters
         // May want to triple "drum size" to account for the x3 scale in the cascading
         // elevator
         kSubSysConstants.kElevSimConfig.kMinElevatorHeight = 0.0; // Meters
-        kSubSysConstants.kElevSimConfig.kMaxElevatorHeight = Units.inchesToMeters(30); // Meters
+        kSubSysConstants.kElevSimConfig.kMaxElevatorHeight = Units.inchesToMeters(31); // Meters
         kSubSysConstants.kElevSimConfig.kElevatorGearing =
-            5.0; // RotorToSensorRatio * SensorToMechanismRatio
-        kSubSysConstants.kElevSimConfig.kSensorReduction = 1.0; // SensorToMechanismRatio
+            9.6; // RotorToSensorRatio * SensorToMechanismRatio
+        kSubSysConstants.kElevSimConfig.kSensorReduction = 9.6; // SensorToMechanismRatio
 
         // Calculate ratio of motor rotation to distance the top of the elevator moves
         // 5:1 and then x3. Each rotation is 1.75in diameter x pi = 5.498in per rotation

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

@@ -1,6 +1,7 @@
 package frc.robot.subsystems.GenericMotionProfiledSubsystem;
 
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
@@ -185,7 +186,7 @@ private void displayInfo()
             Logger.recordOutput(m_name + "/Setpoint", io.getSetpoint());
             Logger.recordOutput(m_name + "/Position(Rotations)", io.getPosition());
             Logger.recordOutput(m_name + "/Position(Degrees)",
-                Math.toDegrees(io.getPosition() * 2 * Math.PI));
+                (Units.rotationsToDegrees(io.getPosition())));
             Logger.recordOutput(m_name + "/Velocity", io.getVelocity());
             Logger.recordOutput(m_name + "/CurrTrajPos", io.getCurrTrajPos());
             Logger.recordOutput(m_name + "/AtPosition?", io.atPosition(0.0));

src/main/java/frc/robot/util/sim/ElevatorSimProfile.java

@@ -88,14 +88,18 @@ public void run()
 
         // ... and set the position and velocity for the lead motor simulation
         // (Multiply elevator positon by total gearing reduction from motor to elevator)
-        simState.setRawRotorPosition(position_meters * m_ElevConst.kElevatorGearing);
-        simState.setRotorVelocity(velocity_mps * m_ElevConst.kElevatorGearing);
+        simState.setRawRotorPosition(position_meters
+            / (2 * Math.PI * m_ElevConst.kElevatorDrumRadius) * m_ElevConst.kElevatorGearing);
+        simState.setRotorVelocity(velocity_mps / (2 * Math.PI * m_ElevConst.kElevatorDrumRadius)
+            * m_ElevConst.kElevatorGearing);
 
         // If using an external encoder, update its sim as well
         if (m_CANcoder != null) {
             // (Multiply elevator position by gearing reduction from sensor to elevator)
-            m_CANcoder.getSimState().setRawPosition(position_meters * m_ElevConst.kSensorReduction);
-            m_CANcoder.getSimState().setVelocity(velocity_mps * m_ElevConst.kSensorReduction);
+            m_CANcoder.getSimState().setRawPosition(position_meters
+                / (2 * Math.PI * m_ElevConst.kElevatorDrumRadius) * m_ElevConst.kSensorReduction);
+            m_CANcoder.getSimState().setVelocity(velocity_mps
+                / (2 * Math.PI * m_ElevConst.kElevatorDrumRadius) * m_ElevConst.kSensorReduction);
         }
 
         // Update elevator sim mechanism

src/main/java/frc/robot/util/sim/mechanisms/ArmElevComboMechanism.java

@@ -86,7 +86,6 @@ public void updateArm(double degrees)
     /** Controls the Elevator segment & updates the mech2d widget in GUI. */
     public void updateElevator(double position)
     {
-
         m_Elevator.setLength(position);
 
         // Publish Pose3d for 3D mechanism sim of 2 stage elevator