diff --git a/elastic-layout.json b/elastic-layout.json index 53d53bb..82cf1df 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -8,10 +8,10 @@ "layouts": [ { "title": "Tag Processed", - "x": 384.0, + "x": 896.0, "y": 0.0, "width": 256.0, - "height": 256.0, + "height": 384.0, "type": "List Layout", "properties": { "label_position": "TOP" @@ -55,27 +55,11 @@ } ], "containers": [ - { - "title": "MatchTime", - "x": 0.0, - "y": 0.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, + "x": 512.0, + "y": 0.0, + "width": 256.0, "height": 128.0, "type": "Boolean Box", "properties": { @@ -90,9 +74,9 @@ }, { "title": "Auto Chooser", - "x": 1024.0, - "y": 256.0, - "width": 384.0, + "x": 896.0, + "y": 384.0, + "width": 256.0, "height": 128.0, "type": "ComboBox Chooser", "properties": { @@ -103,9 +87,9 @@ }, { "title": "Roller Percent Output", - "x": 0.0, + "x": 512.0, "y": 256.0, - "width": 256.0, + "width": 384.0, "height": 128.0, "type": "Number Slider", "properties": { @@ -120,8 +104,8 @@ }, { "title": "Shooter OVERRIDE Velocity", - "x": 128.0, - "y": 384.0, + "x": 768.0, + "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", @@ -134,8 +118,8 @@ }, { "title": "TRIM SHOOTER SPEED", - "x": 256.0, - "y": 256.0, + "x": 512.0, + "y": 384.0, "width": 384.0, "height": 128.0, "type": "Number Slider", @@ -151,8 +135,8 @@ }, { "title": "Mode", - "x": 256.0, - "y": 384.0, + "x": 512.0, + "y": 128.0, "width": 384.0, "height": 128.0, "type": "Text Display", @@ -164,30 +148,49 @@ } }, { - "title": "Time to rev", - "x": 768.0, - "y": 128.0, - "width": 256.0, - "height": 128.0, - "type": "Large Text Display", + "title": "IsActive", + "x": 0.0, + "y": 0.0, + "width": 384.0, + "height": 256.0, + "type": "Boolean Box", "properties": { - "topic": "/AdvantageKit/RealOutputs/Time to rev", + "topic": "/AdvantageKit/RealOutputs/HubShift/IsActive", "period": 0.06, - "data_type": "double" + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" } }, { - "title": "Arm angle extended", - "x": 1152.0, - "y": 128.0, - "width": 256.0, + "title": "RemainingInShift", + "x": 0.0, + "y": 256.0, + "width": 384.0, "height": 128.0, - "type": "Text Display", + "type": "Match Time", "properties": { - "topic": "/SmartDashboard/Arm angle extended", + "topic": "/AdvantageKit/RealOutputs/HubShift/RemainingInShift", "period": 0.06, "data_type": "double", - "show_submit_button": true + "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, + "height": 128.0, + "type": "Large Text Display", + "properties": { + "topic": "/AdvantageKit/RealOutputs/HubShift/Phase", + "period": 0.06, + "data_type": "string" } } ] @@ -527,8 +530,8 @@ }, { "title": "Shooter Idle max current", - "x": 384.0, - "y": 512.0, + "x": 1024.0, + "y": 384.0, "width": 256.0, "height": 128.0, "type": "Text Display", @@ -555,8 +558,8 @@ }, { "title": "Shooter OVERRIDE Velocity", - "x": 640.0, - "y": 512.0, + "x": 1024.0, + "y": 256.0, "width": 256.0, "height": 128.0, "type": "Text Display", @@ -576,16 +579,59 @@ "layouts": [], "containers": [ { - "title": "Auto Chooser", + "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", "x": 0.0, "y": 0.0, - "width": 640.0, - "height": 768.0, - "type": "ComboBox Chooser", + "width": 256.0, + "height": 256.0, + "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Auto Chooser", + "topic": "/SmartDashboard/Chassis negative speed offset", "period": 0.06, - "sort_options": false + "data_type": "double", + "show_submit_button": true } } ] diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a9a600b..dd82aa7 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_robotShooter), + new InstantCommand(() -> m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds()), 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_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds()); })) .onFalse(new InstantCommand(() -> { m_robotShooter.spinUpIdle(); @@ -352,7 +352,7 @@ public class RobotContainer { // m_robotClimber.toggleDeployed(); // })); - new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) + new JoystickButton(getDeadbandedOperatorController(), XboxController.B_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 e0d32a8..eebc943 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -33,6 +33,8 @@ public class Shooter extends SubsystemBase { public boolean badShooterVelocity; public double distanceToHub = 5; + public double chassisXSpeed = 0; + public Shooter( ShooterIO io, @@ -66,7 +68,8 @@ public class Shooter extends SubsystemBase { private ShooterMode mode = ShooterMode.Idle; private boolean shooterButtonReady = false; - public void spinUpShooting() { + public void spinUpShooting(double chassisXSpeed) { + this.chassisXSpeed = chassisXSpeed; this.mode = ShooterMode.Shooting; } @@ -157,14 +160,14 @@ public class Shooter extends SubsystemBase { //revtime calculations // double shooterAcceleration = - double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub).in(RotationsPerSecond); + double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub,chassisXSpeed).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)); + io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub, chassisXSpeed)); 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 4ef195a..c84e5fc 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -36,7 +36,8 @@ 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); @@ -49,16 +50,29 @@ public class ShooterConstants { public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 3); // - public static AngularVelocity getTargetShooterSpeed(double hubDistMeters) { + public static AngularVelocity getTargetShooterSpeed(double hubDistMeters, double chassisXSpeed) { // Model derived from points // double speed = // 1.11576*hubDistMeters*hubDistMeters + // 0.318464*hubDistMeters + // 30.6293; - double speed = + double speed; + if (Math.abs(chassisXSpeed) < 0.1) { + 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 @@ -82,8 +96,8 @@ public class ShooterConstants { // Motor Configuration public static Slot0Configs SHOOTER_PID = new Slot0Configs() .withKV(0.0) - .withKP(0.05) - .withKI(0.20) + .withKP(0.02) + .withKI(0.15) .withKD(0.0); @@ -91,7 +105,7 @@ public class ShooterConstants { - public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.08); + public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.02); 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 7e3ce17..06f9c06 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 Simple", aimOffset); + Logger.recordOutput("Offset Value", aimOffset); } @@ -512,6 +512,14 @@ 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());