diff --git a/elastic-layout.json b/elastic-layout.json index 82cf1df..53d53bb 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -8,10 +8,10 @@ "layouts": [ { "title": "Tag Processed", - "x": 896.0, + "x": 384.0, "y": 0.0, "width": 256.0, - "height": 384.0, + "height": 256.0, "type": "List Layout", "properties": { "label_position": "TOP" @@ -56,10 +56,26 @@ ], "containers": [ { - "title": "RetractedLimit", - "x": 512.0, + "title": "MatchTime", + "x": 0.0, "y": 0.0, - "width": 256.0, + "width": 384.0, + "height": 256.0, + "type": "Match Time", + "properties": { + "topic": "/AdvantageKit/DriverStation/MatchTime", + "period": 0.06, + "data_type": "double", + "time_display_mode": "Minutes and Seconds", + "red_start_time": 15, + "yellow_start_time": 30 + } + }, + { + "title": "RetractedLimit", + "x": 0.0, + "y": 384.0, + "width": 128.0, "height": 128.0, "type": "Boolean Box", "properties": { @@ -74,9 +90,9 @@ }, { "title": "Auto Chooser", - "x": 896.0, - "y": 384.0, - "width": 256.0, + "x": 1024.0, + "y": 256.0, + "width": 384.0, "height": 128.0, "type": "ComboBox Chooser", "properties": { @@ -87,9 +103,9 @@ }, { "title": "Roller Percent Output", - "x": 512.0, + "x": 0.0, "y": 256.0, - "width": 384.0, + "width": 256.0, "height": 128.0, "type": "Number Slider", "properties": { @@ -104,8 +120,8 @@ }, { "title": "Shooter OVERRIDE Velocity", - "x": 768.0, - "y": 0.0, + "x": 128.0, + "y": 384.0, "width": 128.0, "height": 128.0, "type": "Text Display", @@ -118,8 +134,8 @@ }, { "title": "TRIM SHOOTER SPEED", - "x": 512.0, - "y": 384.0, + "x": 256.0, + "y": 256.0, "width": 384.0, "height": 128.0, "type": "Number Slider", @@ -135,8 +151,8 @@ }, { "title": "Mode", - "x": 512.0, - "y": 128.0, + "x": 256.0, + "y": 384.0, "width": 384.0, "height": 128.0, "type": "Text Display", @@ -148,49 +164,30 @@ } }, { - "title": "IsActive", - "x": 0.0, - "y": 0.0, - "width": 384.0, - "height": 256.0, - "type": "Boolean Box", - "properties": { - "topic": "/AdvantageKit/RealOutputs/HubShift/IsActive", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "RemainingInShift", - "x": 0.0, - "y": 256.0, - "width": 384.0, - "height": 128.0, - "type": "Match Time", - "properties": { - "topic": "/AdvantageKit/RealOutputs/HubShift/RemainingInShift", - "period": 0.06, - "data_type": "double", - "time_display_mode": "Minutes and Seconds", - "red_start_time": 15, - "yellow_start_time": 30 - } - }, - { - "title": "Phase", - "x": 0.0, - "y": 384.0, - "width": 384.0, + "title": "Time to rev", + "x": 768.0, + "y": 128.0, + "width": 256.0, "height": 128.0, "type": "Large Text Display", "properties": { - "topic": "/AdvantageKit/RealOutputs/HubShift/Phase", + "topic": "/AdvantageKit/RealOutputs/Time to rev", "period": 0.06, - "data_type": "string" + "data_type": "double" + } + }, + { + "title": "Arm angle extended", + "x": 1152.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Arm angle extended", + "period": 0.06, + "data_type": "double", + "show_submit_button": true } } ] @@ -530,8 +527,8 @@ }, { "title": "Shooter Idle max current", - "x": 1024.0, - "y": 384.0, + "x": 384.0, + "y": 512.0, "width": 256.0, "height": 128.0, "type": "Text Display", @@ -558,8 +555,8 @@ }, { "title": "Shooter OVERRIDE Velocity", - "x": 1024.0, - "y": 256.0, + "x": 640.0, + "y": 512.0, "width": 256.0, "height": 128.0, "type": "Text Display", @@ -579,59 +576,16 @@ "layouts": [], "containers": [ { - "title": "Hub Dist", - "x": 640.0, - "y": 128.0, - "width": 256.0, - "height": 256.0, - "type": "Text Display", - "properties": { - "topic": "/AdvantageKit/RealOutputs/Hub Dist", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Distance Tolerence", - "x": 256.0, - "y": 128.0, - "width": 256.0, - "height": 256.0, - "type": "Text Display", - "properties": { - "topic": "/SmartDashboard/Distance Tolerence", - "period": 0.06, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "Chassis positive speed offset", - "x": 0.0, - "y": 256.0, - "width": 256.0, - "height": 256.0, - "type": "Text Display", - "properties": { - "topic": "/SmartDashboard/Chassis positive speed offset", - "period": 0.06, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "Chassis negative speed offset", + "title": "Auto Chooser", "x": 0.0, "y": 0.0, - "width": 256.0, - "height": 256.0, - "type": "Text Display", + "width": 640.0, + "height": 768.0, + "type": "ComboBox Chooser", "properties": { - "topic": "/SmartDashboard/Chassis negative speed offset", + "topic": "/SmartDashboard/Auto Chooser", "period": 0.06, - "data_type": "double", - "show_submit_button": true + "sort_options": false } } ] diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index dd82aa7..a9a600b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -125,7 +125,7 @@ public class RobotContainer { // ); private Command RobotRev = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds()), m_robotShooter), + new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter), IntakeExtended, new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake) ); @@ -315,7 +315,7 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> { m_robotIntake.setMode(IntakeMode.Idle); m_robotIntake.rollerStop(); - m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds()); + m_robotShooter.spinUpShooting(); })) .onFalse(new InstantCommand(() -> { m_robotShooter.spinUpIdle(); @@ -352,7 +352,7 @@ public class RobotContainer { // m_robotClimber.toggleDeployed(); // })); - new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) + new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> { m_robotShooter.indexerStalled(); })) diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index eebc943..e0d32a8 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -33,8 +33,6 @@ public class Shooter extends SubsystemBase { public boolean badShooterVelocity; public double distanceToHub = 5; - public double chassisXSpeed = 0; - public Shooter( ShooterIO io, @@ -68,8 +66,7 @@ public class Shooter extends SubsystemBase { private ShooterMode mode = ShooterMode.Idle; private boolean shooterButtonReady = false; - public void spinUpShooting(double chassisXSpeed) { - this.chassisXSpeed = chassisXSpeed; + public void spinUpShooting() { this.mode = ShooterMode.Shooting; } @@ -160,14 +157,14 @@ public class Shooter extends SubsystemBase { //revtime calculations // double shooterAcceleration = - double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub,chassisXSpeed).in(RotationsPerSecond); + double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub).in(RotationsPerSecond); double revTime = (Math.abs(shooterSpeed - shooterSpeedTargetPretend)/((7 - shooterSpeedTargetPretend)/ShooterConstants.T_CONSTANT)); // double revTimeExp = ShooterConstants.T_CONSTANT * Math.log(1 - Math.abs(shooterSpeed/shooterSpeedTargetPretend)); Logger.recordOutput("Time to rev", revTime); switch (mode) { case Shooting: - io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub, chassisXSpeed)); + io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub)); int bitmask = ( (shooterButtonReady ? 1 : 0) + diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index c84e5fc..4ef195a 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -36,8 +36,7 @@ public class ShooterConstants { public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0); - public static final ConfigurableDouble CHASSIS_POS_OFFSET = new ConfigurableDouble("Chassis positive speed offset", 1.0); - public static final ConfigurableDouble CHASSIS_NEG_OFFSET = new ConfigurableDouble("Chassis negative speed offset", 1.0); + // Shoot mode tolerances public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8); public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 4.8); @@ -50,29 +49,16 @@ public class ShooterConstants { public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 3); // - public static AngularVelocity getTargetShooterSpeed(double hubDistMeters, double chassisXSpeed) { + public static AngularVelocity getTargetShooterSpeed(double hubDistMeters) { // Model derived from points // double speed = // 1.11576*hubDistMeters*hubDistMeters + // 0.318464*hubDistMeters + // 30.6293; - double speed; - if (Math.abs(chassisXSpeed) < 0.1) { - speed = + double speed = 0.0593402*hubDistMeters*hubDistMeters + 4.90561*hubDistMeters + 30.35696 + MODEL_TRIM.get(); - } else if (chassisXSpeed > 0) { // positive = further from hub - speed = - 0.0593402*hubDistMeters*hubDistMeters + - 4.90561*hubDistMeters + - 30.35696 + (chassisXSpeed * CHASSIS_POS_OFFSET.get()) + MODEL_TRIM.get(); - } else { // negative = moving towards hub - speed = - 0.0593402*hubDistMeters*hubDistMeters + - 4.90561*hubDistMeters + - 30.35696 + (chassisXSpeed * CHASSIS_NEG_OFFSET.get()) + MODEL_TRIM.get(); - } // double speed = // 0.00610938*hubDistMeters*hubDistMeters @@ -96,8 +82,8 @@ public class ShooterConstants { // Motor Configuration public static Slot0Configs SHOOTER_PID = new Slot0Configs() .withKV(0.0) - .withKP(0.02) - .withKI(0.15) + .withKP(0.05) + .withKI(0.20) .withKD(0.0); @@ -105,7 +91,7 @@ public class ShooterConstants { - public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.02); + public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.08); public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0.15); public static ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0); diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 06f9c06..7e3ce17 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -412,7 +412,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { if (ballVelocity > 1E-3 && Math.abs(chassisSpeeds.vyMetersPerSecond) > 1E-3){ double aimOffset = chassisSpeeds.vyMetersPerSecond*(distanceToHub + SwerveDriveConstants.distanceTolerence.get())/(Math.abs(ballVelocity)); fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset); - Logger.recordOutput("Offset Value", aimOffset); + Logger.recordOutput("Offset Simple", aimOffset); } @@ -512,14 +512,6 @@ public class SwerveDrive extends SubsystemBase implements Queryable { ); // stop the modules without breaking } - public double chassisXSpeeds(){ - if (TimesNegativeOne.isRed) { - return chassisSpeeds.vxMetersPerSecond; - } else { - return -chassisSpeeds.vxMetersPerSecond; - } - } - public void stopModules() { // stopped = true; // swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake());