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

双手柄版本

Browse Source
This commit is contained in:
OnDYCLaptop
2026-03-02 14:35:18 +08:00
parent 06340c5724
commit 5f3452e091
8 changed files with 106 additions and 182 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc/robot/controls/DriveControls.java
+16 -49
View File
@@ -126,7 +126,6 @@ public class DriveControls {
vomega = calculateRotationSpeedFromRotationAngle(calculateDistanceAndRotationToPassBall()[1]);
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
break;
case AutoAimming:
vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
@@ -161,15 +160,8 @@ public class DriveControls {
errorB = Math.atan2(Math.sin(errorB), Math.cos(errorB));
// 选误差绝对值更小的那个
double chosenError = (Math.abs(errorA) <= Math.abs(errorB)) ? errorA : errorB;
if (Math.abs(chosenError) < Math.toRadians(2.0)) {
vomega = 0.0;
} else {
vomega = chosenError * Constants.AutoPositioning.TurningkP;
}
// 限制最大角速度
vomega = Math.max(-maxAngularRate, Math.min(maxAngularRate, vomega));
vomega = calculatevomegaFromRotationAngle(chosenError);
autoControlNetworkTable.getEntry("BumpAutoRotationRate").setDouble(vomega);
//vomega = calculateRotationSpeedForBump();
break;
case CrossingTrench:
@@ -188,14 +180,8 @@ public class DriveControls {
double currentRad = currentAngle.getRadians();
double targetRad = Math.toRadians(Constants.AutoPositioning.TrenchtargetAngle);
double TrenchAngleerror = targetRad - currentRad;
if (Math.abs(TrenchAngleerror) < Math.toRadians(1.0)) {
vomega = 0.0;
} else {
vomega = TrenchAngleerror * Constants.AutoPositioning.TurningkP;
}
// 限幅（防止转太快）
vomega = Math.max(-maxAngularRate, Math.min(maxAngularRate, vomega));
vomega = calculatevomegaFromRotationAngle(TrenchAngleerror);
autoControlNetworkTable.getEntry("TrenchAutoRotationRate").setDouble(vomega);
//vomega = calculateRotationSpeedForTrench();
break;
}
@@ -238,33 +224,6 @@ public class DriveControls {
double x2 = target2 - current;
return Math.abs(x1) < Math.abs(x2) ? x1 : x2;
}
public double calculateRotationSpeedForTrench(){
Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
double currentAngle = currentPose.getRotation().getDegrees();
double targetAngle = Constants.AutoPositioning.autoRotationForTrenchTargetDegrees; // 转换为度
double angleDifference = targetAngle - currentAngle;
// Normalize angle difference to the range [-180, 180]
if (angleDifference > 180) {
angleDifference -= 360;
} else if (angleDifference < -180) {
angleDifference += 360;
}
return calculateRotationSpeedFromRotationAngle(angleDifference);
}
public double calculateRotationSpeedForBump(){
Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
double currentAngle = currentPose.getRotation().getDegrees();
double targetAngle = Constants.AutoPositioning.autoRotationForBumpTargetDegrees; // 转换为度
double angleDifference = targetAngle - currentAngle;
// Normalize angle difference to the range [-180, 180]
if (angleDifference > 180) {
angleDifference -= 360;
} else if (angleDifference < -180) {
angleDifference += 360;
}
return calculateRotationSpeedFromRotationAngle(angleDifference);
}
// Calculate distance and rotation (angle difference)
public double[] calculateDistanceAndRotationToHub() {
Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
double targetX = Constants.Field.RedHubPositionX; // 目标 X 坐标
@@ -299,7 +258,6 @@ public class DriveControls {
double targetY1 = Constants.Field.PassingBallPosY1; // 目标 Y 坐标
double targetY2 = Constants.Field.PassingBallPosY2; // 目标 Y 坐标
// 计算目标角度（相对于场地坐标系）
double deltaX = targetX - currentPose.getX();
double deltaY = calculateDifferenceToTwoTarget(currentPose.getY(), targetY1, targetY2);
@@ -324,7 +282,6 @@ public class DriveControls {
return distanceAndRotation;
}
// Calculate rotation speed from rotation angle difference
public double calculateRotationSpeedFromRotationAngle(double angleDifference) {
double calDifference = angleDifference;
@@ -341,8 +298,18 @@ public class DriveControls {
return 0;
}
}
private double calculatevomegaFromRotationAngle(double error) {
double vomega;
if (Math.abs(error) < Math.toRadians(Constants.AutoPositioning.autoPositioningAngleError)) {
vomega = 0.0;
} else {
vomega = error * Constants.AutoPositioning.TurningkP;
}
// 限制最大角速度
vomega = Math.max(-maxAngularRate, Math.min(maxAngularRate, vomega));
return vomega;
}
/**
* 只限制“加速”（幅值变大），不限制“减速”（幅值变小）
src/main/java/frc/robot/controls/IntakerControls.java
+53 -44
View File
@@ -1,9 +1,13 @@
package frc.robot.controls;
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.button.CommandXboxController;
import frc.robot.subsystems.IntakerSubsystem;
import frc.robot.Constants;
public class IntakerControls {
private final IntakerSubsystem intakerSubsystem;
@@ -16,56 +20,61 @@ public class IntakerControls {
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);
// 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);
// // getter 开
// intakerSubsystem.setIntakeGetterOn(true);
// }, intakerSubsystem)
// )
// .onFalse(
// Commands.runOnce(() -> {
// // 松：把当前位置作为target保持
// double cur = intakerSubsystem.getIntakerotaterPosition();
// intakerSubsystem.setIntakerRotaterPosition(cur);
// gtter继续转
intakerSubsystem.setIntakeGetterOn(true);
}, intakerSubsystem)
);
// // 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);
// === 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)
);
}
// // 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();
return Commands.run(() -> {
double getterspeed = controller.getLeftTriggerAxis() * Constants.Intaker.IntakeGetterK + Constants.Intaker.Getterminspeed;
if (controller.getLeftTriggerAxis() < 0.1) {
getterspeed = 0;
}
intakerSubsystem.setIntakerGetterSpeed(getterspeed);
}, intakerSubsystem);
}
}
src/main/java/frc/robot/controls/ShooterControls.java
+4 -69
View File
@@ -110,8 +110,6 @@ public class ShooterControls {
break;
}
}, shooterSubsystem);
}
@@ -145,73 +143,6 @@ public class ShooterControls {
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;
}
@@ -219,4 +150,8 @@ public class ShooterControls {
public void adjustBackboardRateOffset(double offset) {
backboardPositionOffset += offset;
}
public void resetOffsets(){
flywheelSpeedOffset = 0.0;
backboardPositionOffset = 0.0;
}
}