diff --git a/src/main/java/frc/robot/subsystems/Arm/Arm.java b/src/main/java/frc/robot/subsystems/Arm/Arm.java index 9d46f60..fbfcff0 100644 --- a/src/main/java/frc/robot/subsystems/Arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm/Arm.java @@ -27,7 +27,8 @@ public class Arm extends GenericMotionProfiledSubsystem { 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), diff --git a/src/main/java/frc/robot/subsystems/Arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/Arm/ArmConstants.java index 7183952..db6fb1f 100644 --- a/src/main/java/frc/robot/subsystems/Arm/ArmConstants.java +++ b/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 } } diff --git a/src/main/java/frc/robot/subsystems/Elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/Elevator/ElevatorConstants.java index 7fdd71d..e719888 100644 --- a/src/main/java/frc/robot/subsystems/Elevator/ElevatorConstants.java +++ b/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,6 +98,12 @@ 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); @@ -104,15 +111,15 @@ public final class ElevatorConstants { 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 diff --git a/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystem.java b/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystem.java index 28ce837..2e24df6 100644 --- a/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystem.java +++ b/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)); diff --git a/src/main/java/frc/robot/util/sim/ElevatorSimProfile.java b/src/main/java/frc/robot/util/sim/ElevatorSimProfile.java index a616971..f1aa982 100644 --- a/src/main/java/frc/robot/util/sim/ElevatorSimProfile.java +++ b/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 diff --git a/src/main/java/frc/robot/util/sim/mechanisms/ArmElevComboMechanism.java b/src/main/java/frc/robot/util/sim/mechanisms/ArmElevComboMechanism.java index 3190144..ded34c4 100644 --- a/src/main/java/frc/robot/util/sim/mechanisms/ArmElevComboMechanism.java +++ b/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