0227

加了passball状态等

src/main/java/frc/robot/Constants.java

@@ -8,6 +8,7 @@ public final class Constants {
     public enum RobotStatus{
         Stopped,
         AllTelop,
+        PassingBall,
         AutoAimming,
         Climbing,
         CrossingBump,
@@ -86,7 +87,9 @@ public final class Constants {
     public class Field {
         public static final double RedHubPositionX = 11.914;
         public static final double RedHubPositionY = 4.034;
-        
+        public static final double PassingBallPosX = 0;
+        public static final double PassingBallPosY1 = 0;
+        public static final double PassingBallPosY2 = 0;
     }
 
     public class Intaker {

src/main/java/frc/robot/RobotContainer.java

@@ -126,7 +126,6 @@ public class RobotContainer {
 
   private void configueShooter(){
     shooterSubsystem.setDefaultCommand(shooterControls.defaultShooterCommand());
-
     controller.povLeft().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(-1)));
     controller.povRight().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(1)));
     controller.povUp().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(100)));

src/main/java/frc/robot/controls/DriveControls.java

@@ -122,6 +122,11 @@ public class DriveControls {
                 vy = 0;
                 vomega = 0;
                 break;
+            case PassingBall:
+                vomega = calculateRotationSpeedFromRotationAngle(calculateDistanceAndRotationToPassBall()[1]);
+                autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
+                break;
+
             case AutoAimming:
                 vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
                 autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
@@ -288,6 +293,36 @@ public class DriveControls {
         // 返回距离和角度差
         return distanceAndRotation;
     }
+    public double[] calculateDistanceAndRotationToPassBall() {
+        Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
+        double targetX = Constants.Field.PassingBallPosX;  // 目标 X 坐标
+        double targetY1 = Constants.Field.PassingBallPosY1;  // 目标 Y 坐标
+        double targetY2 = Constants.Field.PassingBallPosY2;  // 目标 Y 坐标
+       
+
+        // 计算目标角度（相对于场地坐标系）
+        double deltaX = targetX - currentPose.getX();
+        double deltaY = calculateDifferenceToTwoTarget(currentPose.getY(), targetY1, targetY2);
+        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) {

src/main/java/frc/robot/controls/ShooterControls.java

@@ -7,6 +7,8 @@ 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 {
@@ -16,6 +18,9 @@ public class ShooterControls {
     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;
@@ -31,10 +36,36 @@ public class ShooterControls {
                     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:
-                    double flywheelSpeed = 0;
-                    double backboardPosition = 0;
-                    double conveyorSpeed = 0;
+                
                     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;
@@ -46,6 +77,9 @@ public class ShooterControls {
                         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);

src/main/java/frc/robot/subsystems/ShooterSubsystem.java

@@ -140,6 +140,12 @@ public class ShooterSubsystem extends SubsystemBase {
     public boolean isBackboardAtTarget() {
         return backboardPID.atGoal();  // 判断背板是否到达目标角度
     }
+    public void conveyorWaitForAcceleration(){
+        conveyorMotor.set(0);
+    }
+    public void conveyorRun(){
+        setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeed);
+    }
 
     @Override
     public void periodic() {