admin6494/FRC6494_2026Regional-offseason
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

键位定义 手动协作 bug没改完

Browse Source
This commit is contained in:
OnDYCLaptop
2026-02-23 22:52:10 +08:00
parent b347e93078
commit 146e373f8c
8 changed files with 423 additions and 140 deletions
Download Patch File Download Diff File Expand all files Collapse all files
elastic-layout.json
+200 -38
View File
@@ -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
}
}
]
}
src/main/java/frc/robot/Constants.java
+15 -6
View File
@@ -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{
src/main/java/frc/robot/RobotContainer.java
+32 -15
View File
@@ -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<Command> autoChooser;
public RobotContainer() {
SignalLogger.enableAutoLogging(false);
autoChooser = AutoBuilder.buildAutoChooser();
@@ -73,30 +76,45 @@ public class RobotContainer {
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());
// Back：重置场地坐标系
// // 右保险：按住加速（Boost）
// controller.rightBumper()
// .onTrue(Commands.runOnce(() -> driveControls.setBoostEnabled(true)))
// .onFalse(Commands.runOnce(() -> driveControls.setBoostEnabled(false)));
// 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)));
//LM 按下时使用机器人坐标系
controller.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
controller.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
controller.a().whileTrue(drivetrain.runOnce(drivetrain::forceUsingLimelightMT2));
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 绑定 */
/* ====================== */
src/main/java/frc/robot/RobotStatusManager.java
+25
View File
@@ -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));
}
}
src/main/java/frc/robot/controls/DriveControls.java
+91 -34
View File
@@ -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);
double angleDifference = distanceAndRotation[1];
// Calculate rotation speed using the angle difference
omega = calculateRotationSpeedFromRotationAngle(angleDifference, autoRotationPower);
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(omega);
} else {
autoControlNetworkTable.getEntry("autoRotationAvailable").setBoolean(false);
}
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;
}
autoControlNetworkTable.getEntry("ActualVX").setDouble(vx);
autoControlNetworkTable.getEntry("ActualVY").setDouble(vy);
autoControlNetworkTable.getEntry("ActualVOmega").setDouble(vomega);
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;
}
src/main/java/frc/robot/controls/ShooterControls.java
+37 -11
View File
@@ -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);
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;
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;
}
// 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;
}
}, shooterSubsystem);
}
src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
+2 -1
View File
@@ -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);
src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+1 -15
View File
@@ -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();
}
}
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(){