第３弾 ラズパイAIロボット製作（Phase1）

2025.11.25

過去に製作したArudinoラジコンカーをベース（ご参考）に2モータ差動駆動（両輪が独立して回る方式）で動くようにしました。まだ時々暴走する不安定さもありますが、くるくるとその場で回転するピボット動作は、よりロボット感を感じます。
見た目はこんな感じ、固定テープはご愛敬ってことで。いずれ3Dプリンタでホルダー作ろうと思います。

ハードウェア構成概要（2ESC 差動駆動版）

  • マイコン：Arduino Uno（ATmega328P）
  • 無線：nRF24L01（2.4GHz RC 通信用）
  • 駆動系：ブラシ付きモータ用 ESC × 2（左右独立・差動駆動）
  • カメラ用ヘッドトラッキング：サーボモータ × 2（Yaw／Pitch）
  • カメラ映像：Raspberry Pi + Picamera2

ピンアサイン一覧（Arduino Uno）

1. 無線モジュール（nRF24L01）

機能Arduino ピン備考
CED7RF24 CE
CSND8RF24 CSN（チップセレクト）
MOSID11ハードウェア SPI
MISOD12ハードウェア SPI
SCKD13ハードウェア SPI

2. サーボ／ESC 関連

機能Arduino ピン備考
ステアリングサーボD3フロント舵角制御（未使用）
カメラ Yaw サーボD9ヘッドトラッキング用
カメラ Pitch サーボD5ヘッドトラッキング用
左モータ用 ESC（ESC_L）D4差動駆動・左車輪用 ESC 信号
右モータ用 ESC（ESC_R）D2差動駆動・右車輪用 ESC 信号
  • D2／D4 は L298N を使わなくなったのでそのまま 2ESC 用に再利用。
  • ESC の信号は ServoTimer2 ライブラリで 50Hz（約 1000〜2000µs）パルスを生成。
  • Arduino の GND と各 ESC/BEC の GND は共通化する。

現行の「1ESC＋ステアリングサーボ版」では

  • D3：ステアリングサーボ
  • D6：ESC（1ch）
  • D2／D4：未使用

本 2ESC 版では

  • D3：ステアリングサーボ（そのまま残す）
  • D4：左 ESC
  • D2：右 ESC
  • D6：予備ピン（将来用）
    という構成になります。

3. Raspberry Pi との接続（将来）

I2C 接続

機能Arduino ピン備考
SDAA4I2C データ、現在は未接続（将来 Raspi 用に予約）
SCLA5I2C クロック、現在は未接続（将来 Raspi 用に予約）
  • スケッチ側ではすでに Wire.begin(0x08); として
    Arduino を I2C スレーブ（アドレス 0x08）として動かす構想。
  • 将来的に Raspberry Pi 側 I2C（GPIO2：SDA, GPIO3：SCL）と接続し、
    • Raspi からスロットル／ステアリング／モードなどを上書き制御する予定。

5. ハードウエアまとめ

  • マイコンは Arduino Uno。
  • 駆動系は 左右 2 個の ESC を D4（左）／D2（右）で差動駆動。
  • ステアリングサーボ（D3）、カメラサーボ（D9, D5）、nRF24（D7, D8, D11〜D13）は現行構成をそのまま継承。
  • 将来の自律走行に備え、Raspberry Pi との I2C 接続用に A4/A5 を予約。

ソフト仕様

  • マイコン：Arduino Uno
  • 走行制御：
    • ステアリングサーボは現時点では使わず、
      左右 2 個のブラシ付き ESC のみで差動（スキッドステア）制御を行う。
    • 左右 ESC の信号ピン：左 D4（ESC_L）、右 D2（ESC_R）。
  • 入力系：
    • nRF24L01 からのジョイスティック値を使用
      • X：ステアリング
      • Y：スロットル
    • 必要に応じて I2C スレーブ（アドレス 0x08）経由で X/Y を上書き可能（Raspi 連携を想定）。
  • モード制御（現仕様踏襲）：
    • start == 0：安全停止（両 ESC ニュートラル）
    • start >= 1：走行制御有効（2ESC 差動駆動）
    • start >= 2 && start <= 4：ヘッドトラッキング（カメラ Yaw/Pitch サーボ）も有効
    • モード切替は送信機のボタン（BTN_R）でトグル。
  • 差動ミキシング：
    • ジョイスティック Y（スロットル）と X（ステアリング）を
      • -1000 ～ +1000 の整数に正規化した T, S として扱う。
    • 通常時：
      • L = T + k_mix * S
      • R = T - k_mix * S
    • その場旋回（pivot）条件：
      • |T| が小さく、|S| が大きいときは
        L = +k_pivot * S, R = -k_pivot * S として左右逆回転させる。
  • デッドゾーン：
    • スティックの微小なブレを無視するため、
      • スティック値（T, S）に スティック側デッドゾーン を設定。
    • 差動後の L, R にも コマンド側デッドゾーン を設け、
      ニュートラル付近でローバが動きにくくなるようにする（実車調整前提）。
  • ESC 出力：
    • 左右コマンド L, R（-1000～+1000）を
      • ESC_MIN_US = 1000（フル後退）
      • ESC_NEU_US = 1500（ニュートラル）
      • ESC_MAX_US = 2000（フル前進）
        に線形マッピングして ServoTimer2 でパルス出力。
    • 将来調整用に左右別ニュートラル値 ESC_L_NEU_US / ESC_R_NEU_US を持てる設計。
  • スルーレート（Slew Rate）：
    • 左右 ESC それぞれについて
      • 加速方向はゆっくり（STEP_ACCEL）
      • 減速・ニュートラル側・逆方向は素早く（STEP_BRAKE）
        というスルーレート制御をかける。
    • 前進／後退とも同じパラメータで、対称な応答特性とする。
  • ヘッドトラッキング：
    • モード start >= 2 のとき、
      無線から受け取った yaw / pitch を用いて
      カメラサーボ（Yaw: D9, Pitch: D5）を制御する仕様
  • 今後の検討課題（今回のコードには未実装）：
    • RF ロスト時のフェールセーフ（一定時間受信なしで自動停止）
    • ESC キャリブレーションモード
    • ニュートラル微調整 GUI／コマンド
    • ステアリングサーボとの併用によるハイブリッドステアリング

Arduinoソースコード（現状版）

ときどき、暴走するのでその対処が必要。
あと、ヘッドトラッキングしながらの操縦が結構クイックに動くため難しい。とくにピボット動作を繰り返すとカメラが追従せずに自分の位置が分からなくなるという課題がある。（要検討）

/*
  RC Rover Receiver (2ESC Differential Drive Version)
  ---------------------------------------------------
  - MCU:  Arduino Uno (ATmega328P)
  - Radio: nRF24L01 (TMRh20 RF24)
  - Packet: int16_t[6] = {X, Y, BTN_R, YAW, PITCH, BTN_L}
  - Drive:  Brushed ESC x2 (Left: D4, Right: D2) via ServoTimer2
  - Camera: ServoTimer2 on pins 9 (Yaw), 5 (Pitch)
  - I2C:   Reserved for future use (slave @ 0x08). Currently not used.

  Notes:
  * 2ESC による差動（スキッドステア）制御。
  * ステアリングサーボは現時点では使用しない（将来用にコードは一部残す）。
*/

#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <Wire.h>
#include <ServoTimer2.h>

// ---------------------- RF24 ----------------------
RF24 radio(7, 8);                       // CE, CSN
static const uint64_t PIPE_RX = 0xF0F0F0E1LL; // keep as-is (matches your TX)

// ---------------------- Packet layout ------------
enum {
  IDX_X = 0,     // Joystick X (steering)
  IDX_Y = 1,     // Joystick Y (throttle)
  IDX_BTN_R = 2, // Right button (1=pressed)
  IDX_YAW = 3,   // Yaw (deg, int16)
  IDX_PITCH = 4, // Pitch (deg, int16)
  IDX_BTN_L = 5  // Left button (1=pressed)
};

int16_t joystickData[6] = {0};

// Current control values (I2C may override in future)
volatile int16_t joystickXValue = 0;
volatile int16_t joystickYValue = 0;
int16_t yaw   = 0;
int16_t pitch = 0;

int joystickSWValue    = 0;
int joystickSWValueLft = 1;

// ---------------------- Start state ---------------
volatile int startH = 1;
volatile int startL = 0;
volatile int start  = 0;

// ---------------------- Servos --------------------
// ステアリングサーボ（現時点では未使用だが、将来復活用に残す）
ServoTimer2 myservo; // steering (unused for now)

// Camera servos
ServoTimer2 servoZ;  // camera yaw
ServoTimer2 servoY;  // camera pitch

// 2ESC (Left / Right)
ServoTimer2 escL;    // Left ESC
ServoTimer2 escR;    // Right ESC

// Pins
const int ESC_L_PIN = 4;    // 左 ESC 信号
const int ESC_R_PIN = 2;    // 右 ESC 信号
const int SEVO_pin  = 3;    // ステアリングサーボ（未使用）

int camera_servoZPin = 9;   // yaw
int camera_servoYPin = 5;   // pitch

// ---------------------- ESC pulse spec -----------
const int ESC_MIN_US    = 1000;  // フル後退
const int ESC_MAX_US    = 2000;  // フル前進
const int ESC_L_NEU_US  = 1500;  // 左 ESC ニュートラル（調整用に分離）
const int ESC_R_NEU_US  = 1500;  // 右 ESC ニュートラル

// ---------------------- I2C (future use) ----------
volatile int16_t receivedDatai2c = 520;
volatile int16_t receivedCmndi2c = 5;

// ---------------------- Diff drive params --------
// ジョイスティック値を -1000 ～ +1000 に正規化して扱う
const int16_t JOY_MIN     = 0;
const int16_t JOY_MAX     = 1023;
const int16_t CMD_MIN     = -1000;
const int16_t CMD_MAX     = 1000;

// デッドゾーン
const int16_t STICK_DEAD  = 50;   // スティック側デッドゾーン（約5%）
const int16_t CMD_DEAD    = 50;   // コマンド側デッドゾーン

// その場旋回条件
const int16_t TH_PIVOT_T  = 200;  // |T| < 200 なら T ≒ 0 とみなす
const int16_t TH_PIVOT_S  = 300;  // |S| >= 300 でその場旋回発動

// ミキシング係数（整数で管理： /10 して使う）
const int16_t K_MIX_NUM   = 5;    // 0.5 相当
const int16_t K_PIVOT_NUM = 10;   // 1.0 相当
const int16_t K_DEN       = 10;

// スルーレート
const int STEP_ACCEL = 8;    // 加速時（パワー増加）はゆっくり
const int STEP_BRAKE = 80;   // 減速・ニュートラル側・逆方向は素早く

// スルーレート用の前回値
int lastUsL = ESC_L_NEU_US;
int lastUsR = ESC_R_NEU_US;

// ---------------------- Utility functions --------
int16_t clampCmd(int16_t v) {
  if (v > CMD_MAX)  return CMD_MAX;
  if (v < CMD_MIN)  return CMD_MIN;
  return v;
}

int16_t applyDeadZone(int16_t v, int16_t dead) {
  if (abs(v) < dead) return 0;
  return v;
}

// コマンド（-1000～+1000）を ESC パルス幅に変換
int cmdToPulse(int16_t cmd, int neu_us) {
  if (cmd == 0) return neu_us;

  long out = neu_us;
  if (cmd > 0) {
    // 前進方向：neu ～ MAX
    long range = (long)ESC_MAX_US - (long)neu_us;
    out = neu_us + (range * cmd) / CMD_MAX;
  } else {
    // 後退方向：MIN ～ neu
    long range = (long)neu_us - (long)ESC_MIN_US;
    out = neu_us + (range * cmd) / CMD_MAX; // cmd は負なので neu から下がる
  }

  if (out < ESC_MIN_US) out = ESC_MIN_US;
  if (out > ESC_MAX_US) out = ESC_MAX_US;
  return (int)out;
}

// スルーレート適用：targetUs へ lastUs から追従する
int applySlew(int targetUs, int lastUs) {
  int lastDiff   = lastUs   - ESC_L_NEU_US; // NEU からの距離（前回）
  int targetDiff = targetUs - ESC_L_NEU_US; // NEU からの距離（今回）

  // ほぼニュートラル
  if (abs(targetDiff) < 5) {
    return ESC_L_NEU_US;
  }

  // アクセルを抜く／弱める／逆方向・ニュートラルに向かう → BRAKE で素早く追従
  if ((lastDiff > 0 && targetDiff <= lastDiff) ||   // 前進を戻す方向
      (lastDiff < 0 && targetDiff >= lastDiff) ||   // 後退を戻す方向
      (lastDiff == 0 && targetDiff != 0) ||         // ニュートラルから踏む
      (lastDiff > 0 && targetDiff < 0) ||           // 前→後へ
      (lastDiff < 0 && targetDiff > 0)) {           // 後→前へ

    if      (targetUs > lastUs + STEP_BRAKE) lastUs += STEP_BRAKE;
    else if (targetUs < lastUs - STEP_BRAKE) lastUs -= STEP_BRAKE;
    else                                     lastUs  = targetUs;

    return lastUs;
  }

  // アクセルを踏み増し（同方向で増加） → ACCEL でゆっくり追従
  if (targetUs > lastUs + STEP_ACCEL)      lastUs += STEP_ACCEL;
  else if (targetUs < lastUs - STEP_ACCEL) lastUs -= STEP_ACCEL;
  else                                     lastUs  = targetUs;

  return lastUs;
}

// ---------------------- I2C receive --------------
void receiveEvent(int howMany) {
  // Enter override mode
  start = 5;

  // Expect cmd + 2-byte data per frame
  while (Wire.available() >= 3) {
    uint8_t cmd       = Wire.read();
    uint8_t high_byte = Wire.read();
    uint8_t low_byte  = Wire.read();

    receivedCmndi2c = cmd;
    receivedDatai2c = (int16_t)((high_byte << 8) | low_byte);

    // Override X or Y depending on cmd (kept behavior)
    if (cmd == 3 || cmd == 4 || cmd == 6) {
      joystickXValue = receivedDatai2c;
    }
    if (cmd == 1 || cmd == 2 || cmd == 5) {
      joystickYValue = receivedDatai2c;
    }
  }
  // Avoid Serial prints inside ISR for stability
}

void setup() {
  Serial.begin(9600);

  // -------- RF24 --------
  radio.begin();
  radio.openReadingPipe(1, PIPE_RX);
  radio.setPALevel(RF24_PA_HIGH);
  radio.setDataRate(RF24_1MBPS);
  radio.startListening();

  // -------- Servos (Camera only, steering unused) ------------
  // myservo.attach(SEVO_pin); // ステアリングサーボは現時点で未使用
  servoZ.attach(camera_servoZPin);
  servoY.attach(camera_servoYPin);

  // -------- I2C (future) --------------------------
  Wire.begin(0x08);               // I2C slave address
  Wire.onReceive(receiveEvent);   // Registered ISR

  // -------- ESC init (2ESC) -----------------------
  escL.attach(ESC_L_PIN);
  escR.attach(ESC_R_PIN);

  escL.write(ESC_L_NEU_US);
  escR.write(ESC_R_NEU_US);

  lastUsL = ESC_L_NEU_US;
  lastUsR = ESC_R_NEU_US;

  delay(2000); // ESC アーム待ち（ESC によって調整）
}

void loop() {
  // -------- Read RF24 (latest frame wins) --------
  if (radio.available()) {
    while (radio.available()) {
      radio.read(joystickData, sizeof(joystickData)); 
    }

    joystickXValue     = joystickData[IDX_X];
    joystickYValue     = joystickData[IDX_Y];
    joystickSWValue    = joystickData[IDX_BTN_R];
    joystickSWValueLft = joystickData[IDX_BTN_L];
    yaw                = joystickData[IDX_YAW];
    pitch              = joystickData[IDX_PITCH];

    if (joystickSWValue == 1) {
      startH = 1;
    }
    if (startH == 1 && joystickSWValue == 0) {
      startH = 0;
      start += 1;
      if (start > 3) start = 0;
    }
  }

  // -------- Drive control --------
  if (start >= 1) {
    int localX = joystickXValue;
    int localY = joystickYValue;

    // --- Joystick -> -1000..+1000 (T: throttle, S: steering) ---
    int16_t T = map(localY, JOY_MIN, JOY_MAX, CMD_MIN, CMD_MAX);
    int16_t S = map(localX, JOY_MIN, JOY_MAX, CMD_MIN, CMD_MAX);

    // Stick dead zone
    T = applyDeadZone(T, STICK_DEAD);
    S = applyDeadZone(S, STICK_DEAD);

    // --- Differential mixing with pivot turn ---
    int16_t L_cmd = 0;
    int16_t R_cmd = 0;

    if (abs(T) < TH_PIVOT_T && abs(S) >= TH_PIVOT_S) {
      // その場旋回（Pivot）
      L_cmd = (K_PIVOT_NUM * S) / K_DEN;
      R_cmd = -(K_PIVOT_NUM * S) / K_DEN;
    } else {
      // 通常差動ミキシング
      int16_t mix = (K_MIX_NUM * S) / K_DEN;
      L_cmd = T + mix;
      R_cmd = T - mix;
    }

    // Clamp to -1000..+1000
    L_cmd = clampCmd(L_cmd);
    R_cmd = clampCmd(R_cmd);

    // Command side dead zone
    L_cmd = applyDeadZone(L_cmd, CMD_DEAD);
    R_cmd = applyDeadZone(R_cmd, CMD_DEAD);

    // --- Map to ESC pulse (us) ---
    int targetUsL = cmdToPulse(L_cmd, ESC_L_NEU_US);
    int targetUsR = cmdToPulse(R_cmd, ESC_R_NEU_US);

    // --- Slew rate limiting (independent for L/R) ---
    lastUsL = applySlew(targetUsL, lastUsL);
    lastUsR = applySlew(targetUsR, lastUsR);

    // --- Output to ESCs ---
    escL.write(lastUsL);
    escR.write(lastUsR);

    // --- Camera servos (unchanged, start=2..4) ---
    if (start >= 2 && start <= 4) {
      int servoZAngle = map((int)yaw,   -90, 90, 2500, 100); // keep as-is
      int servoYAngle = map((int)pitch, -90, 90, 2500, 100); // keep as-is
      servoZ.write(servoZAngle);
      servoY.write(servoYAngle);
    }

  } else {
    // start == 0: 安全停止（両 ESC ニュートラル）
    escL.write(ESC_L_NEU_US);
    escR.write(ESC_R_NEU_US);
    lastUsL = ESC_L_NEU_US;
    lastUsR = ESC_R_NEU_US;
  }

  delay(10);
}

タイトルとURLをコピーしました