diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 35d529d..f26b678 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -60,7 +60,8 @@ public static final class CANConstants { // Shooter Angles public static final double ARMENCODEROFFSET = 2.8378526; - public static final double ARMSTARTINGANGLE = 22.5; // WHY MaTH HURT + public static final double ARMSTARTINGANGLEOFFSET = -2.2; + public static final double ARMSTARTINGANGLE = 22.5 + ARMSTARTINGANGLEOFFSET; // WHY MaTH HURT public static final double ARMMINRELATVESTART = 0.0; public static final double ARMLOADANGLE = 65 - ARMSTARTINGANGLE; public static final double ARMSPEAKERANGLE = diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 35faeb3..12e80cb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -107,7 +107,7 @@ public class RobotContainer { // Init For Autonomous // private RamseteAutoBuilder autoBuilder; private SendableChooser autoDashboardChooser = new SendableChooser(); - public final boolean enableAutoProfiling = false; + public final boolean enableAutoProfiling = true; /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { diff --git a/src/main/java/frc/robot/commands/ArmCommand.java b/src/main/java/frc/robot/commands/ArmCommand.java index a9750ca..3ff3bf7 100644 --- a/src/main/java/frc/robot/commands/ArmCommand.java +++ b/src/main/java/frc/robot/commands/ArmCommand.java @@ -42,10 +42,12 @@ public void initialize() {} @Override public void execute() { if (!m_shooterState.shooting) { - if ((!HelperFunctions.inDeadzone(m_yAxis.getAsDouble(), Constants.CONTROLLERDEADZONE)) - && m_shooterState.axisEnabled) { - m_ArmSubsystem.MoveArmRelative(m_yAxis.getAsDouble() * kMaxRadiansPerInput); - } else if (m_shooterState.isLoaded & !m_shooterState.isLowered) { + if (m_shooterState.axisEnabled) { + if ((!HelperFunctions.inDeadzone(m_yAxis.getAsDouble(), Constants.CONTROLLERDEADZONE))) { + m_ArmSubsystem.MoveArmRelative(m_yAxis.getAsDouble() * kMaxRadiansPerInput); + } + } + else if (m_shooterState.isLoaded & !m_shooterState.isLowered) { m_ArmSubsystem.lowerArm(); m_shooterState.setLowered(); } else if (!m_shooterState.isLoaded & !m_shooterState.isLowered) { diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 95817fe..7394aa9 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -32,10 +32,10 @@ public class ArmSubsystem extends SubsystemBase { private final AbsoluteEncoder m_AbsoluteEncoder; private final double kP, kI, kD, kIz, kMaxOutput, kMinOutput; private static double kDt = 0.02; // 20ms (update rate for wpilib) - private final double ksArmVolts = 0.08521; - private final double kgArmGravityGain = 0.1711; - private final double kvArmVoltSecondsPerMeter = 0.0022383; - private final double kaArmVoltSecondsSquaredPerMeter = 0.00041162; + private final double ksArmVolts = 0.62208; + private final double kgArmGravityGain = 0.23678; + private final double kvArmVoltSecondsPerMeter = 1.4195; + private final double kaArmVoltSecondsSquaredPerMeter = 0.44504; private final double kMinArmAngleRadians = Units.degreesToRadians(Constants.ARMMINRELATVESTART); private final double kMaxArmAngleRadians = Units.degreesToRadians(Constants.ARMMAXRELATIVE); private final double kArmLoadAngleRadians = @@ -44,8 +44,8 @@ public class ArmSubsystem extends SubsystemBase { Units.degreesToRadians(Constants.ARMSPEAKERANGLE); // angle to be when shooting into speaker private final double kArmAmpAngleRadians = Units.degreesToRadians(Constants.ARMAMPANGLE); // angle to be when shooting into amp - private final double karmMaxVelocity = 1.0; // m/s - private final double karmMaxAcceleration = 0.5; // m/s^2 + private final double karmMaxVelocity = 0.25; // rad/s + private final double karmMaxAcceleration = 0.10; // rad/s^2 private boolean kPIDEnabled = true; // general drive constants // https://www.chiefdelphi.com/t/encoders-velocity-to-m-s/390332/2 @@ -94,9 +94,9 @@ public ArmSubsystem(ShooterState shooterState) { m_MainEncoder.setVelocityConversionFactor(kVelocityConversionRatio); m_MainEncoder.setPosition(Units.degreesToRadians(Constants.ARMMINRELATVESTART)); // PID coefficients - kP = 1.7496; + kP = 2.3142; kI = 0; - kD = 0.0068645; + kD = 0.0; kIz = 0; kMaxOutput = 0.4; kMinOutput = -0.4; diff --git a/src/main/java/frc/robot/subsystems/CameraSubsystem.java b/src/main/java/frc/robot/subsystems/CameraSubsystem.java index 31c4a9b..5c143b6 100644 --- a/src/main/java/frc/robot/subsystems/CameraSubsystem.java +++ b/src/main/java/frc/robot/subsystems/CameraSubsystem.java @@ -26,15 +26,15 @@ public class CameraSubsystem extends SubsystemBase { private final PhotonCamera frontCamera; public PhotonPipelineResult frontCameraResult; // Physical location of camera relative to center - private final double CameraLocationXMeters = 0.5; - private final double CameraLocationYMeters = 0.0; - private final double CameraLocationZMeters = 0.5; + private final double CameraLocationXMeters = Units.inchesToMeters(6); + private final double CameraLocationYMeters = Units.inchesToMeters(9.3); + private final double CameraLocationZMeters = Units.inchesToMeters(10.5); // angle of camera / orientation // Cam mounted facing forward, half a meter forward of center, half a meter up from center. - private final double CameraRollRadians = Units.degreesToRadians(0.5); + private final double CameraRollRadians = Units.degreesToRadians(90); private final double CameraPitchRadians = Units.degreesToRadians(0.0); - private final double CameraYawRadians = Units.degreesToRadians(0.5); + private final double CameraYawRadians = Units.degreesToRadians(0); private final Transform3d frontCameraLocation = new Transform3d( diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 7337ead..83e0bb5 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -36,9 +36,9 @@ public class ShooterSubsystem extends SubsystemBase { private final double kVelocityConversionRatio = kPositionConversionRatio / 60; // setup feedforward - private final double ksShooterVolts = 0.17875; - private final double kvDriveVoltSecondsPerMeter = 1.5442; - private final double kaDriveVoltSecondsSquaredPerMeter = 0.019079; + private final double ksShooterVolts = 0.2063; + private final double kvDriveVoltSecondsPerMeter = 1.5611; + private final double kaDriveVoltSecondsSquaredPerMeter = 0.1396; SimpleMotorFeedforward m_shooterFeedForward = new SimpleMotorFeedforward( @@ -67,7 +67,7 @@ public ShooterSubsystem() { m_ShooterMainEncoder.setPositionConversionFactor(kPositionConversionRatio); m_ShooterMainEncoder.setVelocityConversionFactor(kVelocityConversionRatio); // PID coefficients - kP = 0.00032423; + kP = 0.00013373; kI = 0; kD = 0; kIz = 0;