陀螺仪(IMU)使用指南（IMU Guide）​¶

// 从硬件图获取IMU对象
IMU imu = hardwareMap.get(IMU.class, "imu");

// 设置IMU方向（告诉IMU控制中心在机器人上的安装方向）
RevHubOrientationOnRobot.LogoFacingDirection logoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP;
RevHubOrientationOnRobot.UsbFacingDirection usbDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
RevHubOrientationOnRobot orientation = new RevHubOrientationOnRobot(logoDirection, usbDirection);

// 用这个方向初始化IMU
imu.initialize(new IMU.Parameters(orientation));

// 获取机器人的偏航-俯仰-横滚角
YawPitchRollAngles robotOrientation = imu.getRobotYawPitchRollAngles();

// 读取各个角度值（以度为单位）
double yaw = robotOrientation.getYaw(AngleUnit.DEGREES);    // 偏航角/航向角
double pitch = robotOrientation.getPitch(AngleUnit.DEGREES); // 俯仰角
double roll = robotOrientation.getRoll(AngleUnit.DEGREES);   // 横滚角

// 显示在驾驶站上
telemetry.addData("偏航角", "%.1f度", yaw);
telemetry.addData("俯仰角", "%.1f度", pitch);
telemetry.addData("横滚角", "%.1f度", roll);
telemetry.update();

imu.resetYaw();  // 将当前偏航角重置为0度

// 获取角速度数据（以度/秒为单位）
AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES);

// 读取各轴的角速度
double yawVelocity = angularVelocity.zRotationRate;    // Z轴角速度
double pitchVelocity = angularVelocity.xRotationRate;  // X轴角速度
double rollVelocity = angularVelocity.yRotationRate;   // Y轴角速度

// 显示在驾驶站上
telemetry.addData("偏航角速度", "%.1f度/秒", yawVelocity);
telemetry.addData("俯仰角速度", "%.1f度/秒", pitchVelocity);
telemetry.addData("横滚角速度", "%.1f度/秒", rollVelocity);
telemetry.update();

// 定义目标角度（向右转90度）
double targetAngle = 90.0;
double currentAngle = 0;
double allowedError = 2.0;  // 允许2度的误差

// 重置航向角
imu.resetYaw();

// 开始转动
leftDrive.setPower(0.4);    // 左电机正转
rightDrive.setPower(-0.4);  // 右电机反转

// 持续检查角度直到转到目标角度
while (opModeIsActive() && Math.abs(currentAngle - targetAngle) > allowedError) {
    // 获取当前角度
    currentAngle = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);

    // 显示当前角度
    telemetry.addData("当前角度", "%.1f度", currentAngle);
    telemetry.addData("目标角度", "%.1f度", targetAngle);
    telemetry.addData("还差", "%.1f度", targetAngle - currentAngle);
    telemetry.update();
}

// 停止转动
leftDrive.setPower(0);
rightDrive.setPower(0);

// 先重置航向角作为参考
imu.resetYaw();

// 设置基础速度
double basePower = 0.5;
double maxCorrection = 0.2;  // 最大校正幅度

// 直线行驶
while (opModeIsActive()) {
    // 获取当前航向角
    double currentAngle = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);

    // 计算转向校正（偏离0度时进行调整）
    // 系数0.03表示每偏离1度，调整0.03的功率
    double correction = currentAngle * 0.03;

    // 限制校正的最大幅度
    correction = Math.max(-maxCorrection, Math.min(correction, maxCorrection));

    // 应用到电机上
    leftDrive.setPower(basePower - correction);
    rightDrive.setPower(basePower + correction);

    // 显示数据
    telemetry.addData("航向角", "%.1f度", currentAngle);
    telemetry.addData("校正值", "%.3f", correction);
    telemetry.update();
}

// 持续监测机器人姿态
while (opModeIsActive()) {
    // 获取俯仰角和横滚角
    YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles();
    double pitch = orientation.getPitch(AngleUnit.DEGREES);
    double roll = orientation.getRoll(AngleUnit.DEGREES);

    // 检查是否翻倒（俯仰角或横滚角超过45度）
    boolean isTippedOver = (Math.abs(pitch) > 45 || Math.abs(roll) > 45);

    if (isTippedOver) {
        // 机器人已翻倒，执行应急操作
        telemetry.addData("警告", "机器人已翻倒!");
        // 停止所有电机
        leftDrive.setPower(0);
        rightDrive.setPower(0);
    } else {
        // 正常操作
        telemetry.addData("状态", "正常");
    }

    telemetry.addData("俯仰角", "%.1f度", pitch);
    telemetry.addData("横滚角", "%.1f度", roll);
    telemetry.update();
}

// 计算从当前角度到目标角度的最短路径
double calculateShortestRotation(double currentAngle, double targetAngle) {
    double error = targetAngle - currentAngle;

    // 处理角度跨越±180度边界的情况
    if (error > 180) {
        error -= 360;
    } else if (error < -180) {
        error += 360;
    }

    return error;
}

// 使用三个旋转角度定义Hub方向
double xRotation = 15;  // 绕X轴旋转15度
double yRotation = 0;   // 绕Y轴旋转0度
double zRotation = 5;   // 绕Z轴旋转5度

// 创建方向对象
Orientation hubRotation = xyzOrientation(xRotation, yRotation, zRotation);
RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(hubRotation);

// 初始化IMU
imu.initialize(new IMU.Parameters(orientationOnRobot));