diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c906349..0da3842 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -88,9 +88,12 @@ 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 BlueHubPositionX = 0; + public static final double BlueHubPositionY = 0; public static final double PassingBallPosY1 = 2.51; public static final double PassingBallPosY2 = 5.556; + public static final double RedPassingBallPosX = 0; + public static final double BluePassingBallPosX = 0; } @@ -111,12 +114,12 @@ public final class Constants { .withKV(0.00).withKA(0.00).withKS(0.00); //如果 rotater 受重力影响明显,可能需要用 withKG/withGravityType/withGravityArmPositionOffset(看机构形式)(这句fromAI) - public static final double IntakeGetterSpeedforAuto = 0.0; + public static final double IntakeGetterSpeedforAuto = 0.4; // 单位RPS public static final double positionToleranceRotations = 0.00; //rotater容差 public static final double IntakeGetterK = 0.3; // intake getter速度与触发器输入的比例 - public static final double Getterminspeed = 0.2; // intake getter的基础速度(即触发器输入为0时的速度,正值表示默认向内转) + public static final double Getterminspeed = 0.3; // intake getter的基础速度(即触发器输入为0时的速度,正值表示默认向内转) } public static class Climber { diff --git a/src/main/java/frc/robot/controls/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java index cf3d087..1b8779f 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -26,6 +26,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; public class DriveControls { + public boolean isRedAlliance; private final CommandSwerveDrivetrain drivetrain; @@ -234,8 +235,8 @@ public class DriveControls { } public double[] calculateDistanceAndRotationToHub() { Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度 - double targetX = Constants.Field.RedHubPositionX; // 目标 X 坐标 - double targetY = Constants.Field.RedHubPositionY; // 目标 Y 坐标 + double targetX = isRedAlliance ? Constants.Field.RedHubPositionX : Constants.Field.BlueHubPositionX; + double targetY = isRedAlliance ? Constants.Field.RedHubPositionY : Constants.Field.BlueHubPositionY; // 计算目标角度(相对于场地坐标系) double deltaX = targetX - currentPose.getX(); @@ -262,7 +263,7 @@ public class DriveControls { } public double[] calculateDistanceAndRotationToPassBall() { Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度 - double targetX = Constants.Field.PassingBallPosX; // 目标 X 坐标 + double targetX = isRedAlliance ? Constants.Field.RedPassingBallPosX : Constants.Field.BluePassingBallPosX; // 目标 X 坐标 double targetY1 = Constants.Field.PassingBallPosY1; // 目标 Y 坐标 double targetY2 = Constants.Field.PassingBallPosY2; // 目标 Y 坐标 diff --git a/src/main/java/frc/robot/controls/IntakerControls.java b/src/main/java/frc/robot/controls/IntakerControls.java index b93de5b..e63b99d 100644 --- a/src/main/java/frc/robot/controls/IntakerControls.java +++ b/src/main/java/frc/robot/controls/IntakerControls.java @@ -76,9 +76,9 @@ public class IntakerControls { IntakerSubsystem.setIntakerGetterSpeed(getterspeed); if(controller.getLeftY() < -0.5){ - IntakerSubsystem.IntakerDownNonStop(); - } else if(controller.getLeftY() > 0.5){ IntakerSubsystem.IntakerUpNonStop(); + } else if(controller.getLeftY() > 0.5){ + IntakerSubsystem.IntakerDownNonStop(); } else { IntakerSubsystem.stopIntakerotater(); } diff --git a/src/main/java/frc/robot/subsystems/IntakerSubsystem.java b/src/main/java/frc/robot/subsystems/IntakerSubsystem.java index 603947b..9fd211f 100644 --- a/src/main/java/frc/robot/subsystems/IntakerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakerSubsystem.java @@ -83,26 +83,9 @@ public class IntakerSubsystem extends SubsystemBase{ stopIntakerotater(); } public void IntakerDownNonStop(){ - double currentPosition = getIntakerotaterPosition(); - if (currentPosition > Constants.Intaker.intakeRotaterDownLimit) { - - intakerotater.set(-0.05);} - // double currentPosition = getIntakerotaterPosition(); - // if (currentPosition > Constants.Intaker.intakeRotaterDownLimit) { - // intakerotater.setControl(velocityRequest.withVelocity(Constants.Intaker.intakeRotaterVelocity)); - // } else { - // stopIntakerotater(); - // } - } + intakerotater.set(-0.05); + } public void IntakerUpNonStop(){ - double currentPosition = getIntakerotaterPosition(); - if (currentPosition < Constants.Intaker.intakeRotaterUpLimit) { - intakerotater.set(0.1);} - - // if (currentPosition < Constants.Intaker.intakeRotaterUpLimit) { - // intakerotater.setControl(velocityRequest.withVelocity(-Constants.Intaker.intakeRotaterVelocity)); - // } else { - // stopIntakerotater(); - // } + intakerotater.set(0.2); } }