diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ad4807d..deccbe3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,6 +6,7 @@ import static edu.wpi.first.units.Units.RadiansPerSecond; +import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; @@ -17,6 +18,7 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.statemachines.DriveState; import frc.robot.statemachines.LaunchState; import frc.robot.subsystems.drive.DriveConstants; @@ -174,9 +176,6 @@ public void configureTestBindings() { RobotModeTriggers.disabled() .whileTrue(drivetrain.applyRequest(() -> idle).ignoringDisable(true)); - // driverJoystick.rightBumper().onTrue(Commands.runOnce(SignalLogger::start)); - // driverJoystick.leftBumper().onTrue(Commands.runOnce(SignalLogger::stop)); - // operatorJoystick.y().whileTrue(shooter.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); // operatorJoystick.a().whileTrue(shooter.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); // operatorJoystick.b().whileTrue(shooter.sysIdDynamic(SysIdRoutine.Direction.kForward)); @@ -309,6 +308,15 @@ public void configureTeleopBindings() { operatorJoystick.rightBumper().onTrue(shooter.stopFlywheelOutputCommand()); drivetrain.registerTelemetry(logger::telemeterize); + + SmartDashboard.putData( + "Shooter/SysIdForwardQuasistatic", shooter.sysIdQuasistatic(Direction.kForward)); + SmartDashboard.putData( + "Shooter/SysIdReverseQuasistatic", shooter.sysIdQuasistatic(Direction.kReverse)); + SmartDashboard.putData("Shooter/SysIdForwardDynamic", shooter.sysIdDynamic(Direction.kForward)); + SmartDashboard.putData("Shooter/SysIdReverseDynamic", shooter.sysIdDynamic(Direction.kReverse)); + SmartDashboard.putData("Start Logger", Commands.runOnce(SignalLogger::start)); + SmartDashboard.putData("Stop Logger", Commands.runOnce(SignalLogger::stop)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index f0a1fb5..206d4e1 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -70,9 +70,10 @@ public static OpenLoopRampsConfigs createRollerMotorRampConfigs() { // Extension Motor public static final double ALLOWABLE_EXTENSION_ERROR = 1; - public static final double EXTENSION_KS = 0.456; - public static final double EXTENSION_KP = 6.8; - public static final double EXTENSION_KD = 0.2; + public static final double EXTENSION_KS = 0.468; + public static final double EXTENSION_KP = 7.5; + public static final double EXTENSION_KI = 1.75; + public static final double EXTENSION_KD = 0.3; public static Slot0Configs createExtensionMotorSlot0Configs() { Slot0Configs slot = new Slot0Configs(); @@ -105,8 +106,8 @@ public static Slot2Configs createExtensionMotorSlot2Configs() { return slot; } - public static final double INTAKE_FORWARD_LIMIT = 14.55; - public static final double INTAKE_REVERSE_LIMIT = 0.1; + public static final double INTAKE_FORWARD_LIMIT = 14.6; + public static final double INTAKE_REVERSE_LIMIT = 3.5; public static SoftwareLimitSwitchConfigs createExtensionSoftwareLimitSwitchConfigs() { SoftwareLimitSwitchConfigs configs = new SoftwareLimitSwitchConfigs(); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 11b8682..5881ae3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -29,7 +29,7 @@ public static MotorOutputConfigs createRightFlywheelMotorOutputConfigs() { public static MotorOutputConfigs createLeftFlywheelLeaderMotorOutputConfigs() { MotorOutputConfigs newConfigs = new MotorOutputConfigs(); - newConfigs.Inverted = InvertedValue.Clockwise_Positive; + newConfigs.Inverted = InvertedValue.CounterClockwise_Positive; newConfigs.NeutralMode = NeutralModeValue.Coast; return newConfigs; } @@ -90,18 +90,16 @@ public static Slot0Configs createHoodMotorSlot0Configs() { return slot; } - /* + // TODO: Tune Flywheel and Hood Motor // Flywheel motor - public static final double FLYWHEEL_KS = 0.11239; - public static final double FLYWHEEL_KV = 0.11589; - public static final double FLYWHEEL_KA = 0.0034356; - public static final double FLYWHEEL_KP = 0.066945 * 2; + public static final double FLYWHEEL_KV = 0.12807; + public static final double FLYWHEEL_KA = 0.020039; + public static final double FLYWHEEL_KP = 0.17969; public static Slot0Configs createFlywheelMotorSlot0Configs() { Slot0Configs slot = new Slot0Configs(); - slot.kS = FLYWHEEL_KS; slot.kV = FLYWHEEL_KV; slot.kP = FLYWHEEL_KP; slot.kA = FLYWHEEL_KA; @@ -109,6 +107,7 @@ public static Slot0Configs createFlywheelMotorSlot0Configs() { } + /* public static final double ALLOWABLE_HOOD_ERROR = 0.1; public static final double HOOD_FORWARD_LIMIT = 6.2; public static final double HOOD_REVERSE_LIMIT = 0; @@ -128,6 +127,7 @@ public static MotorOutputConfigs createHoodMotorOutputConfigs() { newConfigs.NeutralMode = NeutralModeValue.Brake; return newConfigs; } + public static final DutyCycleOut SAFE_HOMING_EFFORT = new DutyCycleOut(-0.2); public static final Current SAFE_STATOR_LIMIT = Amp.of(0.8);