first commit
This commit is contained in:
HarryWangOnLaptop
@@ -0,0 +1,3 @@
|
||||
Files placed in this directory will be deployed to the RoboRIO into the
|
||||
'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function
|
||||
to get a proper path relative to the deploy directory.
|
||||
@@ -0,0 +1,19 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Line"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": false,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -0,0 +1,37 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Line"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 1.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "LineBack"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 1.0
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": false,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Rotate"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": false,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "RotateAndLine"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": false,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
File diff suppressed because one or more lines are too long
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 14.314,
|
||||
"y": 4.15
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 14.299415121255347,
|
||||
"y": 4.61077032810271
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "StartingPointRed1"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 14.314,
|
||||
"y": 6.668017118402283
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 14.247660485021397,
|
||||
"y": 6.305734664764623
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 1.5,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 14.314,
|
||||
"y": 6.668017118402283
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 14.299415121255347,
|
||||
"y": 7.128787446504993
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "Point1"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 14.314,
|
||||
"y": 4.15
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 14.299415121255347,
|
||||
"y": 4.597831669044222
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "StartingPointRed2"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 1.5,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 14.314,
|
||||
"y": 4.15
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 14.064,
|
||||
"y": 4.15
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "StartingPointRed1"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 14.314,
|
||||
"y": 4.15
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 14.650181888846735,
|
||||
"y": 4.15
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 1.5,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 14.314,
|
||||
"y": 4.15
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 14.312353780313838,
|
||||
"y": 4.74015691868759
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "StartingPointRed1"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 14.314,
|
||||
"y": 6.357489300998574
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 14.312353780313838,
|
||||
"y": 6.098716119828817
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 1.5,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -0,0 +1,34 @@
|
||||
{
|
||||
"robotWidth": 0.85,
|
||||
"robotLength": 0.85,
|
||||
"holonomicMode": true,
|
||||
"pathFolders": [],
|
||||
"autoFolders": [],
|
||||
"defaultMaxVel": 1.5,
|
||||
"defaultMaxAccel": 3.0,
|
||||
"defaultMaxAngVel": 360.0,
|
||||
"defaultMaxAngAccel": 720.0,
|
||||
"defaultNominalVoltage": 12.0,
|
||||
"robotMass": 30.0,
|
||||
"robotMOI": 3.6,
|
||||
"robotTrackwidth": 0.546,
|
||||
"driveWheelRadius": 0.045,
|
||||
"driveGearing": 5.902,
|
||||
"maxDriveSpeed": 4.89,
|
||||
"driveMotorType": "krakenX60",
|
||||
"driveCurrentLimit": 40.0,
|
||||
"wheelCOF": 1.2,
|
||||
"flModuleX": 0.289,
|
||||
"flModuleY": 0.289,
|
||||
"frModuleX": 0.289,
|
||||
"frModuleY": -0.289,
|
||||
"blModuleX": -0.289,
|
||||
"blModuleY": 0.289,
|
||||
"brModuleX": -0.289,
|
||||
"brModuleY": -0.289,
|
||||
"bumperOffsetX": 0.0,
|
||||
"bumperOffsetY": 0.0,
|
||||
"robotFeatures": [
|
||||
"{\"name\":\"Circle\",\"type\":\"circle\",\"data\":{\"center\":{\"x\":0.0,\"y\":0.0},\"radius\":0.1,\"strokeWidth\":0.02,\"filled\":false}}"
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,98 @@
|
||||
package frc.robot;
|
||||
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
|
||||
public final class Constants {
|
||||
public static final boolean LEDUsing = false;
|
||||
public class Shooter {
|
||||
public static final double conveyorSpeedkP = 10;
|
||||
public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs()
|
||||
.withKP(0.0).withKI(0).withKD(0)
|
||||
.withKV(0.1).withKA(0.0).withKS(0.15);
|
||||
|
||||
public static final Slot0Configs conveyorSlot0Configs = new Slot0Configs()
|
||||
.withKP(0.0).withKI(0).withKD(0)
|
||||
.withKV(0.02).withKA(0.15).withKS(0);
|
||||
|
||||
public static final Slot0Configs backboardSlot0Configs = new Slot0Configs()
|
||||
.withKP(0.01).withKI(0).withKD(0)
|
||||
.withKV(0.008).withKA(0.0).withKS(0.010);
|
||||
|
||||
public class backboardPositionPID {
|
||||
public static final double kP = 0.1;
|
||||
public static final double kI = 0.0;
|
||||
public static final double kD = 0.0;
|
||||
|
||||
}
|
||||
public static final double backboardSpeedMax = 60; //也就是output
|
||||
// 背板的上下限位角度
|
||||
public static final double backboardUpLimit = 3000.0; // 背板上限角度（根据实际需求设置）
|
||||
public static final double backboardDownLimit = 0.0; // 背板下限角度（根据实际需求设置）
|
||||
}
|
||||
public class ShooterCalculation{
|
||||
public static final double positionNear = 0; // 近距离背板角度
|
||||
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 static final double autoRotationForBumpTargetDegrees = 45;
|
||||
|
||||
public static final boolean teleopOffsetEnabled = true;
|
||||
public static final double teleopOffsetkP = 15;
|
||||
}
|
||||
|
||||
public class StartingPoints{
|
||||
public class Red{
|
||||
public static final Pose2d Point1 = new Pose2d(12.93, 7.47, edu.wpi.first.math.geometry.Rotation2d.fromDegrees(0));
|
||||
public static final Pose2d Point2 = new Pose2d(14.314, 4.15, edu.wpi.first.math.geometry.Rotation2d.fromDegrees(-180));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public enum DriveMode {
|
||||
ROBOT_CENTRIC,
|
||||
FIELD_CENTRIC,
|
||||
SYSID,
|
||||
OTHER
|
||||
}
|
||||
public class Pigeon {
|
||||
public static final int CANID = 0;
|
||||
}
|
||||
|
||||
public class Limelight {
|
||||
public static final String LIMELIGHT_NAME = "limelight-zy";
|
||||
}
|
||||
|
||||
public class Field {
|
||||
public static final double RedHubPositionX = 11.914;
|
||||
public static final double RedHubPositionY = 4.034;
|
||||
|
||||
}
|
||||
|
||||
public class Intaker {
|
||||
|
||||
public static final double intakeRotaterUpLimit = 0.0; // rotator上限角度
|
||||
public static final double intakeRotaterDownLimit = 0.0; // rotator下限角度
|
||||
|
||||
public static final Slot0Configs intakeGetterSlot0Configs = new Slot0Configs()
|
||||
.withKP(0.00).withKI(0.00).withKD(0.00)
|
||||
.withKV(0.00).withKA(0.00).withKS(0.00);
|
||||
// getter 如果你只用固定速度开关，也可以先不启闭环；若用闭环速度，这里 kP/kV/KS 等需要实测调参(这句fromAI)
|
||||
|
||||
public static final Slot0Configs intakeRotaterSlot0Configs = new Slot0Configs()
|
||||
.withKP(0.00).withKI(0.00).withKD(0.00)
|
||||
.withKV(0.00).withKA(0.00).withKS(0.00);
|
||||
//如果 rotater 受重力影响明显，可能需要用 withKG/withGravityType/withGravityArmPositionOffset（看机构形式）(这句fromAI)
|
||||
|
||||
public static final double IntakeGetterSpeed = 0.0;
|
||||
// 单位RPS
|
||||
public static final double positionToleranceRotations = 0.00;
|
||||
//rotater容差
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,15 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc.robot;
|
||||
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
|
||||
public final class Main {
|
||||
private Main() {}
|
||||
|
||||
public static void main(String... args) {
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,76 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc.robot;
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
|
||||
public class Robot extends TimedRobot {
|
||||
private Command m_autonomousCommand;
|
||||
|
||||
private final RobotContainer m_robotContainer;
|
||||
|
||||
public Robot() {
|
||||
m_robotContainer = new RobotContainer();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void robotPeriodic() {
|
||||
CommandScheduler.getInstance().run();
|
||||
m_robotContainer.tunerPeriodic();
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void disabledInit() {}
|
||||
|
||||
@Override
|
||||
public void disabledPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void disabledExit() {}
|
||||
|
||||
@Override
|
||||
public void autonomousInit() {
|
||||
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
|
||||
|
||||
if (m_autonomousCommand != null) {
|
||||
CommandScheduler.getInstance().schedule(m_autonomousCommand);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void autonomousPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void autonomousExit() {}
|
||||
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
if (m_autonomousCommand != null) {
|
||||
CommandScheduler.getInstance().cancel(m_autonomousCommand);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void teleopPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void teleopExit() {}
|
||||
|
||||
@Override
|
||||
public void testInit() {
|
||||
CommandScheduler.getInstance().cancelAll();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void testPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void testExit() {}
|
||||
|
||||
@Override
|
||||
public void simulationPeriodic() {}
|
||||
}
|
||||
@@ -0,0 +1,134 @@
|
||||
package frc.robot;
|
||||
|
||||
import com.ctre.phoenix6.SignalLogger;
|
||||
import com.pathplanner.lib.auto.AutoBuilder;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
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.controls.DriveControls;
|
||||
import frc.robot.controls.ShooterControls;
|
||||
import frc.robot.generated.TunerConstants;
|
||||
import frc.robot.subsystems.CommandSwerveDrivetrain;
|
||||
import frc.robot.subsystems.LEDSubsystem;
|
||||
import frc.robot.subsystems.ShooterSubsystem;
|
||||
import frc.robot.tuning.ConfigTalonFXMotorTuner;
|
||||
import frc.robot.tuning.ConfigTalonFXSMotorTuner;
|
||||
import frc.robot.tuning.DriveGainsTuner;
|
||||
import frc.robot.utils.LedBindings;
|
||||
|
||||
public class RobotContainer {
|
||||
|
||||
/* ====================== */
|
||||
/* 手柄 */
|
||||
/* ====================== */
|
||||
|
||||
// 只使用一个 Xbox 手柄（端口 0）
|
||||
private final CommandXboxController controller = new CommandXboxController(0);
|
||||
|
||||
/* ====================== */
|
||||
/* 子系统 */
|
||||
/* ====================== */
|
||||
|
||||
// Phoenix TunerX 生成的底盘
|
||||
public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
|
||||
public final ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
|
||||
|
||||
// LED 子系统
|
||||
private final LEDSubsystem leds = new LEDSubsystem(0, 72 ,0); // 总长度=72+72
|
||||
|
||||
/* ====================== */
|
||||
/* 控制封装/日志 */
|
||||
/* ====================== */
|
||||
|
||||
private final DriveControls driveControls = new DriveControls(drivetrain, controller);
|
||||
private final ShooterControls shooterControls = new ShooterControls(shooterSubsystem, controller);
|
||||
|
||||
private final DriveGainsTuner driveGainsTuner = new DriveGainsTuner(drivetrain);
|
||||
private ConfigTalonFXMotorTuner configFlywheelTuner = new ConfigTalonFXMotorTuner(shooterSubsystem.flywheelMotorLeft, "flywheel", Constants.Shooter.flyWheelSlot0Configs);
|
||||
private ConfigTalonFXMotorTuner configConveyorTuner = new ConfigTalonFXMotorTuner(shooterSubsystem.conveyorMotor, "conveyor", Constants.Shooter.conveyorSlot0Configs);
|
||||
private ConfigTalonFXSMotorTuner configBackboardTuner = new ConfigTalonFXSMotorTuner(shooterSubsystem.backboardMotor, "backboard", Constants.Shooter.backboardSlot0Configs);
|
||||
|
||||
private final SendableChooser<Command> autoChooser;
|
||||
|
||||
public RobotContainer() {
|
||||
SignalLogger.enableAutoLogging(false);
|
||||
autoChooser = AutoBuilder.buildAutoChooser();
|
||||
SmartDashboard.putData("Auto Chooser", autoChooser);
|
||||
configueSwerve();
|
||||
configueShooter();
|
||||
bingdingLED();
|
||||
}
|
||||
|
||||
/* ====================== */
|
||||
/* 底盘配置 */
|
||||
/* ====================== */
|
||||
|
||||
private void configueSwerve() {
|
||||
Pose2d startingPose = Constants.StartingPoints.Red.Point2;
|
||||
drivetrain.resetPose(startingPose);
|
||||
|
||||
// 默认驾驶命令
|
||||
drivetrain.setDefaultCommand(driveControls.defaultDriveCommand());
|
||||
|
||||
// Disabled 时进入 idle（防止模块乱动）
|
||||
RobotModeTriggers.disabled().whileTrue(driveControls.idleCommand());
|
||||
|
||||
|
||||
//左保险：按住刹车
|
||||
controller.leftBumper()
|
||||
.onTrue(Commands.runOnce(driveControls::emergencyStop))
|
||||
.whileTrue(driveControls.brakeWhileHeld());
|
||||
|
||||
// Back：重置场地坐标系
|
||||
controller.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric));
|
||||
|
||||
// Start：切换场地/车体坐标系
|
||||
controller.start().onTrue(Commands.runOnce(driveControls::toggleDriveFrame));
|
||||
|
||||
// 右保险：按住加速（Boost）
|
||||
controller.rightBumper()
|
||||
.onTrue(Commands.runOnce(() -> driveControls.setBoostEnabled(true)))
|
||||
.onFalse(Commands.runOnce(() -> driveControls.setBoostEnabled(false)));
|
||||
|
||||
controller.a().whileTrue(drivetrain.runOnce(drivetrain::forceUsingLimelightMT2));
|
||||
|
||||
}
|
||||
|
||||
private void configueShooter(){
|
||||
shooterSubsystem.setDefaultCommand(shooterControls.defaultShooterCommand());
|
||||
|
||||
controller.povLeft().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(-5)));
|
||||
controller.povRight().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(5)));
|
||||
controller.povUp().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(1500)));
|
||||
controller.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-1500)));
|
||||
|
||||
controller.b().onTrue(shooterControls.resetBackboardCommand());
|
||||
}
|
||||
|
||||
/* ====================== */
|
||||
/* LED 绑定 */
|
||||
/* ====================== */
|
||||
|
||||
private void bingdingLED() {
|
||||
LedBindings.bindModeIndicators(leds);
|
||||
}
|
||||
|
||||
public Command getAutonomousCommand() {
|
||||
|
||||
return autoChooser.getSelected();
|
||||
}
|
||||
|
||||
public void tunerPeriodic() {
|
||||
driveGainsTuner.periodic();
|
||||
configFlywheelTuner.periodic(() -> shooterSubsystem.applyLeftConfigurationToRight());
|
||||
configConveyorTuner.periodic(null);
|
||||
configBackboardTuner.periodic();
|
||||
shooterSubsystem.setRightFollowLeft();
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,243 @@
|
||||
package frc.robot.controls;
|
||||
import static edu.wpi.first.units.Units.MetersPerSecond;
|
||||
import static edu.wpi.first.units.Units.RotationsPerSecond;
|
||||
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
|
||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||
|
||||
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.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
import frc.robot.Constants;
|
||||
import frc.robot.Constants.DriveMode;
|
||||
import frc.robot.generated.TunerConstants;
|
||||
import frc.robot.subsystems.CommandSwerveDrivetrain;
|
||||
|
||||
public class DriveControls {
|
||||
private final CommandSwerveDrivetrain drivetrain;
|
||||
private final CommandXboxController driver;
|
||||
|
||||
private final double maxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
|
||||
private final double maxAngularRate = RotationsPerSecond.of(0.75).in(edu.wpi.first.units.Units.RadiansPerSecond);
|
||||
|
||||
// 默认速度比例（平时开车）
|
||||
private double normalDriveScale = 0.3;
|
||||
private double normalTurnScale = 0.7;
|
||||
|
||||
// 加速模式比例（按住 RB）
|
||||
private double boostDriveScale = 1.0;
|
||||
private double boostTurnScale = 1.0;
|
||||
|
||||
private boolean boostEnabled = false;
|
||||
|
||||
private final SlewRateLimiter vxLimiter = new SlewRateLimiter(5.0); // m/s^2 等效（调）
|
||||
private final SlewRateLimiter vyLimiter = new SlewRateLimiter(5.0);
|
||||
private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(9.0); // rad/s^2 等效（调）
|
||||
|
||||
// ====== 类成员变量里新增：记录上一次输出 ======
|
||||
private double lastVx = 0.0;
|
||||
private double lastVy = 0.0;
|
||||
private double lastOmega = 0.0;
|
||||
|
||||
private NetworkTable autoControlNetworkTable;
|
||||
|
||||
public void setBoostEnabled(boolean enabled) {
|
||||
boostEnabled = enabled;
|
||||
}
|
||||
|
||||
// 新增：当前是否使用场地坐标系（默认 true）
|
||||
private boolean fieldCentricEnabled = true;
|
||||
|
||||
private final SwerveRequest.FieldCentric fieldCentric =
|
||||
new SwerveRequest.FieldCentric()
|
||||
.withDeadband(maxSpeed * 0.05)
|
||||
.withRotationalDeadband(maxAngularRate * 0.05)
|
||||
.withDriveRequestType(DriveRequestType.Velocity);
|
||||
|
||||
private final SwerveRequest.RobotCentric robotCentric =
|
||||
new SwerveRequest.RobotCentric()
|
||||
.withDeadband(maxSpeed * 0.05)
|
||||
.withRotationalDeadband(maxAngularRate * 0.05)
|
||||
.withDriveRequestType(DriveRequestType.Velocity);
|
||||
|
||||
public DriveControls(CommandSwerveDrivetrain drivetrain, CommandXboxController driver) {
|
||||
this.drivetrain = drivetrain;
|
||||
this.driver = driver;
|
||||
autoControlNetworkTable = NetworkTableInstance.getDefault().getTable("AutoControl");
|
||||
}
|
||||
|
||||
public void toggleDriveFrame() {
|
||||
fieldCentricEnabled = !fieldCentricEnabled;
|
||||
}
|
||||
|
||||
public boolean isFieldCentricEnabled() {
|
||||
return fieldCentricEnabled;
|
||||
}
|
||||
public DriveMode getCurrentDriveMode() {
|
||||
return fieldCentricEnabled ? DriveMode.FIELD_CENTRIC : DriveMode.ROBOT_CENTRIC;
|
||||
}
|
||||
/** 底盘的默认驾驶命令（teleop 期间持续执行） */
|
||||
public Command defaultDriveCommand() {
|
||||
return drivetrain.applyRequest(() -> {
|
||||
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;
|
||||
|
||||
// ====== execute 内替换 limiter：只限加速、不限减速 ======
|
||||
double limitedVx = limitAccelOnly(vx, lastVx, vxLimiter);
|
||||
double limitedVy = limitAccelOnly(vy, lastVy, vyLimiter);
|
||||
double limitedOmega = limitAccelOnly(omega, lastOmega, omegaLimiter);
|
||||
|
||||
lastVx = limitedVx;
|
||||
lastVy = limitedVy;
|
||||
lastOmega = limitedOmega;
|
||||
|
||||
vx = limitedVx;
|
||||
vy = limitedVy;
|
||||
omega = 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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
if (fieldCentricEnabled) {
|
||||
return fieldCentric
|
||||
.withVelocityX(vx)
|
||||
.withVelocityY(vy)
|
||||
.withRotationalRate(omega);
|
||||
} else {
|
||||
return robotCentric
|
||||
.withVelocityX(vx)
|
||||
.withVelocityY(vy)
|
||||
.withRotationalRate(omega);
|
||||
}
|
||||
}, () -> fieldCentricEnabled ? DriveMode.FIELD_CENTRIC : DriveMode.ROBOT_CENTRIC);
|
||||
}
|
||||
|
||||
// Calculate distance and rotation (angle difference)
|
||||
public double[] calculateDistanceAndRotationToHub() {
|
||||
Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
|
||||
double targetX = Constants.Field.RedHubPositionX; // 目标 X 坐标
|
||||
double targetY = Constants.Field.RedHubPositionY; // 目标 Y 坐标
|
||||
|
||||
// 计算目标角度（相对于场地坐标系）
|
||||
double deltaX = targetX - currentPose.getX();
|
||||
double deltaY = targetY - currentPose.getY();
|
||||
double targetAngle = Math.toDegrees(Math.atan2(deltaY, deltaX)); // 转换为度
|
||||
|
||||
// 当前机器人角度
|
||||
double currentAngle = currentPose.getRotation().getDegrees();
|
||||
double angleDifference = targetAngle - currentAngle;
|
||||
// Normalize angle difference to the range [-180, 180]
|
||||
if (angleDifference > 180) {
|
||||
angleDifference -= 360;
|
||||
} else if (angleDifference < -180) {
|
||||
angleDifference += 360;
|
||||
}
|
||||
|
||||
// 计算距离
|
||||
double distance = Math.sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
// 计算距离和旋转角度
|
||||
double[] distanceAndRotation = new double[]{distance, angleDifference};
|
||||
|
||||
// 返回距离和角度差
|
||||
return distanceAndRotation;
|
||||
}
|
||||
|
||||
// Calculate rotation speed from rotation angle difference
|
||||
public double calculateRotationSpeedFromRotationAngle(double angleDifference, double autoRotationPower) {
|
||||
double calDifference = angleDifference;
|
||||
|
||||
// Optional: Apply teleop offset if enabled
|
||||
if (Constants.AutoRotation.teleopOffsetEnabled) {
|
||||
double teleopOffset = -driver.getRightX();
|
||||
calDifference += teleopOffset * Constants.AutoRotation.teleopOffsetkP;
|
||||
}
|
||||
|
||||
// If angle difference is within threshold, don't rotate
|
||||
if (Math.abs(calDifference) > 1.0) {
|
||||
return calDifference * autoRotationPower * Constants.AutoRotation.autoRotationToHubkP;
|
||||
} else {
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* 只限制“加速”（幅值变大），不限制“减速”（幅值变小）
|
||||
* 这样松杆/刹车会更及时，同时 boost 仍然能抑制电流冲击
|
||||
*/
|
||||
private double limitAccelOnly(double target, double last, SlewRateLimiter limiter) {
|
||||
// 如果正在减速（幅值变小），直接放行到目标值
|
||||
if (Math.abs(target) < Math.abs(last)) {
|
||||
limiter.reset(target); // 同步 limiter 内部状态，防止下一次突变
|
||||
return target;
|
||||
}
|
||||
// 否则是加速，使用 limiter
|
||||
return limiter.calculate(target);
|
||||
}
|
||||
|
||||
/** 紧急停止：清零限速器，让速度输出立刻归零 */
|
||||
public void emergencyStop() {
|
||||
vxLimiter.reset(0.0);
|
||||
vyLimiter.reset(0.0);
|
||||
omegaLimiter.reset(0.0);
|
||||
|
||||
lastVx = 0.0;
|
||||
lastVy = 0.0;
|
||||
lastOmega = 0.0;
|
||||
}
|
||||
|
||||
public Command idleCommand() {
|
||||
final var idle = new SwerveRequest.Idle();
|
||||
return drivetrain.applyRequest(() -> idle).ignoringDisable(true);
|
||||
}
|
||||
|
||||
public Command brakeWhileHeld() {
|
||||
final var brake = new SwerveRequest.SwerveDriveBrake();
|
||||
return drivetrain.applyRequest(() -> brake);
|
||||
}
|
||||
|
||||
|
||||
public SwerveRequest.SwerveDriveBrake brakeRequest() {
|
||||
return new SwerveRequest.SwerveDriveBrake();
|
||||
}
|
||||
|
||||
public SwerveRequest.Idle idleRequest() {
|
||||
return new SwerveRequest.Idle();
|
||||
}
|
||||
|
||||
public double getMaxSpeed() {
|
||||
return maxSpeed;
|
||||
}
|
||||
|
||||
public double getMaxAngularRate() {
|
||||
return maxAngularRate;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,71 @@
|
||||
package frc.robot.controls;
|
||||
|
||||
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.subsystems.IntakerSubsystem;
|
||||
|
||||
public class IntakerControls {
|
||||
private final IntakerSubsystem intakerSubsystem;
|
||||
private final CommandXboxController controller;
|
||||
|
||||
private static final double ROTATER_STEP_PER_CYCLE = 0.00; // 需要你按实际速度调：越大越快（fromAI）
|
||||
|
||||
public IntakerControls(IntakerSubsystem intakerSubsystem, CommandXboxController controller) {
|
||||
this.intakerSubsystem = intakerSubsystem;
|
||||
this.controller = controller;
|
||||
}
|
||||
|
||||
public void configureBindings() {
|
||||
|
||||
//B.A：按住下降 + getter转；松开后 rotater保持当前位置，getter继续转
|
||||
controller.a()
|
||||
.whileTrue(
|
||||
Commands.run(() -> {
|
||||
// rotater下降：范围会被 subsystem 限制
|
||||
double cur = intakerSubsystem.getIntakerotaterPosition();
|
||||
intakerSubsystem.setIntakerRotaterPosition(cur - ROTATER_STEP_PER_CYCLE);
|
||||
|
||||
// getter 开
|
||||
intakerSubsystem.setIntakeGetterOn(true);
|
||||
}, intakerSubsystem)
|
||||
)
|
||||
.onFalse(
|
||||
Commands.runOnce(() -> {
|
||||
// 松：把当前位置作为target保持
|
||||
double cur = intakerSubsystem.getIntakerotaterPosition();
|
||||
intakerSubsystem.setIntakerRotaterPosition(cur);
|
||||
|
||||
// gtter继续转
|
||||
intakerSubsystem.setIntakeGetterOn(true);
|
||||
}, intakerSubsystem)
|
||||
);
|
||||
|
||||
// === B.B：按住上升+getter停；松开后rotater保持当前位置
|
||||
controller.b()
|
||||
.whileTrue(
|
||||
Commands.run(() -> {
|
||||
// rotater升
|
||||
double cur = intakerSubsystem.getIntakerotaterPosition();
|
||||
intakerSubsystem.setIntakerRotaterPosition(cur + ROTATER_STEP_PER_CYCLE);
|
||||
|
||||
// getter关
|
||||
intakerSubsystem.setIntakeGetterOn(false);
|
||||
}, intakerSubsystem)
|
||||
)
|
||||
.onFalse(
|
||||
Commands.runOnce(() -> {
|
||||
// 松：保持当前位置
|
||||
double cur = intakerSubsystem.getIntakerotaterPosition();
|
||||
intakerSubsystem.setIntakerRotaterPosition(cur);
|
||||
|
||||
// getter仍然停止
|
||||
intakerSubsystem.setIntakeGetterOn(false);
|
||||
}, intakerSubsystem)
|
||||
);
|
||||
}
|
||||
|
||||
public Command defaultIntakerCommand() {
|
||||
return Commands.none();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,143 @@
|
||||
package frc.robot.controls;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
import frc.robot.Constants;
|
||||
import frc.robot.subsystems.ShooterSubsystem;
|
||||
import frc.robot.subsystems.ShooterSubsystem.Mode;
|
||||
|
||||
public class ShooterControls {
|
||||
private final ShooterSubsystem shooterSubsystem;
|
||||
private final CommandXboxController controller;
|
||||
|
||||
private double flywheelSpeedOffset = 0.0;
|
||||
private double backboardPositionOffset = 0.0;
|
||||
|
||||
public ShooterControls(ShooterSubsystem shooterSubsystem, CommandXboxController controller) {
|
||||
this.shooterSubsystem = shooterSubsystem;
|
||||
this.controller = controller;
|
||||
}
|
||||
|
||||
public Command defaultShooterCommand() {
|
||||
return Commands.run(() -> {
|
||||
// System.out.println("running");
|
||||
if (controller.getRightTriggerAxis() < 0.1) {
|
||||
shooterSubsystem.setConveyorSpeedByRPS(0);
|
||||
shooterSubsystem.setFlywheelSpeedByRPS(0);
|
||||
return;
|
||||
}
|
||||
double flywheelSpeed;
|
||||
double backboardPosition;
|
||||
double conveyorSpeed;
|
||||
flywheelSpeed = calculateFlywheelSpeedOnlywithOffset(20);
|
||||
backboardPosition = calculateBackboardPositionOnlywithOffset(100);
|
||||
conveyorSpeed = controller.getRightTriggerAxis() * Constants.Shooter.conveyorSpeedkP;
|
||||
|
||||
// double flywheelSpeed = calculateFlywheelSpeed(0);
|
||||
// double BackboardPosition = calculateBackboardPosition(0);
|
||||
// shooterSubsystem.setFlywheelSpeedByRPS(flywheelSpeed);
|
||||
// shooterSubsystem.setConveyorSpeedByRPS(conveyorSpeed);
|
||||
shooterSubsystem.setBackboardPosition(backboardPosition);
|
||||
}, shooterSubsystem);
|
||||
}
|
||||
|
||||
// public Command resetBackboardCommand(){
|
||||
// return
|
||||
// Commands.runOnce(()->{
|
||||
// shooterSubsystem.backboardMotor.set(-0.1);
|
||||
// shooterSubsystem.currentBackboardInalyzeMode = Mode.Inalyzing;}, shooterSubsystem)
|
||||
// .andThen(
|
||||
// Commands.waitSeconds(0.5))
|
||||
// .andThen(Commands.runOnce(()->{
|
||||
// shooterSubsystem.backboardMotor.set(0);
|
||||
// shooterSubsystem.backboardEncoder.reset();
|
||||
// shooterSubsystem.backboardPID.reset(0);}, shooterSubsystem))
|
||||
// .andThen(
|
||||
// Commands.waitSeconds(1))
|
||||
// .andThen(Commands.runOnce(()->{
|
||||
// shooterSubsystem.currentBackboardInalyzeMode = Mode.Inalyzed;}, shooterSubsystem));
|
||||
// }
|
||||
|
||||
public Command resetBackboardCommand(){
|
||||
return
|
||||
Commands.runOnce(()->{
|
||||
shooterSubsystem.backboardEncoder.reset();
|
||||
shooterSubsystem.backboardPID.reset(0);}, shooterSubsystem);
|
||||
}
|
||||
|
||||
//打表参数
|
||||
private static final double[][] FLYWHEEL_TABLE = {
|
||||
{1.5, 0.42},
|
||||
{2.0, 0.46},
|
||||
{2.5, 0.51},
|
||||
{3.0, 0.57},
|
||||
{3.5, 0.64},
|
||||
{4.0, 0.72}
|
||||
};
|
||||
|
||||
//线性插值函数
|
||||
private static double lerp(double x0, double y0, double x1, double y1, double x) {
|
||||
if (Math.abs(x1 - x0) < 1e-9) {
|
||||
return y0;
|
||||
}
|
||||
double t = (x - x0) / (x1 - x0);
|
||||
return y0 + t * (y1 - y0);
|
||||
}
|
||||
private double calculateFlywheelSpeedOnlywithOffset(double defaultSpeed) {
|
||||
return defaultSpeed + flywheelSpeedOffset;
|
||||
}
|
||||
private double calculateBackboardPositionOnlywithOffset(double defaultPosition) {
|
||||
return defaultPosition + backboardPositionOffset;
|
||||
}
|
||||
|
||||
private double calculateFlywheelSpeed(double distance) {
|
||||
// 小于最小距离：用第一个点
|
||||
if (distance <= FLYWHEEL_TABLE[0][0]) {
|
||||
return FLYWHEEL_TABLE[0][1] + flywheelSpeedOffset;
|
||||
}
|
||||
// 大于最大距离：用最后一个点
|
||||
int last = FLYWHEEL_TABLE.length - 1;
|
||||
if (distance >= FLYWHEEL_TABLE[last][0]) {
|
||||
return FLYWHEEL_TABLE[last][1] + flywheelSpeedOffset;
|
||||
}
|
||||
// 查找区间并插值
|
||||
for (int i = 0; i < FLYWHEEL_TABLE.length - 1; i++) {
|
||||
double d0 = FLYWHEEL_TABLE[i][0];
|
||||
double s0 = FLYWHEEL_TABLE[i][1];
|
||||
double d1 = FLYWHEEL_TABLE[i + 1][0];
|
||||
double s1 = FLYWHEEL_TABLE[i + 1][1];
|
||||
if (distance >= d0 && distance <= d1) {
|
||||
return lerp(d0, s0, d1, s1, distance) + flywheelSpeedOffset;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
private double calculateBackboardPosition(double distance) {
|
||||
double BackboardPosition;
|
||||
// 距离阈值（单位自己按distance进行修改）
|
||||
final double DISTANCE_NEAR = 2.0; // 近距离阈值
|
||||
final double DISTANCE_MID = 4.0; // 中距离阈值
|
||||
if (distance < DISTANCE_NEAR) {
|
||||
BackboardPosition = Constants.ShooterCalculation.positionNear;
|
||||
}
|
||||
else if (distance < DISTANCE_MID) {
|
||||
BackboardPosition = Constants.ShooterCalculation.positionMid;
|
||||
}
|
||||
else {
|
||||
BackboardPosition = Constants.ShooterCalculation.postionFar;
|
||||
}
|
||||
BackboardPosition += backboardPositionOffset;
|
||||
return BackboardPosition;
|
||||
}
|
||||
|
||||
public void adjustFlywheelSpeedOffset(double offset) {
|
||||
flywheelSpeedOffset += offset;
|
||||
}
|
||||
|
||||
public void adjustBackboardRateOffset(double offset) {
|
||||
backboardPositionOffset += offset;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,286 @@
|
||||
package frc.robot.generated;
|
||||
|
||||
import static edu.wpi.first.units.Units.*;
|
||||
|
||||
import com.ctre.phoenix6.CANBus;
|
||||
import com.ctre.phoenix6.configs.*;
|
||||
import com.ctre.phoenix6.hardware.*;
|
||||
import com.ctre.phoenix6.signals.*;
|
||||
import com.ctre.phoenix6.swerve.*;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstants.*;
|
||||
|
||||
import edu.wpi.first.math.Matrix;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
import edu.wpi.first.math.numbers.N3;
|
||||
import edu.wpi.first.units.measure.*;
|
||||
|
||||
import frc.robot.subsystems.CommandSwerveDrivetrain;
|
||||
|
||||
// Generated by the 2026 Tuner X Swerve Project Generator
|
||||
// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
|
||||
public class TunerConstants {
|
||||
// Both sets of gains need to be tuned to your individual robot.
|
||||
|
||||
// The steer motor uses any SwerveModule.SteerRequestType control request with the
|
||||
// output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
|
||||
private static final Slot0Configs steerGains = new Slot0Configs()
|
||||
.withKP(70).withKI(0).withKD(0.5)
|
||||
.withKS(0.1).withKV(1.59).withKA(0)
|
||||
.withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
|
||||
// When using closed-loop control, the drive motor uses the control
|
||||
// output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
|
||||
private static final Slot0Configs driveGains = new Slot0Configs()
|
||||
.withKP(0.01).withKI(0).withKD(0)
|
||||
.withKS(0.05).withKV(0.117);
|
||||
|
||||
// The closed-loop output type to use for the steer motors;
|
||||
// This affects the PID/FF gains for the steer motors
|
||||
private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage;
|
||||
// The closed-loop output type to use for the drive motors;
|
||||
// This affects the PID/FF gains for the drive motors
|
||||
private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage;
|
||||
|
||||
// The type of motor used for the drive motor
|
||||
private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated;
|
||||
// The type of motor used for the drive motor
|
||||
private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated;
|
||||
|
||||
// The remote sensor feedback type to use for the steer motors;
|
||||
// When not Pro-licensed, Fused*/Sync* automatically fall back to Remote*
|
||||
private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder;
|
||||
|
||||
// The stator current at which the wheels start to slip;
|
||||
// This needs to be tuned to your individual robot
|
||||
private static final Current kSlipCurrent = Amps.of(120);
|
||||
|
||||
// Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
|
||||
// Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.
|
||||
private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
|
||||
private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration()
|
||||
.withCurrentLimits(
|
||||
new CurrentLimitsConfigs()
|
||||
// Swerve azimuth does not require much torque output, so we can set a relatively low
|
||||
// stator current limit to help avoid brownouts without impacting performance.
|
||||
.withStatorCurrentLimit(Amps.of(60))
|
||||
.withStatorCurrentLimitEnable(true)
|
||||
);
|
||||
private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
|
||||
// Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
|
||||
private static final Pigeon2Configuration pigeonConfigs = null;
|
||||
|
||||
// CAN bus that the devices are located on;
|
||||
// All swerve devices must share the same CAN bus
|
||||
public static final CANBus kCANBus = new CANBus("", "./logs/example.hoot");
|
||||
|
||||
// Theoretical free speed (m/s) at 12 V applied output;
|
||||
// This needs to be tuned to your individual robot
|
||||
public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.73);
|
||||
|
||||
// Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
|
||||
// This may need to be tuned to your individual robot
|
||||
private static final double kCoupleRatio = 3.125;
|
||||
|
||||
private static final double kDriveGearRatio = 5.902777777777778;
|
||||
private static final double kSteerGearRatio = 12.8;
|
||||
private static final Distance kWheelRadius = Inches.of(1.81);
|
||||
|
||||
private static final boolean kInvertLeftSide = false;
|
||||
private static final boolean kInvertRightSide = true;
|
||||
|
||||
private static final int kPigeonId = 0;
|
||||
|
||||
// These are only used for simulation
|
||||
private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01);
|
||||
private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01);
|
||||
// Simulated voltage necessary to overcome friction
|
||||
private static final Voltage kSteerFrictionVoltage = Volts.of(0.2);
|
||||
private static final Voltage kDriveFrictionVoltage = Volts.of(0.2);
|
||||
|
||||
public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants()
|
||||
.withCANBusName(kCANBus.getName())
|
||||
.withPigeon2Id(kPigeonId)
|
||||
.withPigeon2Configs(pigeonConfigs);
|
||||
|
||||
private static final SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> ConstantCreator =
|
||||
new SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>()
|
||||
.withDriveMotorGearRatio(kDriveGearRatio)
|
||||
.withSteerMotorGearRatio(kSteerGearRatio)
|
||||
.withCouplingGearRatio(kCoupleRatio)
|
||||
.withWheelRadius(kWheelRadius)
|
||||
.withSteerMotorGains(steerGains)
|
||||
.withDriveMotorGains(driveGains)
|
||||
.withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput)
|
||||
.withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput)
|
||||
.withSlipCurrent(kSlipCurrent)
|
||||
.withSpeedAt12Volts(kSpeedAt12Volts)
|
||||
.withDriveMotorType(kDriveMotorType)
|
||||
.withSteerMotorType(kSteerMotorType)
|
||||
.withFeedbackSource(kSteerFeedbackType)
|
||||
.withDriveMotorInitialConfigs(driveInitialConfigs)
|
||||
.withSteerMotorInitialConfigs(steerInitialConfigs)
|
||||
.withEncoderInitialConfigs(encoderInitialConfigs)
|
||||
.withSteerInertia(kSteerInertia)
|
||||
.withDriveInertia(kDriveInertia)
|
||||
.withSteerFrictionVoltage(kSteerFrictionVoltage)
|
||||
.withDriveFrictionVoltage(kDriveFrictionVoltage);
|
||||
|
||||
|
||||
// Front Left
|
||||
private static final int kFrontLeftDriveMotorId = 11;
|
||||
private static final int kFrontLeftSteerMotorId = 12;
|
||||
private static final int kFrontLeftEncoderId = 13;
|
||||
private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.412841796875);
|
||||
private static final boolean kFrontLeftSteerMotorInverted = false;
|
||||
private static final boolean kFrontLeftEncoderInverted = false;
|
||||
|
||||
private static final Distance kFrontLeftXPos = Inches.of(11.375);
|
||||
private static final Distance kFrontLeftYPos = Inches.of(11.375);
|
||||
|
||||
// Front Right
|
||||
private static final int kFrontRightDriveMotorId = 21;
|
||||
private static final int kFrontRightSteerMotorId = 22;
|
||||
private static final int kFrontRightEncoderId = 23;
|
||||
private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.012939453125);
|
||||
private static final boolean kFrontRightSteerMotorInverted = false;
|
||||
private static final boolean kFrontRightEncoderInverted = false;
|
||||
|
||||
private static final Distance kFrontRightXPos = Inches.of(11.375);
|
||||
private static final Distance kFrontRightYPos = Inches.of(-11.375);
|
||||
|
||||
// Back Left
|
||||
private static final int kBackLeftDriveMotorId = 1;
|
||||
private static final int kBackLeftSteerMotorId = 2;
|
||||
private static final int kBackLeftEncoderId = 3;
|
||||
private static final Angle kBackLeftEncoderOffset = Rotations.of(0.07275390625);
|
||||
private static final boolean kBackLeftSteerMotorInverted = false;
|
||||
private static final boolean kBackLeftEncoderInverted = false;
|
||||
|
||||
private static final Distance kBackLeftXPos = Inches.of(-11.375);
|
||||
private static final Distance kBackLeftYPos = Inches.of(11.375);
|
||||
|
||||
// Back Right
|
||||
private static final int kBackRightDriveMotorId = 31;
|
||||
private static final int kBackRightSteerMotorId = 32;
|
||||
private static final int kBackRightEncoderId = 33;
|
||||
private static final Angle kBackRightEncoderOffset = Rotations.of(-0.4892578125);
|
||||
private static final boolean kBackRightSteerMotorInverted = false;
|
||||
private static final boolean kBackRightEncoderInverted = false;
|
||||
|
||||
private static final Distance kBackRightXPos = Inches.of(-11.375);
|
||||
private static final Distance kBackRightYPos = Inches.of(-11.375);
|
||||
|
||||
|
||||
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontLeft =
|
||||
ConstantCreator.createModuleConstants(
|
||||
kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset,
|
||||
kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted
|
||||
);
|
||||
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontRight =
|
||||
ConstantCreator.createModuleConstants(
|
||||
kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset,
|
||||
kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted
|
||||
);
|
||||
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackLeft =
|
||||
ConstantCreator.createModuleConstants(
|
||||
kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset,
|
||||
kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted
|
||||
);
|
||||
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackRight =
|
||||
ConstantCreator.createModuleConstants(
|
||||
kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset,
|
||||
kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted
|
||||
);
|
||||
|
||||
/**
|
||||
* Creates a CommandSwerveDrivetrain instance.
|
||||
* This should only be called once in your robot program,.
|
||||
*/
|
||||
public static CommandSwerveDrivetrain createDrivetrain() {
|
||||
return new CommandSwerveDrivetrain(
|
||||
DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight
|
||||
);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types.
|
||||
*/
|
||||
public static class TunerSwerveDrivetrain extends SwerveDrivetrain<TalonFX, TalonFX, CANcoder> {
|
||||
/**
|
||||
* Constructs a CTRE SwerveDrivetrain using the specified constants.
|
||||
* <p>
|
||||
* This constructs the underlying hardware devices, so users should not construct
|
||||
* the devices themselves. If they need the devices, they can access them through
|
||||
* getters in the classes.
|
||||
*
|
||||
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
|
||||
* @param modules Constants for each specific module
|
||||
*/
|
||||
public TunerSwerveDrivetrain(
|
||||
SwerveDrivetrainConstants drivetrainConstants,
|
||||
SwerveModuleConstants<?, ?, ?>... modules
|
||||
) {
|
||||
super(
|
||||
TalonFX::new, TalonFX::new, CANcoder::new,
|
||||
drivetrainConstants, modules
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a CTRE SwerveDrivetrain using the specified constants.
|
||||
* <p>
|
||||
* This constructs the underlying hardware devices, so users should not construct
|
||||
* the devices themselves. If they need the devices, they can access them through
|
||||
* getters in the classes.
|
||||
*
|
||||
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
|
||||
* @param odometryUpdateFrequency The frequency to run the odometry loop. If
|
||||
* unspecified or set to 0 Hz, this is 250 Hz on
|
||||
* CAN FD, and 100 Hz on CAN 2.0.
|
||||
* @param modules Constants for each specific module
|
||||
*/
|
||||
public TunerSwerveDrivetrain(
|
||||
SwerveDrivetrainConstants drivetrainConstants,
|
||||
double odometryUpdateFrequency,
|
||||
SwerveModuleConstants<?, ?, ?>... modules
|
||||
) {
|
||||
super(
|
||||
TalonFX::new, TalonFX::new, CANcoder::new,
|
||||
drivetrainConstants, odometryUpdateFrequency, modules
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a CTRE SwerveDrivetrain using the specified constants.
|
||||
* <p>
|
||||
* This constructs the underlying hardware devices, so users should not construct
|
||||
* the devices themselves. If they need the devices, they can access them through
|
||||
* getters in the classes.
|
||||
*
|
||||
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
|
||||
* @param odometryUpdateFrequency The frequency to run the odometry loop. If
|
||||
* unspecified or set to 0 Hz, this is 250 Hz on
|
||||
* CAN FD, and 100 Hz on CAN 2.0.
|
||||
* @param odometryStandardDeviation The standard deviation for odometry calculation
|
||||
* in the form [x, y, theta]ᵀ, with units in meters
|
||||
* and radians
|
||||
* @param visionStandardDeviation The standard deviation for vision calculation
|
||||
* in the form [x, y, theta]ᵀ, with units in meters
|
||||
* and radians
|
||||
* @param modules Constants for each specific module
|
||||
*/
|
||||
public TunerSwerveDrivetrain(
|
||||
SwerveDrivetrainConstants drivetrainConstants,
|
||||
double odometryUpdateFrequency,
|
||||
Matrix<N3, N1> odometryStandardDeviation,
|
||||
Matrix<N3, N1> visionStandardDeviation,
|
||||
SwerveModuleConstants<?, ?, ?>... modules
|
||||
) {
|
||||
super(
|
||||
TalonFX::new, TalonFX::new, CANcoder::new,
|
||||
drivetrainConstants, odometryUpdateFrequency,
|
||||
odometryStandardDeviation, visionStandardDeviation, modules
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,281 @@
|
||||
package frc.robot.subsystems;
|
||||
import java.util.Optional;
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import com.ctre.phoenix6.Utils;
|
||||
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstants;
|
||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||
import com.pathplanner.lib.auto.AutoBuilder;
|
||||
import com.pathplanner.lib.config.PIDConstants;
|
||||
import com.pathplanner.lib.config.RobotConfig;
|
||||
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
|
||||
|
||||
import edu.wpi.first.math.Matrix;
|
||||
import edu.wpi.first.math.VecBuilder;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
import edu.wpi.first.math.numbers.N3;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Subsystem;
|
||||
import frc.robot.Constants.DriveMode;
|
||||
import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain;
|
||||
import frc.robot.Constants;
|
||||
|
||||
import edu.wpi.first.networktables.*;
|
||||
|
||||
|
||||
/**
|
||||
* Class that extends the Phoenix 6 SwerveDrivetrain class and implements
|
||||
* Subsystem so it can easily be used in command-based projects.
|
||||
*
|
||||
* Generated by the 2026 Tuner X Swerve Project Generator
|
||||
* https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
|
||||
*/
|
||||
public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem {
|
||||
|
||||
/* 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;
|
||||
/* Keep track if we've ever applied the operator perspective before or not */
|
||||
private boolean m_hasAppliedOperatorPerspective = false;
|
||||
|
||||
private DriveMode m_driveMode = DriveMode.OTHER;
|
||||
|
||||
private NetworkTable positioningNetworkTable;
|
||||
private NetworkTable driveNetworkTable;
|
||||
|
||||
private final StructPublisher<Pose2d> posePub =
|
||||
NetworkTableInstance.getDefault()
|
||||
.getStructTopic("/Odometry/RobotPose", Pose2d.struct)
|
||||
.publish();
|
||||
|
||||
private Field2d m_field;
|
||||
public CommandSwerveDrivetrain(
|
||||
SwerveDrivetrainConstants drivetrainConstants,
|
||||
SwerveModuleConstants<?, ?, ?>... modules
|
||||
) {
|
||||
super(drivetrainConstants, modules);
|
||||
m_field = new Field2d();
|
||||
SmartDashboard.putData("Field", m_field);
|
||||
LimelightHelpers.SetIMUMode(Constants.Limelight.LIMELIGHT_NAME, 0);
|
||||
positioningNetworkTable = NetworkTableInstance.getDefault().getTable("Positioning");
|
||||
driveNetworkTable = NetworkTableInstance.getDefault().getTable("Drive");
|
||||
|
||||
configueAutoBuilder();
|
||||
|
||||
}
|
||||
|
||||
|
||||
private void configueAutoBuilder(){
|
||||
|
||||
RobotConfig config;
|
||||
try {
|
||||
config = RobotConfig.fromGUISettings();
|
||||
} catch (Exception e) {
|
||||
throw new RuntimeException("Failed to load PathPlanner RobotConfig", e);
|
||||
}
|
||||
|
||||
AutoBuilder.configure(
|
||||
this::getPose, // 当前机器人 Pose
|
||||
this::resetPose, // 重置里程计
|
||||
this::getRobotRelativeSpeeds, // ROBOT RELATIVE speeds
|
||||
(speeds, feedforwards) -> driveRobotRelative(speeds), // 用 speeds 驱动机器人
|
||||
new PPHolonomicDriveController(
|
||||
new PIDConstants(5.0, 0.0, 0.0), // X/Y PID
|
||||
new PIDConstants(5.0, 0.0, 0.0) // Theta PID
|
||||
),
|
||||
config,
|
||||
() -> false,
|
||||
this
|
||||
);
|
||||
|
||||
}
|
||||
public Pose2d getPose() {
|
||||
return getState().Pose;
|
||||
}
|
||||
|
||||
public ChassisSpeeds getRobotRelativeSpeeds() {
|
||||
return getState().Speeds;
|
||||
}
|
||||
|
||||
public void driveRobotRelative(ChassisSpeeds speeds) {
|
||||
ChassisSpeeds scaled = new ChassisSpeeds(
|
||||
speeds.vxMetersPerSecond, // X 不变
|
||||
speeds.vyMetersPerSecond, // Y 不变
|
||||
speeds.omegaRadiansPerSecond * 1.3 // 只放大旋转
|
||||
);
|
||||
|
||||
setControl(
|
||||
new SwerveRequest.ApplyRobotSpeeds().withSpeeds(scaled)
|
||||
);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
private static final double kMaxVisionJumpMeters = 1.0;
|
||||
private static final double kMaxOmegaDegPerSec = 360.0;
|
||||
|
||||
/**
|
||||
* Returns a command that applies the specified control request to this swerve drivetrain.
|
||||
*
|
||||
* @param request Function returning the request to apply
|
||||
* @return Command to run
|
||||
*/
|
||||
public Command applyRequest(Supplier<SwerveRequest> request) {
|
||||
return run(() -> this.setControl(request.get()));
|
||||
}
|
||||
|
||||
public Command applyRequest(Supplier<SwerveRequest> requestSupplier, Supplier<DriveMode> driveModeSupplier) {
|
||||
return run(() -> {
|
||||
DriveMode mode = driveModeSupplier.get();
|
||||
m_driveMode = mode;
|
||||
this.setControl(requestSupplier.get());
|
||||
});
|
||||
}
|
||||
|
||||
public void forceUsingLimelightMT2() {
|
||||
LimelightHelpers.PoseEstimate mt2 =
|
||||
LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME);
|
||||
|
||||
if (mt2 == null || mt2.tagCount == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
var stdDevs = VecBuilder.fill(0.7, 0.7, 9999999);
|
||||
addVisionMeasurement(mt2.pose, mt2.timestampSeconds, stdDevs);
|
||||
}
|
||||
|
||||
private void fuseLimelightMT2() {
|
||||
|
||||
LimelightHelpers.PoseEstimate mt2 =
|
||||
LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME);
|
||||
|
||||
Pose2d current = getState().Pose;
|
||||
Pose2d mt2Pose = mt2.pose;
|
||||
|
||||
double distance = current.getTranslation().getDistance(mt2Pose.getTranslation());
|
||||
Rotation2d deltaRotation = current.getRotation().minus(mt2Pose.getRotation());
|
||||
|
||||
positioningNetworkTable.getEntry("CurrentPoseX").setDouble(current.getX());
|
||||
positioningNetworkTable.getEntry("CurrentPoseY").setDouble(current.getY());
|
||||
positioningNetworkTable.getEntry("MetaTag2PoseX").setDouble(mt2Pose.getX());
|
||||
positioningNetworkTable.getEntry("MetaTag2PoseY").setDouble(mt2Pose.getY());
|
||||
positioningNetworkTable.getEntry("PoseDistance").setDouble(distance);
|
||||
positioningNetworkTable.getEntry("RotationDifference").setDouble(Math.abs(deltaRotation.getDegrees()));
|
||||
|
||||
// 1) 没看到 tag：拒绝
|
||||
if (mt2 == null || mt2.tagCount == 0) {
|
||||
positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(false);
|
||||
return;
|
||||
}
|
||||
// 2) 转太快：拒绝（角速度你可以从 Pigeon2 或 drivetrain state 拿）
|
||||
double omegaDegPerSec = getPigeon2().getAngularVelocityZWorld().getValueAsDouble(); // 若你有这个信号
|
||||
if (Math.abs(omegaDegPerSec) > kMaxOmegaDegPerSec) {
|
||||
positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(false);
|
||||
System.out.println("NoUsingMetaTag2ForTooFastRotationSpeed");
|
||||
return;
|
||||
}
|
||||
|
||||
// 3) 跳变太大：拒绝（Phoenix6 推荐的稳定策略）
|
||||
//Pose2d current = getState().Pose;
|
||||
if (current.getTranslation().getDistance(mt2.pose.getTranslation()) > kMaxVisionJumpMeters) {
|
||||
positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(false);
|
||||
System.out.println("NoUsingMetaTag2ForTooChangingPose");
|
||||
return;
|
||||
}
|
||||
|
||||
// 4) 设置测量噪声（示例：theta 基本不信任就给极大）
|
||||
// 你也可以根据 mt2.avgTagDist / mt2.tagSpan 动态调
|
||||
var stdDevs = VecBuilder.fill(0.7, 0.7, 9999999);
|
||||
|
||||
// 5) 融合：注意 timestampSeconds 的处理
|
||||
addVisionMeasurement(mt2.pose, mt2.timestampSeconds, stdDevs);
|
||||
positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(true);
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
Pose2d pose = getState().Pose;
|
||||
m_field.setRobotPose(pose);
|
||||
posePub.set(pose);
|
||||
driveNetworkTable.getEntry("YawFromState").setDouble(getState().Pose.getRotation().getDegrees());
|
||||
driveNetworkTable.getEntry("DriveMode").setString(m_driveMode.name());
|
||||
driveNetworkTable.getEntry("IsFieldCentric").setBoolean(m_driveMode == DriveMode.FIELD_CENTRIC);
|
||||
|
||||
// 1) 每帧喂给 LL：机器人当前 yaw（度），其余先全 0
|
||||
LimelightHelpers.SetRobotOrientation(
|
||||
Constants.Limelight.LIMELIGHT_NAME,
|
||||
pose.getRotation().getDegrees(), // 关键：要符合 wpiBlue 约定
|
||||
0, 0, 0, 0, 0
|
||||
);
|
||||
|
||||
fuseLimelightMT2();
|
||||
|
||||
if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
|
||||
DriverStation.getAlliance().ifPresent(allianceColor -> {
|
||||
setOperatorPerspectiveForward(
|
||||
allianceColor == Alliance.Red
|
||||
? kRedAlliancePerspectiveRotation
|
||||
: kBlueAlliancePerspectiveRotation
|
||||
);
|
||||
m_hasAppliedOperatorPerspective = true;
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
|
||||
* while still accounting for measurement noise.
|
||||
*
|
||||
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
|
||||
* @param timestampSeconds The timestamp of the vision measurement in seconds.
|
||||
*/
|
||||
@Override
|
||||
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
|
||||
super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds));
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
|
||||
* while still accounting for measurement noise.
|
||||
* <p>
|
||||
* Note that the vision measurement standard deviations passed into this method
|
||||
* will continue to apply to future measurements until a subsequent call to
|
||||
* {@link #setVisionMeasurementStdDevs(Matrix)} or this method.
|
||||
*
|
||||
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
|
||||
* @param timestampSeconds The timestamp of the vision measurement in seconds.
|
||||
* @param visionMeasurementStdDevs Standard deviations of the vision pose measurement
|
||||
* in the form [x, y, theta]ᵀ, with units in meters and radians.
|
||||
*/
|
||||
@Override
|
||||
public void addVisionMeasurement(
|
||||
Pose2d visionRobotPoseMeters,
|
||||
double timestampSeconds,
|
||||
Matrix<N3, N1> visionMeasurementStdDevs
|
||||
) {
|
||||
super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs);
|
||||
}
|
||||
|
||||
/**
|
||||
* Return the pose at a given timestamp, if the buffer is not empty.
|
||||
*
|
||||
* @param timestampSeconds The timestamp of the pose in seconds.
|
||||
* @return The pose at the given timestamp (or Optional.empty() if the buffer is empty).
|
||||
*/
|
||||
@Override
|
||||
public Optional<Pose2d> samplePoseAt(double timestampSeconds) {
|
||||
return super.samplePoseAt(Utils.fpgaToCurrentTime(timestampSeconds));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,67 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import com.ctre.phoenix6.controls.PositionVoltage;
|
||||
import com.ctre.phoenix6.controls.VelocityVoltage;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.Constants;
|
||||
|
||||
public class IntakerSubsystem extends SubsystemBase{
|
||||
//定义intakerotater&intakegetter
|
||||
private TalonFX intakerotater;
|
||||
private TalonFX intakegetter;
|
||||
|
||||
//PID
|
||||
private final VelocityVoltage velocityRequest = new VelocityVoltage(0);
|
||||
private final PositionVoltage positionRequest = new PositionVoltage(0);
|
||||
|
||||
private double intakeRotaterTargetPosition = 0.0;
|
||||
|
||||
public IntakerSubsystem() {
|
||||
//初始化rotater&getter
|
||||
intakegetter = new TalonFX(47);
|
||||
intakerotater = new TalonFX(48);
|
||||
intakegetter.getConfigurator().apply(Constants.Intaker.intakeGetterSlot0Configs);
|
||||
intakerotater.getConfigurator().apply(Constants.Intaker.intakeRotaterSlot0Configs);
|
||||
}
|
||||
|
||||
public void setIntakerRotaterPosition(double targetPosition) {
|
||||
//目标position
|
||||
if (targetPosition > Constants.Intaker.intakeRotaterUpLimit) {
|
||||
targetPosition = Constants.Intaker.intakeRotaterUpLimit; // 限制最大上限
|
||||
} else if (targetPosition < Constants.Intaker.intakeRotaterDownLimit) {
|
||||
targetPosition = Constants.Intaker.intakeRotaterDownLimit; // 限制最小下限
|
||||
}
|
||||
|
||||
intakeRotaterTargetPosition = targetPosition;
|
||||
|
||||
intakerotater.setControl(positionRequest.withPosition(targetPosition)); // 设置rotater位置（TalonFX 内部闭环 PID）
|
||||
}
|
||||
|
||||
// 获取intakerotater当前角度
|
||||
public double getIntakerotaterPosition() {
|
||||
return intakerotater.getPosition().getValueAsDouble();
|
||||
}
|
||||
|
||||
public boolean isIntakerotaterAtTarget() {
|
||||
// 到位判断容差
|
||||
double error = Math.abs(intakeRotaterTargetPosition - getIntakerotaterPosition());
|
||||
return error <= Constants.Intaker.positionToleranceRotations;
|
||||
}
|
||||
|
||||
public void stopintakerotater() {
|
||||
intakerotater.setControl(velocityRequest.withVelocity(0));
|
||||
// 如果你希望停止后保持当前位置，建议改用中性模式/或维持 positionRequest（这句fromAI）
|
||||
}
|
||||
|
||||
//intakegetter的on/off
|
||||
public void setIntakeGetterOn(boolean on) {
|
||||
if (on) {
|
||||
intakegetter.setControl(velocityRequest.withVelocity(Constants.Intaker.IntakeGetterSpeed));
|
||||
}
|
||||
else {
|
||||
intakegetter.setControl(velocityRequest.withVelocity(0));
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,151 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.wpilibj.AddressableLED;
|
||||
import edu.wpi.first.wpilibj.AddressableLEDBuffer;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.Constants;
|
||||
|
||||
public class LEDSubsystem extends SubsystemBase {
|
||||
|
||||
public enum Mode {
|
||||
OFF,
|
||||
SOLID,
|
||||
BREATH,
|
||||
RAINBOW
|
||||
}
|
||||
|
||||
private final AddressableLED led;
|
||||
private final AddressableLEDBuffer buffer;
|
||||
private final Timer timer = new Timer();
|
||||
|
||||
// 左右灯带分段（左 0~71，右 72~143）
|
||||
private final int leftStart = 0;
|
||||
private final int leftLen;
|
||||
private final int rightStart;
|
||||
private final int rightLen;
|
||||
|
||||
private Mode leftMode = Mode.OFF;
|
||||
private Mode rightMode = Mode.OFF;
|
||||
|
||||
// 左右颜色
|
||||
private int lr = 0, lg = 0, lb = 0;
|
||||
private int rr = 0, rg = 0, rb = 0;
|
||||
|
||||
// 动画参数
|
||||
private double leftBreathPeriodSec = 2.0;
|
||||
private double rightBreathPeriodSec = 2.0;
|
||||
private int leftRainbowOffset = 0;
|
||||
private int rightRainbowOffset = 0;
|
||||
|
||||
/**
|
||||
* @param pwmPort PWM 端口（只能用一个！）
|
||||
* @param leftCount 左灯数量
|
||||
* @param rightCount 右灯数量
|
||||
*/
|
||||
public LEDSubsystem(int pwmPort, int leftCount, int rightCount) {
|
||||
this.leftLen = leftCount;
|
||||
this.rightLen = rightCount;
|
||||
this.rightStart = leftStart + leftLen;
|
||||
|
||||
led = new AddressableLED(pwmPort);
|
||||
buffer = new AddressableLEDBuffer(leftLen + rightLen);
|
||||
|
||||
led.setLength(buffer.getLength());
|
||||
led.start();
|
||||
|
||||
timer.start();
|
||||
}
|
||||
|
||||
/* ====================== */
|
||||
/* 对外API */
|
||||
/* ====================== */
|
||||
|
||||
public void offLeft() { leftMode = Mode.OFF; }
|
||||
public void offRight() { rightMode = Mode.OFF; }
|
||||
public void offAll() { leftMode = Mode.OFF; rightMode = Mode.OFF; }
|
||||
|
||||
public void solidLeft(int r, int g, int b) {
|
||||
lr = clamp8(r); lg = clamp8(g); lb = clamp8(b);
|
||||
leftMode = Mode.SOLID;
|
||||
}
|
||||
|
||||
public void solidRight(int r, int g, int b) {
|
||||
rr = clamp8(r); rg = clamp8(g); rb = clamp8(b);
|
||||
rightMode = Mode.SOLID;
|
||||
}
|
||||
|
||||
public void breathLeft(int r, int g, int b, double periodSeconds) {
|
||||
lr = clamp8(r); lg = clamp8(g); lb = clamp8(b);
|
||||
leftBreathPeriodSec = Math.max(0.2, periodSeconds);
|
||||
leftMode = Mode.BREATH;
|
||||
}
|
||||
|
||||
public void breathRight(int r, int g, int b, double periodSeconds) {
|
||||
rr = clamp8(r); rg = clamp8(g); rb = clamp8(b);
|
||||
rightBreathPeriodSec = Math.max(0.2, periodSeconds);
|
||||
rightMode = Mode.BREATH;
|
||||
}
|
||||
|
||||
public void rainbowLeft() { leftMode = Mode.RAINBOW; }
|
||||
public void rainbowRight() { rightMode = Mode.RAINBOW; }
|
||||
public void rainbowAll() { leftMode = Mode.RAINBOW; rightMode = Mode.RAINBOW; }
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
if(!Constants.LEDUsing) {led.close(); return;}
|
||||
// 左段
|
||||
switch (leftMode) {
|
||||
case OFF -> setRange(leftStart, leftLen, 0, 0, 0);
|
||||
case SOLID -> setRange(leftStart, leftLen, lr, lg, lb);
|
||||
case BREATH -> updateBreath(leftStart, leftLen, lr, lg, lb, leftBreathPeriodSec);
|
||||
case RAINBOW -> updateRainbow(leftStart, leftLen, true);
|
||||
}
|
||||
|
||||
// 右段
|
||||
switch (rightMode) {
|
||||
case OFF -> setRange(rightStart, rightLen, 0, 0, 0);
|
||||
case SOLID -> setRange(rightStart, rightLen, rr, rg, rb);
|
||||
case BREATH -> updateBreath(rightStart, rightLen, rr, rg, rb, rightBreathPeriodSec);
|
||||
case RAINBOW -> updateRainbow(rightStart, rightLen, false);
|
||||
}
|
||||
|
||||
led.setData(buffer);
|
||||
}
|
||||
|
||||
/* ====================== */
|
||||
/* 内部实现 */
|
||||
/* ====================== */
|
||||
|
||||
private void setRange(int start, int len, int r, int g, int b) {
|
||||
for (int i = 0; i < len; i++) {
|
||||
buffer.setRGB(start + i, r, g, b);
|
||||
}
|
||||
}
|
||||
|
||||
private void updateBreath(int start, int len, int r, int g, int b, double periodSec) {
|
||||
double t = timer.get();
|
||||
double phase = (t % periodSec) / periodSec; // 0..1
|
||||
double brightness = 0.5 * (1.0 + Math.sin(2.0 * Math.PI * phase)); // 0..1
|
||||
int rr = (int) (r * brightness);
|
||||
int gg = (int) (g * brightness);
|
||||
int bb = (int) (b * brightness);
|
||||
setRange(start, len, rr, gg, bb);
|
||||
}
|
||||
|
||||
private void updateRainbow(int start, int len, boolean isLeft) {
|
||||
int offset = isLeft ? leftRainbowOffset : rightRainbowOffset;
|
||||
|
||||
for (int i = 0; i < len; i++) {
|
||||
int hue = (offset + (i * 180 / len)) % 180;
|
||||
buffer.setHSV(start + i, hue, 255, 128);
|
||||
}
|
||||
|
||||
if (isLeft) leftRainbowOffset = (leftRainbowOffset + 2) % 180;
|
||||
else rightRainbowOffset = (rightRainbowOffset + 2) % 180;
|
||||
}
|
||||
|
||||
private static int clamp8(int v) {
|
||||
return Math.max(0, Math.min(255, v));
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,171 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import com.ctre.phoenix6.configs.MotorOutputConfigs;
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.controls.Follower;
|
||||
import com.ctre.phoenix6.controls.VelocityDutyCycle;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.hardware.TalonFXS;
|
||||
import com.ctre.phoenix6.signals.InvertedValue;
|
||||
import com.ctre.phoenix6.signals.MotorAlignmentValue;
|
||||
|
||||
import edu.wpi.first.math.controller.ProfiledPIDController;
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj.Encoder;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.Constants;
|
||||
|
||||
public class ShooterSubsystem extends SubsystemBase {
|
||||
// 定义飞轮、传动和背板电机
|
||||
public TalonFX flywheelMotorLeft;
|
||||
private TalonFX flywheelMotorRight;
|
||||
public TalonFX conveyorMotor;
|
||||
public TalonFXS backboardMotor;
|
||||
public Encoder backboardEncoder;
|
||||
|
||||
// PID 控制器，控制背板角度
|
||||
public ProfiledPIDController backboardPID;
|
||||
private Follower flyWheelFollower;
|
||||
|
||||
private NetworkTable shooterNetworkTable;
|
||||
|
||||
private final VelocityDutyCycle velocityRequest = new VelocityDutyCycle(0);
|
||||
|
||||
public enum Mode{
|
||||
Inalyzed,
|
||||
UnInalyed,
|
||||
Inalyzing;
|
||||
}
|
||||
public Mode currentBackboardInalyzeMode;
|
||||
// 初始化各个部件
|
||||
public ShooterSubsystem() {
|
||||
// 初始化飞轮电机
|
||||
flywheelMotorLeft = new TalonFX(41); // 41是飞轮left的CAN ID
|
||||
flywheelMotorRight = new TalonFX(42); // 42是飞轮right的CAN ID
|
||||
flywheelMotorLeft.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs);
|
||||
flywheelMotorRight.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs);
|
||||
// 初始化传动电机
|
||||
conveyorMotor = new TalonFX(43);
|
||||
conveyorMotor.getConfigurator().apply(Constants.Shooter.conveyorSlot0Configs);
|
||||
|
||||
var configs1 = new MotorOutputConfigs();
|
||||
configs1.Inverted = InvertedValue.Clockwise_Positive;
|
||||
flywheelMotorLeft.getConfigurator().apply(configs1);
|
||||
flyWheelFollower = new Follower(flywheelMotorLeft.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
|
||||
|
||||
var configs2 = new MotorOutputConfigs();
|
||||
configs2.Inverted = InvertedValue.Clockwise_Positive;
|
||||
conveyorMotor.getConfigurator().apply(configs2);
|
||||
|
||||
// 初始化背板电机和编码器
|
||||
backboardMotor = new TalonFXS(44);
|
||||
backboardMotor.getConfigurator().apply(Constants.Shooter.backboardSlot0Configs);
|
||||
backboardEncoder = new Encoder(0, 1, false, Encoder.EncodingType.k4X);
|
||||
backboardEncoder.setSamplesToAverage(5);
|
||||
backboardEncoder.setMinRate(1.0);
|
||||
backboardEncoder.setDistancePerPulse(1);
|
||||
|
||||
// 初始化背板的PID控制器
|
||||
backboardPID = new ProfiledPIDController(Constants.Shooter.backboardPositionPID.kP, Constants.Shooter.backboardPositionPID.kI, Constants.Shooter.backboardPositionPID.kD,
|
||||
new Constraints(10000,100000));
|
||||
|
||||
backboardPID.setTolerance(10.0);
|
||||
|
||||
shooterNetworkTable = NetworkTableInstance.getDefault().getTable("Shooter");
|
||||
|
||||
currentBackboardInalyzeMode = Mode.UnInalyed;
|
||||
}
|
||||
|
||||
public void setRightFollowLeft(){
|
||||
flywheelMotorRight.setControl(flyWheelFollower);
|
||||
}
|
||||
|
||||
public void applyLeftConfigurationToRight(){
|
||||
Slot0Configs configs = new Slot0Configs();
|
||||
flywheelMotorLeft.getConfigurator().refresh(configs);
|
||||
flywheelMotorRight.getConfigurator().apply(configs);
|
||||
}
|
||||
|
||||
|
||||
// 设置飞轮的速度
|
||||
public void setFlywheelSpeedByRPS(double targetSpeed) {
|
||||
flywheelMotorLeft.setControl(velocityRequest.withVelocity(targetSpeed)); //RPS 转每秒
|
||||
setRightFollowLeft();
|
||||
// System.out.println("set flywheel "+targetSpeed);
|
||||
}
|
||||
|
||||
// 设置传动的速度
|
||||
public void setConveyorSpeedByRPS(double speed) {
|
||||
conveyorMotor.setControl(velocityRequest.withVelocity(speed)); // 设置传动电机速度
|
||||
}
|
||||
|
||||
//调试用
|
||||
public void setBackboardSpeedByRPS(double speed){
|
||||
backboardMotor.setControl(velocityRequest.withVelocity(speed));
|
||||
}
|
||||
|
||||
// 设置背板的目标位置，通过PID控制器来调整 范围0-3000
|
||||
public void setBackboardPosition(double targetPosition) {
|
||||
// 限制目标位置在上下限之间
|
||||
if (targetPosition > Constants.Shooter.backboardUpLimit) {
|
||||
targetPosition = Constants.Shooter.backboardUpLimit; // 限制最大上限
|
||||
} else if (targetPosition < Constants.Shooter.backboardDownLimit) {
|
||||
targetPosition = Constants.Shooter.backboardDownLimit; // 限制最小下限
|
||||
}
|
||||
backboardPID.setGoal(targetPosition);
|
||||
}
|
||||
public void outputBackboard(){
|
||||
|
||||
// 使用PID控制器来计算背板的运动速度
|
||||
double pidOutput = backboardPID.calculate(getBackboardPosition());
|
||||
pidOutput = Math.max(-Constants.Shooter.backboardSpeedMax, Math.min(Constants.Shooter.backboardSpeedMax, pidOutput)); // 限制输出范围在-1到1之间
|
||||
shooterNetworkTable.getEntry("backboardPIDOutput").setDouble(pidOutput);
|
||||
// // 设置背板电机的速度，根据PID输出进行调整
|
||||
setBackboardSpeedByRPS(pidOutput);
|
||||
}
|
||||
// 停止所有电机
|
||||
public void stopMotors() {
|
||||
flywheelMotorLeft.set(0);
|
||||
flywheelMotorRight.set(0);
|
||||
conveyorMotor.set(0);
|
||||
backboardMotor.set(0);
|
||||
}
|
||||
|
||||
// 获取背板当前角度
|
||||
public double getBackboardPosition() {
|
||||
return backboardEncoder.getDistance(); // 返回背板电机编码器的当前角度
|
||||
}
|
||||
|
||||
// 检查背板是否到达目标角度
|
||||
public boolean isBackboardAtTarget() {
|
||||
return backboardPID.atGoal(); // 判断背板是否到达目标角度
|
||||
}
|
||||
|
||||
@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());
|
||||
shooterNetworkTable.getEntry("conveyorPIDReference").setDouble(conveyorMotor.getClosedLoopReference().getValueAsDouble());
|
||||
shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(getBackboardPosition());
|
||||
shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardPID.getSetpoint().position);
|
||||
shooterNetworkTable.getEntry("isBackboardAtTarget").setBoolean(isBackboardAtTarget());
|
||||
shooterNetworkTable.getEntry("distanceGetted").setDouble(getDistance());
|
||||
shooterNetworkTable.getEntry("InalyzeBackboardMode").setString(currentBackboardInalyzeMode.name());
|
||||
}
|
||||
|
||||
private double getDistance(){
|
||||
//获得车子距离Hub的距离
|
||||
return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToHub").getDouble(0.0);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,92 @@
|
||||
package frc.robot.tuning;
|
||||
import java.util.function.Function;
|
||||
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.controls.VelocityDutyCycle;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import edu.wpi.first.networktables.*;
|
||||
|
||||
public class ConfigTalonFXMotorTuner {
|
||||
private final TalonFX motor;
|
||||
private final String motorName;
|
||||
private final Slot0Configs motorSlot0Configs;
|
||||
private final NetworkTable table;
|
||||
|
||||
// ---- Subscribers：读取 Elastic 改过的新值 ----
|
||||
private final BooleanSubscriber enableSub;
|
||||
private final DoubleSubscriber KpSub;
|
||||
private final DoubleSubscriber KiSub;
|
||||
private final DoubleSubscriber KdSub;
|
||||
private final DoubleSubscriber KvSub;
|
||||
private final DoubleSubscriber KaSub;
|
||||
private final DoubleSubscriber KsSub;
|
||||
|
||||
private final BooleanSubscriber enableRuningSub;
|
||||
private final DoubleSubscriber runningSpeedRPS;
|
||||
private double lastKp = 0, lastKi = 0, lastKd = 0, lastKv = 0, lastKa = 0, lastKs = 0;
|
||||
|
||||
private final VelocityDutyCycle velocityRequest = new VelocityDutyCycle(0);
|
||||
public ConfigTalonFXMotorTuner(TalonFX motor_, String motorName_, Slot0Configs motorSlot0Configs_) {
|
||||
this.motor = motor_;
|
||||
this.motorName = motorName_;
|
||||
this.motorSlot0Configs = motorSlot0Configs_;
|
||||
|
||||
this.table = NetworkTableInstance.getDefault().getTable("Tuning/" + motorName);
|
||||
|
||||
table.getBooleanTopic(motorName+"Enable").publish().set(false);
|
||||
table.getDoubleTopic(motorName+"kP").publish().set(motorSlot0Configs.kP);;
|
||||
table.getDoubleTopic(motorName+"kI").publish().set(motorSlot0Configs.kI);
|
||||
table.getDoubleTopic(motorName+"kD").publish().set(motorSlot0Configs.kD);
|
||||
table.getDoubleTopic(motorName+"kV").publish().set(motorSlot0Configs.kV);
|
||||
table.getDoubleTopic(motorName+"kA").publish().set(motorSlot0Configs.kA);
|
||||
table.getDoubleTopic(motorName+"kS").publish().set(motorSlot0Configs.kS);
|
||||
|
||||
table.getBooleanTopic(motorName+"EnableRunning").publish().set(false);;
|
||||
table.getDoubleTopic(motorName+"RunningSpeedRPS").publish().set(0);;
|
||||
|
||||
this.enableSub = table.getBooleanTopic(motorName+"Enable").subscribe(false);
|
||||
this.KpSub = table.getDoubleTopic(motorName+"kP").subscribe(motorSlot0Configs.kP);
|
||||
this.KiSub = table.getDoubleTopic(motorName+"kI").subscribe(motorSlot0Configs.kI);
|
||||
this.KdSub = table.getDoubleTopic(motorName+"kD").subscribe(motorSlot0Configs.kD);
|
||||
this.KvSub = table.getDoubleTopic(motorName+"kV").subscribe(motorSlot0Configs.kV);
|
||||
this.KaSub = table.getDoubleTopic(motorName+"kA").subscribe(motorSlot0Configs.kA);
|
||||
this.KsSub = table.getDoubleTopic(motorName+"kS").subscribe(motorSlot0Configs.kS);
|
||||
this.enableRuningSub = table.getBooleanTopic(motorName+"EnableRunning").subscribe(false);
|
||||
this.runningSpeedRPS = table.getDoubleTopic(motorName+"RunningSpeedRPS").subscribe(0);
|
||||
}
|
||||
|
||||
public void periodic(Runnable func) {
|
||||
if (!enableSub.get()) return;
|
||||
double kp = KpSub.get();
|
||||
double ki = KiSub.get();
|
||||
double kd = KdSub.get();
|
||||
double kv = KvSub.get();
|
||||
double ka = KaSub.get();
|
||||
double ks = KsSub.get();
|
||||
|
||||
if (!(kp == lastKp && ki == lastKi && kd == lastKd && kv == lastKv && ka == lastKa && ks == lastKs)) {
|
||||
|
||||
Slot0Configs slot0 = new Slot0Configs()
|
||||
.withKP(kp).withKI(ki).withKD(kd).withKV(kv).withKA(ka).withKS(ks);
|
||||
|
||||
motor.getConfigurator().apply(slot0);
|
||||
System.out.println("applyed settings: "+motorName);
|
||||
|
||||
lastKp = kp; lastKi = ki; lastKd = kd; lastKv = kv; lastKa = ka; lastKs = ks;
|
||||
|
||||
if(func!=null){
|
||||
func.run();
|
||||
}
|
||||
}
|
||||
|
||||
if (enableRuningSub.get()) {;
|
||||
double speed = runningSpeedRPS.get();
|
||||
motor.setControl(velocityRequest.withVelocity(speed));
|
||||
// System.out.println("running motor with speed: "+speed+" on motor: "+motorName);
|
||||
}else{
|
||||
motor.setControl(velocityRequest.withVelocity(0));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
@@ -0,0 +1,86 @@
|
||||
package frc.robot.tuning;
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.controls.VelocityDutyCycle;
|
||||
import com.ctre.phoenix6.controls.VelocityVoltage;
|
||||
import com.ctre.phoenix6.hardware.TalonFXS;
|
||||
|
||||
import edu.wpi.first.networktables.*;
|
||||
|
||||
public class ConfigTalonFXSMotorTuner {
|
||||
private final TalonFXS motor;
|
||||
private final String motorName;
|
||||
private final Slot0Configs motorSlot0Configs;
|
||||
private final NetworkTable table;
|
||||
|
||||
// ---- Subscribers：读取 Elastic 改过的新值 ----
|
||||
private final BooleanSubscriber enableSub;
|
||||
private final DoubleSubscriber KpSub;
|
||||
private final DoubleSubscriber KiSub;
|
||||
private final DoubleSubscriber KdSub;
|
||||
private final DoubleSubscriber KvSub;
|
||||
private final DoubleSubscriber KaSub;
|
||||
private final DoubleSubscriber KsSub;
|
||||
|
||||
private final BooleanSubscriber enableRuningSub;
|
||||
private final DoubleSubscriber runningSpeedRPS;
|
||||
private double lastKp = 0, lastKi = 0, lastKd = 0, lastKv = 0, lastKa = 0, lastKs = 0;
|
||||
|
||||
private final VelocityDutyCycle velocityRequest = new VelocityDutyCycle(0);
|
||||
public ConfigTalonFXSMotorTuner(TalonFXS motor_, String motorName_, Slot0Configs motorSlot0Configs_) {
|
||||
this.motor = motor_;
|
||||
this.motorName = motorName_;
|
||||
this.motorSlot0Configs = motorSlot0Configs_;
|
||||
|
||||
this.table = NetworkTableInstance.getDefault().getTable("Tuning/" + motorName);
|
||||
table.getBooleanTopic(motorName+"Enable").publish().set(false);
|
||||
table.getDoubleTopic(motorName+"kP").publish().set(motorSlot0Configs.kP);
|
||||
table.getDoubleTopic(motorName+"kI").publish().set(motorSlot0Configs.kI);
|
||||
table.getDoubleTopic(motorName+"kD").publish().set(motorSlot0Configs.kD);
|
||||
table.getDoubleTopic(motorName+"kV").publish().set(motorSlot0Configs.kV);
|
||||
table.getDoubleTopic(motorName+"kA").publish().set(motorSlot0Configs.kA);
|
||||
table.getDoubleTopic(motorName+"kS").publish().set(motorSlot0Configs.kS);
|
||||
|
||||
table.getBooleanTopic(motorName+"EnableRunning").publish().set(false);
|
||||
table.getDoubleTopic(motorName+"RunningSpeedRPS").publish().set(0);
|
||||
|
||||
this.enableSub = table.getBooleanTopic(motorName+"Enable").subscribe(false);
|
||||
this.KpSub = table.getDoubleTopic(motorName+"kP").subscribe(motorSlot0Configs.kP);
|
||||
this.KiSub = table.getDoubleTopic(motorName+"kI").subscribe(motorSlot0Configs.kI);
|
||||
this.KdSub = table.getDoubleTopic(motorName+"kD").subscribe(motorSlot0Configs.kD);
|
||||
this.KvSub = table.getDoubleTopic(motorName+"kV").subscribe(motorSlot0Configs.kV);
|
||||
this.KaSub = table.getDoubleTopic(motorName+"kA").subscribe(motorSlot0Configs.kA);
|
||||
this.KsSub = table.getDoubleTopic(motorName+"kS").subscribe(motorSlot0Configs.kS);
|
||||
this.enableRuningSub = table.getBooleanTopic(motorName+"EnableRunning").subscribe(false);
|
||||
this.runningSpeedRPS = table.getDoubleTopic(motorName+"RunningSpeedRPS").subscribe(0);
|
||||
}
|
||||
|
||||
public void periodic() {
|
||||
if (!enableSub.get()) return;
|
||||
|
||||
double kp = KpSub.get();
|
||||
double ki = KiSub.get();
|
||||
double kd = KdSub.get();
|
||||
double kv = KvSub.get();
|
||||
double ka = KaSub.get();
|
||||
double ks = KsSub.get();
|
||||
|
||||
if (!(kp == lastKp && ki == lastKi && kd == lastKd && kv == lastKv && ka == lastKa && ks == lastKs)) {
|
||||
|
||||
Slot0Configs slot0 = new Slot0Configs()
|
||||
.withKP(kp).withKI(ki).withKD(kd).withKV(kv).withKA(ka).withKS(ks);
|
||||
|
||||
motor.getConfigurator().apply(slot0);
|
||||
System.out.println("applyed settings: "+motorName);
|
||||
|
||||
lastKp = kp; lastKi = ki; lastKd = kd; lastKv = kv; lastKa = ka; lastKs = ks;
|
||||
}
|
||||
|
||||
if (enableRuningSub.get()) {;
|
||||
double speed = runningSpeedRPS.get();
|
||||
motor.setControl(velocityRequest.withVelocity(speed));
|
||||
// System.out.println("running motor with speed: "+speed+" on motor: "+motorName);
|
||||
}else{
|
||||
motor.setControl(velocityRequest.withVelocity(0));
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,68 @@
|
||||
package frc.robot.tuning;
|
||||
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.swerve.SwerveDrivetrain;
|
||||
import edu.wpi.first.networktables.*;
|
||||
|
||||
public class DriveGainsTuner {
|
||||
private final NetworkTable table =
|
||||
NetworkTableInstance.getDefault().getTable("Tuning/Drive");
|
||||
|
||||
// ---- Publishers：让 Elastic 能“看到”这些键，并给初值 ----
|
||||
private final BooleanPublisher enablePub =
|
||||
table.getBooleanTopic("Enable").publish();
|
||||
|
||||
private final DoublePublisher kpPub = table.getDoubleTopic("kP").publish();
|
||||
private final DoublePublisher kiPub = table.getDoubleTopic("kI").publish();
|
||||
private final DoublePublisher kdPub = table.getDoubleTopic("kD").publish();
|
||||
private final DoublePublisher kvPub = table.getDoubleTopic("kV").publish();
|
||||
|
||||
// ---- Subscribers：读取 Elastic 改过的新值 ----
|
||||
private final BooleanSubscriber enableSub =
|
||||
table.getBooleanTopic("Enable").subscribe(false);
|
||||
|
||||
private final DoubleSubscriber kpSub = table.getDoubleTopic("kP").subscribe(0.01);
|
||||
private final DoubleSubscriber kiSub = table.getDoubleTopic("kI").subscribe(0.0);
|
||||
private final DoubleSubscriber kdSub = table.getDoubleTopic("kD").subscribe(0.0);
|
||||
private final DoubleSubscriber kvSub = table.getDoubleTopic("kV").subscribe(0.124);
|
||||
|
||||
private double lastKP = Double.NaN, lastKI = Double.NaN, lastKD = Double.NaN, lastKV = Double.NaN;
|
||||
|
||||
private final TalonFX[] driveMotors;
|
||||
|
||||
public DriveGainsTuner(SwerveDrivetrain<?, ?, ?> drivetrain) {
|
||||
// 发布初始值（这一步做完 Elastic 就能看到这些键）
|
||||
enablePub.set(false);
|
||||
kpPub.set(0.01);
|
||||
kiPub.set(0.0);
|
||||
kdPub.set(0.0);
|
||||
kvPub.set(0.124);
|
||||
|
||||
var modules = drivetrain.getModules();
|
||||
driveMotors = new TalonFX[modules.length];
|
||||
for (int i = 0; i < modules.length; i++) {
|
||||
driveMotors[i] = (TalonFX) modules[i].getDriveMotor();
|
||||
}
|
||||
}
|
||||
|
||||
public void periodic() {
|
||||
if (!enableSub.get()) return;
|
||||
|
||||
double kp = kpSub.get();
|
||||
double ki = kiSub.get();
|
||||
double kd = kdSub.get();
|
||||
double kv = kvSub.get();
|
||||
|
||||
if (kp == lastKP && ki == lastKI && kd == lastKD && kv == lastKV) return;
|
||||
|
||||
Slot0Configs slot0 = new Slot0Configs()
|
||||
.withKP(kp).withKI(ki).withKD(kd).withKV(kv);
|
||||
|
||||
for (TalonFX m : driveMotors) {
|
||||
m.getConfigurator().apply(slot0);
|
||||
}
|
||||
|
||||
lastKP = kp; lastKI = ki; lastKD = kd; lastKV = kv;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,32 @@
|
||||
package frc.robot.utils;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
|
||||
import frc.robot.subsystems.LEDSubsystem;
|
||||
|
||||
public final class LedBindings {
|
||||
private LedBindings() {}
|
||||
|
||||
public static void bindModeIndicators(LEDSubsystem leds) {
|
||||
// Disabled：左右红色呼吸（允许在禁用状态运行）
|
||||
RobotModeTriggers.disabled().onTrue(
|
||||
Commands.runOnce(() -> {
|
||||
leds.breathLeft(255, 0, 0, 1.0);
|
||||
leds.breathRight(255, 0, 0, 1.0);
|
||||
}, leds).ignoringDisable(true)
|
||||
);
|
||||
|
||||
// Teleop：左右彩虹
|
||||
RobotModeTriggers.teleop().onTrue(
|
||||
Commands.runOnce(leds::rainbowAll, leds)
|
||||
);
|
||||
|
||||
// Auto：左右蓝色常亮
|
||||
RobotModeTriggers.autonomous().onTrue(
|
||||
Commands.runOnce(() -> {
|
||||
leds.solidLeft(0, 0, 255);
|
||||
leds.solidRight(0, 0, 255);
|
||||
}, leds)
|
||||
);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user