ジャイロセンサーと2輪ロボットの回転角を使って2輪ロボットを指定した角度まで回転を行うプログラム

プログラムの概要は、以下のとおりです。

1. ジャイロセンサーから回転角を取得する
2. タイヤの回転角を取得する
3. 目標の回転角と現在の回転角から、回転量を計算する
4. 左右の車輪にモーターを制御して、回転する

具体的なプログラムは、以下のとおりです。

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

// ジャイロセンサーの誤差
#define GYRO_ERROR 0.5
// タイヤの回転角の誤差
#define WHEEL_ERROR 1

// ジャイロセンサーから回転角を取得する
double get_gyro_angle() {
  // ジャイロセンサーの回転角を取得する
  double angle = gyro_sensor.read();
  // ジャイロセンサーの誤差を補正する
  angle += GYRO_ERROR;
  return angle;
}

// タイヤの回転角を取得する
double get_wheel_angle() {
  // タイヤの回転角を取得する
  double angle = wheel_sensor.read();
  // タイヤの回転角の誤差を補正する
  angle += WHEEL_ERROR;
  return angle;
}

// 目標の回転角と現在の回転角から、回転量を計算する
double calculate_turn_angle(double target_angle, double current_angle) {
  // 目標の回転角と現在の回転角の差を計算する
  double diff = target_angle - current_angle;
  // 回転量を計算する
  double turn_angle = diff * (M_PI / 180);
  return turn_angle;
}

// 左右の車輪にモーターを制御して、回転する
void turn(double turn_angle) {
  // 目標の回転角を設定
  target_angle = turn_angle;
  // 現在の回転角を取得する
  current_angle = get_gyro_angle();
  // 回転量を計算する
  turn_angle = calculate_turn_angle(target_angle, current_angle);
  // 左右の車輪にモーターを制御して、回転する
  left_wheel.set_speed(turn_angle);
  right_wheel.set_speed(-turn_angle);
}

int main() {
  // 初期化
  gyro_sensor.initialize();
  wheel_sensor.initialize();

  // 目標の回転角を設定
  target_angle = 90;

  // 回転開始
  turn(target_angle);

  // 回転終了待ち
  while (1) {
    if (get_gyro_angle() == target_angle) {
      break;
    }
  }

  // 終了
  return 0;
}

このプログラムを実行すると、2輪ロボットは指定した角度まで回転します。