diff --git a/elastic-layout.json b/elastic-layout.json index e592787..61c7013 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -539,7 +539,7 @@ } }, { - "name": "SwervePID", + "name": "PID", "grid_layout": { "layouts": [], "containers": [ @@ -669,9 +669,9 @@ }, { "title": "flywheelEnableRunning", - "x": 640.0, - "y": 256.0, - "width": 128.0, + "x": 896.0, + "y": 0.0, + "width": 256.0, "height": 128.0, "type": "Toggle Switch", "properties": { @@ -683,8 +683,8 @@ { "title": "flywheelRunningSpeedRPS", "x": 640.0, - "y": 384.0, - "width": 128.0, + "y": 128.0, + "width": 256.0, "height": 128.0, "type": "Text Display", "properties": { @@ -697,8 +697,8 @@ { "title": "flywheelEnable", "x": 640.0, - "y": 128.0, - "width": 128.0, + "y": 0.0, + "width": 256.0, "height": 128.0, "type": "Toggle Switch", "properties": { @@ -706,6 +706,48 @@ "period": 0.06, "data_type": "boolean" } + }, + { + "title": "flywheelkV", + "x": 640.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/flywheel/flywheelkV", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "flywheelkS", + "x": 640.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/flywheel/flywheelkS", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "flywheelkA", + "x": 640.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/flywheel/flywheelkA", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } } ] } @@ -730,10 +772,10 @@ }, { "title": "limelight-shooter", - "x": 384.0, - "y": 0.0, - "width": 256.0, - "height": 256.0, + "x": 768.0, + "y": 512.0, + "width": 640.0, + "height": 384.0, "type": "Camera Stream", "properties": { "topic": "/CameraPublisher/limelight-shooter", @@ -1070,7 +1112,7 @@ "x": 1152.0, "y": 0.0, "width": 384.0, - "height": 384.0, + "height": 256.0, "type": "Camera Stream", "properties": { "topic": "/CameraPublisher/limelight-shooter", @@ -1160,7 +1202,7 @@ { "title": "Field", "x": 1152.0, - "y": 384.0, + "y": 256.0, "width": 384.0, "height": 256.0, "type": "Field", @@ -1237,56 +1279,56 @@ } }, { - "title": "backboardTargetRate", + "title": "backboardTargetPosition", "x": 1024.0, "y": 256.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shooter/backboardTargetRate", + "topic": "/ShooterControl/backboardTargetPosition", "period": 0.06, "data_type": "double", "show_submit_button": true } }, { - "title": "conveyorPIDReference", - "x": 512.0, - "y": 256.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shooter/conveyorPIDReference", - "period": 0.06, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "flywheelPIDReference", + "title": "flywheelTargetSpeed", "x": 768.0, "y": 256.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shooter/flywheelPIDReference", + "topic": "/ShooterControl/flywheelTargetSpeed", "period": 0.06, "data_type": "double", "show_submit_button": true } }, { - "title": "conveyorPIDReference", + "title": "conveyerTargetSpeed", + "x": 512.0, + "y": 256.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/ShooterControl/conveyerTargetSpeed", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "conveyerTargetSpeed", "x": 384.0, "y": 384.0, "width": 256.0, "height": 256.0, "type": "Graph", "properties": { - "topic": "/Shooter/conveyorPIDReference", + "topic": "/ShooterControl/conveyerTargetSpeed", "period": 0.033, "data_type": "double", "time_displayed": 5.0, @@ -1295,14 +1337,14 @@ } }, { - "title": "flywheelPIDReference", + "title": "flywheelTargetSpeed", "x": 640.0, "y": 384.0, "width": 256.0, "height": 256.0, "type": "Graph", "properties": { - "topic": "/Shooter/flywheelPIDReference", + "topic": "/ShooterControl/flywheelTargetSpeed", "period": 0.033, "data_type": "double", "time_displayed": 5.0, @@ -1311,20 +1353,140 @@ } }, { - "title": "backboardTargetRate", + "title": "backboardTargetPosition", "x": 896.0, "y": 384.0, "width": 256.0, "height": 256.0, "type": "Graph", "properties": { - "topic": "/Shooter/backboardTargetRate", + "topic": "/ShooterControl/backboardTargetPosition", "period": 0.033, "data_type": "double", "time_displayed": 5.0, "color": 4278238420, "line_width": 2.0 } + }, + { + "title": "Usingll", + "x": 1152.0, + "y": 512.0, + "width": 384.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Positioning/Usingll", + "period": 0.06, + "data_type": "string", + "show_submit_button": true + } + }, + { + "title": "robotStatus", + "x": 0.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/robotStatus", + "period": 0.06, + "data_type": "string", + "show_submit_button": true + } + } + ] + } + }, + { + "name": "Tab 7", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "ActualVOmega", + "x": 256.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AutoControl/ActualVOmega", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "ActualVX", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AutoControl/ActualVX", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "ActualVY", + "x": 128.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AutoControl/ActualVY", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "BumpDifference", + "x": 384.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AutoControl/BumpDifference", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "TrenchDifference", + "x": 384.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AutoControl/TrenchDifference", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "robotStatus", + "x": 0.0, + "y": 256.0, + "width": 384.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/robotStatus", + "period": 0.06, + "data_type": "string", + "show_submit_button": true + } } ] } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5a16862..e2982bf 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -5,6 +5,14 @@ import edu.wpi.first.math.geometry.Pose2d; public final class Constants { public static final boolean LEDUsing = false; + public enum RobotStatus{ + Stopped, + AllTelop, + AutoAimming, + Climbing, + CrossingBump, + CrossingTrench + } public class Shooter { public static final double conveyorSpeed = 20; public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs() @@ -35,15 +43,16 @@ public final class Constants { public static final double positionMid = 1600; // 中距离背板角度 public static final double postionFar = 3200; // 远距离背板角度 } - public class AutoRotation { - public static final boolean autoRotationToHubEnabled = true; - public static final double autoRotationToHubkP = 0.12; - public static final boolean autoRotationForBump = false; - public static final double autoRotationForBumpkP = 0.2; + public class AutoPositioning { + public static final double autoRotationkP = 0.1; public static final double autoRotationForBumpTargetDegrees = 45; - + public static final double autoRotationForTrenchTargetDegrees = 0; public static final boolean teleopOffsetEnabled = true; public static final double teleopOffsetkP = 15; + + public static final double autoPositioningkP = 0.1; + public static final double[] bumpY = {2.51,5.556}; + public static final double[] trenchY = {0.639,7.43}; } public class StartingPoints{ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e663b52..03670d0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; +import frc.robot.Constants.RobotStatus; import frc.robot.controls.DriveControls; import frc.robot.controls.ShooterControls; import frc.robot.generated.TunerConstants; @@ -41,12 +42,13 @@ public class RobotContainer { // LED 子系统 private final LEDSubsystem leds = new LEDSubsystem(0, 72 ,0); // 总长度=72+72 + private final RobotStatusManager robotStatusManager = new RobotStatusManager(); /* ====================== */ /* 控制封装/日志 */ /* ====================== */ - private final DriveControls driveControls = new DriveControls(drivetrain, controller); - private final ShooterControls shooterControls = new ShooterControls(shooterSubsystem, controller); + private final DriveControls driveControls = new DriveControls(drivetrain, controller,robotStatusManager ); + private final ShooterControls shooterControls = new ShooterControls(shooterSubsystem, controller,robotStatusManager); private final DriveGainsTuner driveGainsTuner = new DriveGainsTuner(drivetrain); private ConfigTalonFXMotorTuner configFlywheelTuner = new ConfigTalonFXMotorTuner(shooterSubsystem.flywheelMotorLeft, "flywheel", Constants.Shooter.flyWheelSlot0Configs); @@ -55,6 +57,7 @@ public class RobotContainer { private final SendableChooser autoChooser; + public RobotContainer() { SignalLogger.enableAutoLogging(false); autoChooser = AutoBuilder.buildAutoChooser(); @@ -72,31 +75,46 @@ public class RobotContainer { Pose2d startingPose = Constants.StartingPoints.Red.Point2; drivetrain.resetPose(startingPose); - // 默认驾驶命令 + // 默认驾驶命令 + /* + * LeftX 底盘左右 + * LeftY 底盘前后 Bump/Trench偏移未完成 + * RightX Aiming偏移 AllTelop模式下 底盘旋转 + * + */ drivetrain.setDefaultCommand(driveControls.defaultDriveCommand()); // Disabled 时进入 idle（防止模块乱动） RobotModeTriggers.disabled().whileTrue(driveControls.idleCommand()); - //左保险：按住刹车 - controller.leftBumper() - .onTrue(Commands.runOnce(driveControls::emergencyStop)) - .whileTrue(driveControls.brakeWhileHeld()); + // //左保险：按住刹车 + // controller.leftBumper() + // .onTrue(Commands.runOnce(driveControls::emergencyStop)) + // .whileTrue(driveControls.brakeWhileHeld()); + + // // 右保险：按住加速（Boost） + // controller.rightBumper() + // .onTrue(Commands.runOnce(() -> driveControls.setBoostEnabled(true))) + // .onFalse(Commands.runOnce(() -> driveControls.setBoostEnabled(false))); - // Back：重置场地坐标系 + // Back：重置场地坐标系角度 controller.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric)); - // Start：切换场地/车体坐标系 - controller.start().onTrue(Commands.runOnce(driveControls::toggleDriveFrame)); + // Start：强行使用metaTag2全场定位 + controller.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightMT2)); - // 右保险：按住加速（Boost） - controller.rightBumper() - .onTrue(Commands.runOnce(() -> driveControls.setBoostEnabled(true))) - .onFalse(Commands.runOnce(() -> driveControls.setBoostEnabled(false))); - - controller.a().whileTrue(drivetrain.runOnce(drivetrain::forceUsingLimelightMT2)); + //LM 按下时使用机器人坐标系 + controller.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming)); + controller.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); + controller.rightStick().and(controller.leftBumper()) + .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingBump)) + .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); + + controller.rightStick().and(controller.rightBumper()) + .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingTrench)) + .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); } private void configueShooter(){ @@ -107,7 +125,6 @@ public class RobotContainer { controller.povUp().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(100))); controller.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-100))); } - /* ====================== */ /* LED 绑定 */ /* ====================== */ diff --git a/src/main/java/frc/robot/RobotStatusManager.java b/src/main/java/frc/robot/RobotStatusManager.java new file mode 100644 index 0000000..67050b0 --- /dev/null +++ b/src/main/java/frc/robot/RobotStatusManager.java @@ -0,0 +1,25 @@ +package frc.robot; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Constants.RobotStatus; + +public class RobotStatusManager { + private RobotStatus robotStatus; + + public RobotStatusManager(){ + this.robotStatus = RobotStatus.AllTelop; + } + + public RobotStatus getStatus(){ + return this.robotStatus; + } + + public void setStatus(RobotStatus rs){ + this.robotStatus = rs; + } + + public Command setStatusCommand(RobotStatus rs){ + return Commands.runOnce(()->setStatus(rs)); + } +} diff --git a/src/main/java/frc/robot/controls/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java index 570fd70..db2d524 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -8,10 +8,13 @@ import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants; +import frc.robot.RobotStatusManager; import frc.robot.Constants.DriveMode; +import frc.robot.Constants.RobotStatus; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -43,6 +46,7 @@ public class DriveControls { private NetworkTable autoControlNetworkTable; + private final RobotStatusManager robotStatusManager; public void setBoostEnabled(boolean enabled) { boostEnabled = enabled; } @@ -62,36 +66,34 @@ public class DriveControls { .withRotationalDeadband(maxAngularRate * 0.05) .withDriveRequestType(DriveRequestType.Velocity); - public DriveControls(CommandSwerveDrivetrain drivetrain, CommandXboxController driver) { + public DriveControls(CommandSwerveDrivetrain drivetrain, CommandXboxController driver, RobotStatusManager rsm) { this.drivetrain = drivetrain; this.driver = driver; + this.robotStatusManager = rsm; autoControlNetworkTable = NetworkTableInstance.getDefault().getTable("AutoControl"); } - public void toggleDriveFrame() { - fieldCentricEnabled = !fieldCentricEnabled; + public void setFieldCentricEnabled(boolean enabled) { + fieldCentricEnabled = enabled; } public boolean isFieldCentricEnabled() { return fieldCentricEnabled; } - public DriveMode getCurrentDriveMode() { - return fieldCentricEnabled ? DriveMode.FIELD_CENTRIC : DriveMode.ROBOT_CENTRIC; - } + /** 底盘的默认驾驶命令（teleop 期间持续执行） */ public Command defaultDriveCommand() { return drivetrain.applyRequest(() -> { + SmartDashboard.putString("robotStatus",robotStatusManager.getStatus().name()); double driveScale = boostEnabled ? boostDriveScale : normalDriveScale; double turnScale = boostEnabled ? boostTurnScale : normalTurnScale; - double vx = -driver.getLeftY() * maxSpeed * driveScale; double vy = -driver.getLeftX() * maxSpeed * driveScale; - double omega = -driver.getRightX() * maxAngularRate * turnScale; - + double vomega = -driver.getRightX() * maxAngularRate * turnScale; // ====== execute 内替换 limiter：只限加速、不限减速 ====== double limitedVx = limitAccelOnly(vx, lastVx, vxLimiter); double limitedVy = limitAccelOnly(vy, lastVy, vyLimiter); - double limitedOmega = limitAccelOnly(omega, lastOmega, omegaLimiter); + double limitedOmega = limitAccelOnly(vomega, lastOmega, omegaLimiter); lastVx = limitedVx; lastVy = limitedVy; @@ -99,45 +101,100 @@ public class DriveControls { vx = limitedVx; vy = limitedVy; - omega = limitedOmega; - + vomega = limitedOmega; + double[] distanceAndRotation = calculateDistanceAndRotationToHub(); // 将距离值写入 NetworkTable autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]); autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]); - if(Constants.AutoRotation.autoRotationToHubEnabled) { - double autoRotationPower = driver.getLeftTriggerAxis(); - if(autoRotationPower > 0.1) { - autoControlNetworkTable.getEntry("autoRotationAvailable").setBoolean(true); + switch (robotStatusManager.getStatus()) { + case Stopped: + vx = 0; + vy = 0; + vomega = 0; + break; + case AutoAimming: + vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]); + autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega); + break; + case AllTelop: + autoControlNetworkTable.getEntry("autoRotationRate").setDouble(Double.NaN); + break; + case Climbing: + break; + case CrossingBump: + double differenceBump = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.bumpY[0], Constants.AutoPositioning.bumpY[1]); + vy = differenceBump * Constants.AutoPositioning.autoPositioningkP; + autoControlNetworkTable.getEntry("BumpDifference").setDouble(differenceBump); + //vomega = calculateRotationSpeedForBump(); + break; + case CrossingTrench: + double differenceTrench = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.trenchY[0], Constants.AutoPositioning.trenchY[1]); + vy = differenceTrench * Constants.AutoPositioning.autoPositioningkP; + autoControlNetworkTable.getEntry("TrenchDifference").setDouble(differenceTrench); + //vomega = calculateRotationSpeedForTrench(); + break; + } - double angleDifference = distanceAndRotation[1]; + autoControlNetworkTable.getEntry("ActualVX").setDouble(vx); + autoControlNetworkTable.getEntry("ActualVY").setDouble(vy); + autoControlNetworkTable.getEntry("ActualVOmega").setDouble(vomega); - // Calculate rotation speed using the angle difference - omega = calculateRotationSpeedFromRotationAngle(angleDifference, autoRotationPower); - - autoControlNetworkTable.getEntry("autoRotationRate").setDouble(omega); - } else { - autoControlNetworkTable.getEntry("autoRotationAvailable").setBoolean(false); - } - } - - - - if (fieldCentricEnabled) { + if (fieldCentricEnabled || + robotStatusManager.getStatus() == RobotStatus.CrossingBump || + robotStatusManager.getStatus() == RobotStatus.CrossingTrench) { return fieldCentric .withVelocityX(vx) .withVelocityY(vy) - .withRotationalRate(omega); + .withRotationalRate(vomega); } else { return robotCentric .withVelocityX(vx) .withVelocityY(vy) - .withRotationalRate(omega); + .withRotationalRate(vomega); } - }, () -> fieldCentricEnabled ? DriveMode.FIELD_CENTRIC : DriveMode.ROBOT_CENTRIC); + }, () -> fieldCentricEnabled || + robotStatusManager.getStatus() == RobotStatus.CrossingBump || + robotStatusManager.getStatus() == RobotStatus.CrossingTrench + ? DriveMode.FIELD_CENTRIC : DriveMode.ROBOT_CENTRIC); + } + + /** + * 输出移动到距离两个目标之间较近的那个点的位移 + * @return 位移 有正负 + */ + public double calculateDifferenceToTwoTarget(double current, double target1, double target2){ + double x1 = target1 - current; + double x2 = target2 - current; + return Math.abs(x1) < Math.abs(x2) ? x1 : x2; + } + public double calculateRotationSpeedForTrench(){ + Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度 + double currentAngle = currentPose.getRotation().getDegrees(); + double targetAngle = Constants.AutoPositioning.autoRotationForTrenchTargetDegrees; // 转换为度 + double angleDifference = targetAngle - currentAngle; + // Normalize angle difference to the range [-180, 180] + if (angleDifference > 180) { + angleDifference -= 360; + } else if (angleDifference < -180) { + angleDifference += 360; + } + return calculateRotationSpeedFromRotationAngle(angleDifference); + } + public double calculateRotationSpeedForBump(){ + Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度 + double currentAngle = currentPose.getRotation().getDegrees(); + double targetAngle = Constants.AutoPositioning.autoRotationForBumpTargetDegrees; // 转换为度 + double angleDifference = targetAngle - currentAngle; + // Normalize angle difference to the range [-180, 180] + if (angleDifference > 180) { + angleDifference -= 360; + } else if (angleDifference < -180) { + angleDifference += 360; + } + return calculateRotationSpeedFromRotationAngle(angleDifference); } - // Calculate distance and rotation (angle difference) public double[] calculateDistanceAndRotationToHub() { Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度 @@ -169,18 +226,18 @@ public class DriveControls { } // Calculate rotation speed from rotation angle difference - public double calculateRotationSpeedFromRotationAngle(double angleDifference, double autoRotationPower) { + public double calculateRotationSpeedFromRotationAngle(double angleDifference) { double calDifference = angleDifference; // Optional: Apply teleop offset if enabled - if (Constants.AutoRotation.teleopOffsetEnabled) { + if (Constants.AutoPositioning.teleopOffsetEnabled) { double teleopOffset = -driver.getRightX(); - calDifference += teleopOffset * Constants.AutoRotation.teleopOffsetkP; + calDifference += teleopOffset * Constants.AutoPositioning.teleopOffsetkP; } // If angle difference is within threshold, don't rotate if (Math.abs(calDifference) > 1.0) { - return calDifference * autoRotationPower * Constants.AutoRotation.autoRotationToHubkP; + return calDifference * Constants.AutoPositioning.autoRotationkP; } else { return 0; } diff --git a/src/main/java/frc/robot/controls/ShooterControls.java b/src/main/java/frc/robot/controls/ShooterControls.java index df4907e..42397a5 100644 --- a/src/main/java/frc/robot/controls/ShooterControls.java +++ b/src/main/java/frc/robot/controls/ShooterControls.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants; +import frc.robot.RobotStatusManager; import frc.robot.subsystems.ShooterSubsystem; public class ShooterControls { @@ -15,39 +16,64 @@ public class ShooterControls { private double flywheelSpeedOffset = 0.0; private double backboardPositionOffset = 0.0; private NetworkTable shooterControlTable; - public ShooterControls(ShooterSubsystem shooterSubsystem, CommandXboxController controller) { + private final RobotStatusManager robotStatusManager; + public ShooterControls(ShooterSubsystem shooterSubsystem, CommandXboxController controller,RobotStatusManager rsm) { this.shooterSubsystem = shooterSubsystem; this.controller = controller; - + this.robotStatusManager = rsm; shooterControlTable = NetworkTableInstance.getDefault().getTable("ShooterControl"); } public Command defaultShooterCommand() { return Commands.run(() -> { - double flywheelSpeed; - double backboardPosition; - double conveyorSpeed; - // flywheelSpeed = calculateFlywheelSpeedOnlywithOffset(60); - var x = shooterSubsystem.getDistance(); - flywheelSpeed = 0.6956596811733782*x*x*x*x*x*x*x-14.164408860183267*x*x*x*x*x*x+119.7115375240918*x*x*x*x*x-543.0984876677477*x*x*x*x+1425.1928543240738*x*x*x-2156.199296490315*x*x+1736.568139600292*x-516.097598057351; - flywheelSpeed += flywheelSpeedOffset; - backboardPosition = -122.11889023593778*x*x*x*x*x*x*x+2453.385583841308*x*x*x*x*x*x-20553.05413872581*x*x*x*x*x+92963.33175750206*x*x*x*x-244905.25916147238*x*x*x+375150.1363997868*x*x-308031.51715612336*x+104203.52690842684; - backboardPosition += backboardPositionOffset; - conveyorSpeed = Constants.Shooter.conveyorSpeed; - shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed); - shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed); - shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition); - // System.out.println("running"); - if (controller.getRightTriggerAxis() < 0.1) { - flywheelSpeed = 0; - conveyorSpeed = 0; - } + switch (robotStatusManager.getStatus()) { + case Stopped: + shooterSubsystem.stopMotors(); + break; + case AllTelop: + case AutoAimming: + double flywheelSpeed = 0; + double backboardPosition = 0; + double conveyorSpeed = 0; + var x = shooterSubsystem.getDistance(); + flywheelSpeed = 0.6956596811733782*x*x*x*x*x*x*x-14.164408860183267*x*x*x*x*x*x+119.7115375240918*x*x*x*x*x-543.0984876677477*x*x*x*x+1425.1928543240738*x*x*x-2156.199296490315*x*x+1736.568139600292*x-516.097598057351; + flywheelSpeed += flywheelSpeedOffset; + backboardPosition = -122.11889023593778*x*x*x*x*x*x*x+2453.385583841308*x*x*x*x*x*x-20553.05413872581*x*x*x*x*x+92963.33175750206*x*x*x*x-244905.25916147238*x*x*x+375150.1363997868*x*x-308031.51715612336*x+104203.52690842684; + backboardPosition += backboardPositionOffset; + conveyorSpeed = Constants.Shooter.conveyorSpeed; + if (controller.getRightTriggerAxis() < 0.1) { + flywheelSpeed = 0; + conveyorSpeed = 0; + } + // flywheelSpeed = calculateFlywheelSpeedOnlywithOffset(60); + // double flywheelSpeed = calculateFlywheelSpeed(0); + // double BackboardPosition = calculateBackboardPosition(0); + shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed); + shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed); + shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition); + shooterSubsystem.setFlywheelSpeedByRPS(flywheelSpeed); + shooterSubsystem.setConveyorSpeedByRPS(conveyorSpeed); + shooterSubsystem.setBackboardPosition(backboardPosition); + if(shooterSubsystem.isBackboardAtTarget()){ + shooterSubsystem.backboardMotor.set(0); + }else{ + shooterSubsystem.outputBackboard(); + } + break; + case Climbing: + case CrossingBump: + case CrossingTrench: + if(shooterSubsystem.getBackboardPosition() <= 20) + shooterSubsystem.stopMotors(); + else{ + shooterSubsystem.setBackboardPosition(0); + shooterSubsystem.outputBackboard(); + } + break; + + } + - // double flywheelSpeed = calculateFlywheelSpeed(0); - // double BackboardPosition = calculateBackboardPosition(0); - shooterSubsystem.setFlywheelSpeedByRPS(flywheelSpeed); - shooterSubsystem.setConveyorSpeedByRPS(conveyorSpeed); - shooterSubsystem.setBackboardPosition(backboardPosition); }, shooterSubsystem); } diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 64b4b90..46987f0 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -43,7 +43,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ - private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.kCW_90deg; + private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; /* Keep track if we've ever applied the operator perspective before or not */ private boolean m_hasAppliedOperatorPerspective = false; @@ -278,6 +278,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su } @Override public void periodic() { + Pose2d pose = getState().Pose; m_field.setRobotPose(pose); posePub.set(pose); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 626d3bc..23337d0 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -33,12 +33,7 @@ public class ShooterSubsystem extends SubsystemBase { private final VelocityDutyCycle velocityRequest = new VelocityDutyCycle(0); - public enum Mode{ - Inalyzed, - UnInalyed, - Inalyzing; - } - public Mode currentBackboardInalyzeMode; + // 初始化各个部件 public ShooterSubsystem() { // 初始化飞轮电机 @@ -75,7 +70,6 @@ public class ShooterSubsystem extends SubsystemBase { shooterNetworkTable = NetworkTableInstance.getDefault().getTable("Shooter"); - currentBackboardInalyzeMode = Mode.UnInalyed; } public void setRightFollowLeft(){ @@ -145,14 +139,7 @@ public class ShooterSubsystem extends SubsystemBase { @Override public void periodic() { - if(currentBackboardInalyzeMode != Mode.Inalyzing){ - backboardPID.calculate(getBackboardPosition()); - if(isBackboardAtTarget()){ - backboardMotor.set(0); - }else{ - outputBackboard(); - } - } + backboardPID.calculate(getBackboardPosition()); shooterNetworkTable.getEntry("flywheelSpeed").setDouble(flywheelMotorLeft.getVelocity().getValueAsDouble()); shooterNetworkTable.getEntry("flywheelPIDReference").setDouble(flywheelMotorLeft.getClosedLoopReference().getValueAsDouble()); shooterNetworkTable.getEntry("conveyorSpeed").setDouble(conveyorMotor.getVelocity().getValueAsDouble()); @@ -161,7 +148,6 @@ public class ShooterSubsystem extends SubsystemBase { shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardPID.getSetpoint().position); shooterNetworkTable.getEntry("isBackboardAtTarget").setBoolean(isBackboardAtTarget()); shooterNetworkTable.getEntry("distanceGetted").setDouble(getDistance()); - shooterNetworkTable.getEntry("InalyzeBackboardMode").setString(currentBackboardInalyzeMode.name()); } public double getDistance(){