FTC 中常见的驱动控制模式​¶

// 假设我们已经初始化了电机和游戏手柄

// 获取左摇杆和右摇杆的值
double drive = -gamepad1.left_stick_y;  // 前后移动，取反是因为在FTC编程中，向前推摇杆通常返回负值。
double turn  = gamepad1.right_stick_x;  // 左右转向

// 计算电机功率
double leftPower = Range.clip(drive + turn, -1.0, 1.0);  // 左电机功率
double rightPower = Range.clip(drive - turn, -1.0, 1.0); // 右电机功率

// 设置电机功率
leftDrive.setPower(leftPower);
rightDrive.setPower(rightPower);

//DcMotor.Direction.FORWARD  // 正功率→正转，负功率→反转
//DcMotor.Direction.REVERSE  // 正功率→反转，负功率→正转
// 原来的设置
leftDrive.setDirection(DcMotor.Direction.REVERSE);   // 左电机反转
rightDrive.setDirection(DcMotor.Direction.FORWARD);  // 右电机正转

// 互换后的设置
leftDrive.setDirection(DcMotor.Direction.FORWARD);   // 左电机正转
rightDrive.setDirection(DcMotor.Direction.REVERSE);  // 右电机反转

// 1. 修改电机方向设置
leftDrive.setDirection(DcMotor.Direction.FORWARD);   // 改为FORWARD
rightDrive.setDirection(DcMotor.Direction.REVERSE);  // 改为REVERSE

// 2. 修改功率计算公式
// 原来的计算
leftPower = Range.clip(drive + turn, -1.0, 1.0);
rightPower = Range.clip(drive - turn, -1.0, 1.0);

// 需要改为
leftPower = Range.clip(drive - turn, -1.0, 1.0);    // + 改为 -
rightPower = Range.clip(drive + turn, -1.0, 1.0);   // - 改为 +

/**
 * 坦克驱动模式
 * 左摇杆控制左侧电机，右摇杆控制右侧电机
 */
public void tankDrive() {
    double leftPower = -gamepad1.left_stick_y;   // 左侧电机
    double rightPower = -gamepad1.right_stick_y; // 右侧电机

    // 设置电机功率
    leftDrive.setPower(leftPower);
    rightDrive.setPower(rightPower);
}

/**
 * 弧形驱动模式
 * 提供更平滑的转向控制
 */
public void arcadeDrive() {
    double drive = -gamepad1.left_stick_y;
    double turn = gamepad1.right_stick_x;

    // 使用平方函数使控制更精确
    drive = Math.copySign(drive * drive, drive);
    turn = Math.copySign(turn * turn, turn);

    double leftPower = Range.clip(drive + turn, -1.0, 1.0);
    double rightPower = Range.clip(drive - turn, -1.0, 1.0);

    leftDrive.setPower(leftPower);
    rightDrive.setPower(rightPower);
}

/**
 * 麦克纳姆轮驱动模式
 * 支持全方向移动
 */
public void mecanumDrive() {
    double drive = -gamepad1.left_stick_y;   // 前进/后退
    double strafe = gamepad1.left_stick_x;   // 左右平移
    double turn = gamepad1.right_stick_x;    // 转向

    // 计算每个轮子的功率
    double frontLeftPower = drive + strafe + turn;
    double frontRightPower = drive - strafe - turn;
    double backLeftPower = drive - strafe + turn;
    double backRightPower = drive + strafe - turn;

    // 归一化功率值
    double maxPower = Math.max(Math.abs(frontLeftPower),
                     Math.max(Math.abs(frontRightPower),
                     Math.max(Math.abs(backLeftPower),
                     Math.abs(backRightPower))));

    if (maxPower > 1.0) {
        frontLeftPower /= maxPower;
        frontRightPower /= maxPower;
        backLeftPower /= maxPower;
        backRightPower /= maxPower;
    }

    // 设置电机功率
    frontLeft.setPower(frontLeftPower);
    frontRight.setPower(frontRightPower);
    backLeft.setPower(backLeftPower);
    backRight.setPower(backRightPower);
}

/**
 * 速度控制模式
 * 提供不同的速度级别
 */
public class SpeedControl {
    private double speedMultiplier = 1.0;

    public void controlSpeed() {
        // 切换速度模式
        if (gamepad1.a) speedMultiplier = 1.0;    // 全速
        if (gamepad1.b) speedMultiplier = 0.5;    // 半速
        if (gamepad1.x) speedMultiplier = 0.25;   // 低速

        // 应用速度控制
        double drive = -gamepad1.left_stick_y * speedMultiplier;
        double turn = gamepad1.right_stick_x * speedMultiplier;

        // 设置电机功率...
    }
}

/**
 * 平滑加速控制
 * 防止突然的功率变化
 */
public class SmoothControl {
    private double currentPower = 0.0;
    private static final double ACCELERATION_RATE = 0.1;

    public double smoothPower(double targetPower) {
        // 逐渐调整当前功率到目标功率
        if (currentPower < targetPower) {
            currentPower = Math.min(currentPower + ACCELERATION_RATE, targetPower);
        } else if (currentPower > targetPower) {
            currentPower = Math.max(currentPower - ACCELERATION_RATE, targetPower);
        }
        return currentPower;
    }
}

/**
 * 直线驱动辅助
 * 使用陀螺仪保持直线行驶
 */
public class StraightDriveAssist {
    private IMU imu;
    private double targetHeading;

    public void driveStraight() {
        double drive = -gamepad1.left_stick_y;

        if (Math.abs(drive) > 0.1) {
            double currentHeading = imu.getAngularOrientation().firstAngle;
            double correction = (targetHeading - currentHeading) * 0.03;

            // 应用修正
            leftDrive.setPower(drive + correction);
            rightDrive.setPower(drive - correction);
        }
    }
}