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

自动瞄准+射球已封装 增加conveyor独立控制

Browse Source
This commit is contained in:
OnDYCLaptop
2026-03-09 12:17:42 +08:00
parent 9a426a8f8a
commit c8b7359b99
26 changed files with 351 additions and 306 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc/robot/Constants.java
+1
View File
@@ -149,6 +149,7 @@ public final class Constants {
public static double climbUpTicks = 94; // TODO: 实测后调整
public static double climbDownTicks = 0; // TODO: 实测后调整
public static double climbDownAutoTicks = -5; // TODO: 实测后调整，自动爬时可能不需要完全降到底
public static final double climbVelocity = -200;
public static final double holdToleranceRot = 0.5;
public static final double deltaDownTicks = 1;
src/main/java/frc/robot/RobotContainer.java
+5 -2
View File
@@ -10,6 +10,7 @@ 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.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -76,7 +77,8 @@ public class RobotContainer {
climberSubsystem,
IntakerSubsystem,
drivetrain,
shooterSubsystem
shooterSubsystem,
driveControls
);
SignalLogger.enableAutoLogging(false);
@@ -162,7 +164,8 @@ public class RobotContainer {
controllerupper.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-100)));
controllerupper.back().onTrue(Commands.runOnce(() -> shooterControls.resetOffsets()));
controllerupper.start().onTrue(Commands.runOnce(() -> shooterControls.resetBackboardCommand()));
controllerupper.b().onTrue(Commands.runOnce(() -> shooterControls.resetBackboardCommand()));
controllerupper.b().whileTrue(new RunCommand(() -> shooterSubsystem.setConveyorSpeedByRPS(10), shooterSubsystem));
controllerupper.a().whileTrue(new RunCommand(() -> shooterSubsystem.setConveyorSpeedByRPS(-25), shooterSubsystem));
}
/* ====================== */
/* LED 绑定 */
src/main/java/frc/robot/controls/Autocommand.java
+18 -8
View File
@@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.Subsystem;
import frc.robot.subsystems.ClimberSubsystem;
import frc.robot.RobotStatusManager;
import frc.robot.subsystems.IntakerSubsystem;
@@ -26,7 +27,9 @@ import frc.robot.Constants.RobotStatus;
ClimberSubsystem climber,
IntakerSubsystem intaker,
CommandSwerveDrivetrain drivetrain,
ShooterSubsystem shooter
ShooterSubsystem shooter,
DriveControls driveControls,
ShooterControls shooterControls
) {
NamedCommands.registerCommand("climb_up", climb_up(climber));
NamedCommands.registerCommand("climb_down", climb_down(climber));
@@ -37,20 +40,22 @@ import frc.robot.Constants.RobotStatus;
NamedCommands.registerCommand("intaker_up", intaker_up(intaker));
NamedCommands.registerCommand("auto_align_to_climb", autoAlignToClimb(drivetrain));
NamedCommands.registerCommand("shoot", shoot(shooter));
NamedCommands.registerCommand("auto_aim_and_shoot", autoAimAndShoot(driveControls, shooterControls));
}
public static Command climb_up(ClimberSubsystem climber) {
return new RunCommand(() -> {
climber.climbUp(); // 控制爬升器上升
}, climber).withTimeout(1.0) //
.andThen(new InstantCommand(() -> {
}, climber).withTimeout(1.0).andThen(new RunCommand(() -> {
climber.climbUpNonStopforAuto();
}, climber).withTimeout(0.5)).andThen(new InstantCommand(() -> {
climber.holdPosition(); // 停止并锁定当前爬升器位置
}, climber));
}
public static Command climb_down(ClimberSubsystem climber) {
return new RunCommand(() -> {
climber.climbDown(); // 控制爬升器下降
}, climber).withTimeout(1.0) //
climber.climbDownAuto(); // 控制爬升器下降
}, climber).withTimeout(2.0) //
.andThen(new InstantCommand(() -> {
climber.holdPosition(); // 停止并锁定当前爬升器位置
}, climber));
@@ -100,7 +105,7 @@ import frc.robot.Constants.RobotStatus;
},
drivetrain
).until(() -> drivetrain.isAlignedToAprilTag()));
}
}
public static Command shoot(ShooterSubsystem shooter) {
return new RunCommand(() -> {
@@ -125,9 +130,14 @@ import frc.robot.Constants.RobotStatus;
//shooter.setBackboardPosition(0); // 恢复背板位置
shooter.setConveyorSpeedByRPS(0); // 停止输送
}, shooter)));
}
public static Command autoAimAndShoot(DriveControls driveControls, ShooterControls shooterControl) {
return driveControls.autoAimCommand()
.andThen(shooterControl.autoShootToHubCommand());
}
}
src/main/java/frc/robot/controls/DriveControls.java
+17
View File
@@ -12,6 +12,7 @@ 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.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.Constants;
import frc.robot.RobotStatusManager;
@@ -414,6 +415,22 @@ public class DriveControls {
drivetrain.stop();
}
}
public Command autoAimCommand() {
return drivetrain.applyRequest(() -> {
double[] distanceAndRotation = calculateDistanceAndRotationToHub();
double vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
return fieldCentric
.withVelocityX(0.0)
.withVelocityY(0.0)
.withRotationalRate(vomega);
}, () -> DriveMode.FIELD_CENTRIC)
.until(() -> Math.abs(calculateDistanceAndRotationToHub()[1]) < 1.5)
.withTimeout(1.0)
.andThen(new InstantCommand(() -> {
drivetrain.stop();
}, drivetrain));
}
}
src/main/java/frc/robot/controls/ShooterControls.java
+48 -2
View File
@@ -1,9 +1,13 @@
package frc.robot.controls;
import java.util.Set;
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.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.Constants;
import frc.robot.RobotStatusManager;
@@ -147,8 +151,50 @@ public class ShooterControls {
flywheelSpeedOffset = 0.0;
backboardPositionOffset = 0.0;
}
public void startconveyor(){
shooterSubsystem.setConveyorSpeedByRPS(10);
public Command autoShootToHubCommand() {
return Commands.defer(() -> {
double x = shooterSubsystem.getDistanceToHub();
x = Math.max(1.2, Math.min(4.5, x));
double flywheelTargetSpeed = -1.4964026859575532*x*x*x*x*x*x*x+29.82445640241506*x*x*x*x*x*x-247.71603153159896*x*x*x*x*x+1108.8727302184625*x*x*x*x-2881.517500617481*x*x*x+4332.05363145275*x*x-3467.2508337871454*x+1192.1508062663759;
double backboardTargetPosition = +34.5286998307899*x*x*x*x*x*x*x*x-788.5788623683884*x*x*x*x*x*x*x+7658.073303693148*x*x*x*x*x*x-41191.69338972519*x*x*x*x*x+133870.54588688645*x*x*x*x-268559.6987062158*x*x*x+324254.6530324262*x*x-215015.48893131423*x+59977.46545382827;
double conveyorTargetSpeed = Constants.Shooter.conveyorSpeed;
return new RunCommand(() -> {
shooterSubsystem.setFlywheelSpeedByRPS(flywheelTargetSpeed);
shooterSubsystem.setConveyorSpeedByRPS(0.0);
shooterSubsystem.setBackboardPosition(backboardTargetPosition);
if (shooterSubsystem.isBackboardAtTarget()) {
shooterSubsystem.backboardMotor.set(0);
} else {
shooterSubsystem.outputBackboard();
}
}, shooterSubsystem)
.until(() ->
shooterSubsystem.isBackboardAtTarget()
&& shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble() >= flywheelTargetSpeed * 0.9
)
.withTimeout(1.0)
.andThen(new RunCommand(() -> {
shooterSubsystem.setFlywheelSpeedByRPS(flywheelTargetSpeed);
shooterSubsystem.setBackboardPosition(backboardTargetPosition);
if (shooterSubsystem.isBackboardAtTarget()) {
shooterSubsystem.backboardMotor.set(0);
} else {
shooterSubsystem.outputBackboard();
}
if (shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble() >= flywheelTargetSpeed * 0.9) {
shooterSubsystem.setConveyorSpeedByRPS(conveyorTargetSpeed);
} else {
shooterSubsystem.setConveyorSpeedByRPS(0.0);
}
}, shooterSubsystem).withTimeout(3.0))
.andThen(new InstantCommand(() -> {
shooterSubsystem.setFlywheelSpeedByRPS(0.0);
shooterSubsystem.setConveyorSpeedByRPS(0.0);
shooterSubsystem.backboardMotor.set(0.0);
}, shooterSubsystem));
}, Set.of(shooterSubsystem));
}
}
src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+7
View File
@@ -70,6 +70,10 @@ public class ClimberSubsystem extends SubsystemBase {
targetPositionTicks = Constants.Climber.climbDownTicks;
climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
}
public void climbDownAuto() {
targetPositionTicks = Constants.Climber.climbDownAutoTicks;
climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
}
public void climbDownNonStop(){
climberMotor.setControl(velocityRequest.withVelocity(Constants.Climber.climbVelocity));
@@ -78,6 +82,9 @@ public class ClimberSubsystem extends SubsystemBase {
public void climbUpNonStop(){
climberMotor.setControl(velocityRequest.withVelocity(-Constants.Climber.climbVelocity));
}
public void climbUpNonStopforAuto(){
climberMotor.set(0.07);
}
public void captureHoldPosition() {
holdPositionTicks = getPositionTicks();