219 lines
10 KiB
Java
219 lines
10 KiB
Java
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.Constants;
|
|
import frc.robot.RobotStatusManager;
|
|
import frc.robot.Constants.Intaker;
|
|
import frc.robot.subsystems.IntakerSubsystem;
|
|
import frc.robot.subsystems.ShooterSubsystem;
|
|
|
|
public class ShooterControls {
|
|
private final ShooterSubsystem shooterSubsystem;
|
|
private final CommandXboxController controller;
|
|
|
|
private double flywheelSpeedOffset = 0.0;
|
|
private double backboardPositionOffset = 0.0;
|
|
private NetworkTable shooterControlTable;
|
|
private double conveyorSpeed = 0.0;
|
|
double flywheelSpeed = 0;
|
|
double backboardPosition = 0;
|
|
private final RobotStatusManager robotStatusManager;
|
|
public ShooterControls(ShooterSubsystem shooterSubsystem, CommandXboxController controller,RobotStatusManager rsm) {
|
|
this.shooterSubsystem = shooterSubsystem;
|
|
this.controller = controller;
|
|
this.robotStatusManager = rsm;
|
|
shooterControlTable = NetworkTableInstance.getDefault().getTable("ShooterControl");
|
|
}
|
|
|
|
public Command defaultShooterCommand() {
|
|
return Commands.run(() -> {
|
|
switch (robotStatusManager.getStatus()) {
|
|
case Stopped:
|
|
shooterSubsystem.stopMotors();
|
|
break;
|
|
case AllTelop:
|
|
case PassingBall:
|
|
flywheelSpeed = 0;//todo:实测传球的时候的速度
|
|
flywheelSpeed += flywheelSpeedOffset;
|
|
backboardPosition = 0;//todo:实测传球的时候的背板位置
|
|
backboardPosition += backboardPositionOffset;
|
|
conveyorSpeed = Constants.Shooter.conveyorSpeed;
|
|
if (controller.getRightTriggerAxis() < 0.1) {
|
|
flywheelSpeed = 0;
|
|
conveyorSpeed = 0;
|
|
}
|
|
if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.7){
|
|
conveyorSpeed = 0;
|
|
}
|
|
// flywheelSpeed = calculateFlywheelSpeedOnlywithOffset(60);
|
|
// double flywheelSpeed = calculateFlywheelSpeed(0);
|
|
// double BackboardPosition = calculateBackboardPosition(0);
|
|
shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
|
|
shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
|
|
shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition);
|
|
shooterSubsystem.setFlywheelSpeedByRPS(flywheelSpeed);
|
|
shooterSubsystem.setConveyorSpeedByRPS(conveyorSpeed);
|
|
shooterSubsystem.setBackboardPosition(backboardPosition);
|
|
if(shooterSubsystem.isBackboardAtTarget()){
|
|
shooterSubsystem.backboardMotor.set(0);
|
|
}else{
|
|
shooterSubsystem.outputBackboard();
|
|
}
|
|
break;
|
|
case AutoAimming:
|
|
|
|
var x = shooterSubsystem.getDistance();
|
|
x = Math.max(1.2, Math.min(4.5, x));
|
|
flywheelSpeed = 0.6956596811733782*x*x*x*x*x*x*x-14.164408860183267*x*x*x*x*x*x+119.7115375240918*x*x*x*x*x-543.0984876677477*x*x*x*x+1425.1928543240738*x*x*x-2156.199296490315*x*x+1736.568139600292*x-516.097598057351;
|
|
flywheelSpeed += flywheelSpeedOffset;
|
|
backboardPosition = -122.11889023593778*x*x*x*x*x*x*x+2453.385583841308*x*x*x*x*x*x-20553.05413872581*x*x*x*x*x+92963.33175750206*x*x*x*x-244905.25916147238*x*x*x+375150.1363997868*x*x-308031.51715612336*x+104203.52690842684;
|
|
backboardPosition += backboardPositionOffset;
|
|
conveyorSpeed = Constants.Shooter.conveyorSpeed;
|
|
if (controller.getRightTriggerAxis() < 0.1) {
|
|
flywheelSpeed = 0;
|
|
conveyorSpeed = 0;
|
|
}
|
|
if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.7){
|
|
conveyorSpeed = 0;
|
|
}
|
|
// flywheelSpeed = calculateFlywheelSpeedOnlywithOffset(60);
|
|
// double flywheelSpeed = calculateFlywheelSpeed(0);
|
|
// double BackboardPosition = calculateBackboardPosition(0);
|
|
shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
|
|
shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
|
|
shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition);
|
|
shooterSubsystem.setFlywheelSpeedByRPS(flywheelSpeed);
|
|
shooterSubsystem.setConveyorSpeedByRPS(conveyorSpeed);
|
|
shooterSubsystem.setBackboardPosition(backboardPosition);
|
|
if(shooterSubsystem.isBackboardAtTarget()){
|
|
shooterSubsystem.backboardMotor.set(0);
|
|
}else{
|
|
shooterSubsystem.outputBackboard();
|
|
}
|
|
break;
|
|
case Climbing:
|
|
case CrossingBump:
|
|
case CrossingTrench:
|
|
if(shooterSubsystem.getBackboardPosition() <= 20)
|
|
shooterSubsystem.stopMotors();
|
|
else{
|
|
shooterSubsystem.setBackboardPosition(0);
|
|
shooterSubsystem.outputBackboard();
|
|
}
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
}, shooterSubsystem);
|
|
}
|
|
|
|
// 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 void outputInfo(){
|
|
double currentConveyorSpeed = shooterSubsystem.conveyorMotor.getVelocity().getValueAsDouble();
|
|
double currentFlywheelSpeed = shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble();
|
|
double currentBackboardRate = shooterSubsystem.getBackboardPosition();
|
|
System.out.println("Conveyor: " + currentConveyorSpeed + "\t Flywheel: " + currentFlywheelSpeed + "\t Backboard:" + currentBackboardRate);
|
|
}
|
|
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;
|
|
}
|
|
} |