ESP32-CAMのラジコン用汎用コード

概要

ESP32-CAMで作るラジコン用のコードを公開します。
主な機能はカメラの映像送信とコマンドの受信によるGPIO出力の制御です。
機能的には以上のため、汎用的に使えるようになっています。
GPIO出力をするため、SDカードは使用できません。

ひとまず完成
カメラ見ながら動かすの楽しい！！
カメラに合わせて画角を変えられるようにコントローラーを改修
なんとか手のひらサイズに収まった#esp32 #Kotlin #ラジコン#マイコン pic.twitter.com/TiCrpCU5v2

— 亀吉 (@by2aE1WOPoBdfLo) January 23, 2023

ESP32-CAMで作ったラジコンの回路図は以下の記事で書いてます。

コントローラ公開しました。

仕様

通信の仕様

図の通り、WiFiで接続して映像受信及びコマンドの送信を操作端末側で行います。
映像はESP32-CAM標準のカメラサーバによる発信、コマンドの通信はUDPにて行います。
下記コードはアドホック通信を行います。
GPIOの動作検証のためにシリアルからのコマンド送信も可能です。

コマンドの仕様

各GPIOの出力値を操作するためのコマンドです。
図の通り、コマンド全体を[]（角カッコ）で囲み、カンマ区切りで指定します。:の左がGPIOのピンNO、右が値になります。

GPIO出力の仕様

GPIO出力はPWM出力です。コマンドで指定できる値は0〜255になります。
コード上部で定義しているgpio_pins[]にて使用するGPIOを指定します。出力として指定していないGPIOに対するコマンドは無視されます。
また、ESP32-CAMの仕様上、GPIO4はLEDに接続されています。なお、0を出力するとGNDに接続されます。

下記コードではGPIOの2,4,12,13,14,15を初期化し、12,13を使わないようにしています。原因は不明ですが、12,13を初期化しないと14,15が使えない現象が発生しました。（ボードの個体差か？コードが悪いのか？不明。指定するPINの順番もコレでないとダメ。）

コード

以下が自作したコードになります。定義した関数の概要は以下の通りです。

void command_ditection(String d)

受信したコマンド文字列を引数に指定します。コマンドを解析し、gpio_output関数を呼び出します。

void gpio_output(String pins[], String values[], int cmd_count)

解析したコマンドを引数に指定することでGPIOの出力を行います。

int split(String data, char delimiter, String *dst)

他言語のsplit関数を実現しています。第1引数は文字列、第2引数に区切り文字、第3引数は分離後の文字列配列を格納するためのポインタを指定します。戻り値は分離後の要素数を返します。

ESP32-CAM-AP-UDP.ino

#include "esp_camera.h"
#include <WiFi.h>
#include "AsyncUDP.h"

int split(String data, char delimiter, String *dst);
void gpio_output(String pins[], String values[], int cmd_count);
void command_ditection(String d);

//
// WARNING!!! PSRAM IC required for UXGA resolution and high JPEG quality
//            Ensure ESP32 Wrover Module or other board with PSRAM is selected
//            Partial images will be transmitted if image exceeds buffer size
//

// Select camera model
//#define CAMERA_MODEL_WROVER_KIT // Has PSRAM
//#define CAMERA_MODEL_ESP_EYE // Has PSRAM
//#define CAMERA_MODEL_M5STACK_PSRAM // Has PSRAM
//#define CAMERA_MODEL_M5STACK_V2_PSRAM // M5Camera version B Has PSRAM
//#define CAMERA_MODEL_M5STACK_WIDE // Has PSRAM
//#define CAMERA_MODEL_M5STACK_ESP32CAM // No PSRAM
#define CAMERA_MODEL_AI_THINKER // Has PSRAM
//#define CAMERA_MODEL_TTGO_T_JOURNAL // No PSRAM

#include "camera_pins.h"

//const char* ssid = "*********";
//const char* password = "*********";

void startCameraServer();

//ESP32 SoftAP Configration
const char* ssid = "ESP32_Tester";
const char* pass = "password";
const IPAddress ip(192,168,0,100);
const IPAddress subnet(255,255,255,0);

// UDP通信
AsyncUDP udp;
const int RecvPort = 30000;

// GPIOピンの管理
// PINチャンネルとGPIOピンの関連付け
// IO4はフラッシュ
int gpio_pins[] = {12,13,15,14,2,4};
int pin_count = sizeof(gpio_pins) / sizeof(int);

void setup() {
  Serial.begin(115200);
  Serial.setDebugOutput(true);
  Serial.println();

  camera_config_t config;
  config.ledc_channel = LEDC_CHANNEL_0;
  config.ledc_timer = LEDC_TIMER_0;
  config.pin_d0 = Y2_GPIO_NUM;
  config.pin_d1 = Y3_GPIO_NUM;
  config.pin_d2 = Y4_GPIO_NUM;
  config.pin_d3 = Y5_GPIO_NUM;
  config.pin_d4 = Y6_GPIO_NUM;
  config.pin_d5 = Y7_GPIO_NUM;
  config.pin_d6 = Y8_GPIO_NUM;
  config.pin_d7 = Y9_GPIO_NUM;
  config.pin_xclk = XCLK_GPIO_NUM;
  config.pin_pclk = PCLK_GPIO_NUM;
  config.pin_vsync = VSYNC_GPIO_NUM;
  config.pin_href = HREF_GPIO_NUM;
  config.pin_sscb_sda = SIOD_GPIO_NUM;
  config.pin_sscb_scl = SIOC_GPIO_NUM;
  config.pin_pwdn = PWDN_GPIO_NUM;
  config.pin_reset = RESET_GPIO_NUM;
  config.xclk_freq_hz = 20000000;
  config.pixel_format = PIXFORMAT_JPEG;
  
  // if PSRAM IC present, init with UXGA resolution and higher JPEG quality
  //                      for larger pre-allocated frame buffer.
  if(psramFound()){
    config.frame_size = FRAMESIZE_UXGA;
    config.jpeg_quality = 10;
    config.fb_count = 2;
  } else {
    config.frame_size = FRAMESIZE_SVGA;
    config.jpeg_quality = 12;
    config.fb_count = 1;
  }

#if defined(CAMERA_MODEL_ESP_EYE)
  pinMode(13, INPUT_PULLUP);
  pinMode(14, INPUT_PULLUP);
#endif

  // GPIOの出力準備
  for(int i = 0; i < pin_count; i++){
    pinMode(gpio_pins[i], OUTPUT);
    ledcSetup(i, 12000, 8);
    ledcAttachPin(gpio_pins[i], i);
    if(gpio_pins[i] == 15 or gpio_pins[i] == 14 or gpio_pins[i] == 2 or gpio_pins[i] == 4){
      delay(50);
      ledcWrite(i, 0);      
    }
  }

  // camera init
  esp_err_t err = esp_camera_init(&config);
  if (err != ESP_OK) {
    Serial.printf("Camera init failed with error 0x%x", err);
    return;
  }

  sensor_t * s = esp_camera_sensor_get();
  // initial sensors are flipped vertically and colors are a bit saturated
  if (s->id.PID == OV3660_PID) {
    s->set_vflip(s, 1); // flip it back
    s->set_brightness(s, 1); // up the brightness just a bit
    s->set_saturation(s, -2); // lower the saturation
  }
  // drop down frame size for higher initial frame rate
  s->set_framesize(s, FRAMESIZE_QVGA);

#if defined(CAMERA_MODEL_M5STACK_WIDE) || defined(CAMERA_MODEL_M5STACK_ESP32CAM)
  s->set_vflip(s, 1);
  s->set_hmirror(s, 1);
#endif

//  WiFi.begin(ssid, password);
//
//  while (WiFi.status() != WL_CONNECTED) {
//    delay(500);
//    Serial.print(".");
//  }
//  Serial.println("");
//  Serial.println("WiFi connected");

  //アドホックモードの設定
  WiFi.softAP(ssid,pass);
  Serial.print("ssid:");
  Serial.print(ssid);
  Serial.print("/pass:");
  Serial.println(pass);
  delay(100);
  WiFi.softAPConfig(ip,ip,subnet);
  IPAddress myIP = WiFi.softAPIP();

  startCameraServer();

  Serial.print("Camera Ready! Use 'http://");
//  Serial.print(WiFi.localIP());
  Serial.print(myIP);
  Serial.println("' to connect");

  
  // UDPServer 受信待ち
  if(udp.listen(RecvPort)) {
        Serial.print("UDP Listening on Port: ");
        Serial.println(RecvPort);
        udp.onPacket([](AsyncUDPPacket packet) {
            Serial.print("UDP Packet Type: ");
            Serial.print(packet.isBroadcast()?"Broadcast":packet.isMulticast()?"Multicast":"Unicast");
            Serial.print(", From: ");
            Serial.print(packet.remoteIP());
            Serial.print(":");
            Serial.print(packet.remotePort());
            Serial.print(", To: ");
            Serial.print(packet.localIP());
            Serial.print(":");
            Serial.print(packet.localPort());
            Serial.print(", Length: ");
            Serial.print(packet.length());
            Serial.print(", Data: ");
            Serial.write(packet.data(), packet.length());
            Serial.println();
            //reply to the client
//            packet.printf("Got %u bytes of data", packet.length());
            // コマンド解析
            byte* bdata = packet.data();
            bdata[packet.length()] = '\0';
            String d = String((char*)bdata);
            //Serial.println("debug:" + d);
            command_ditection(d);
        });
    }
}

void loop() {
  // put your main code here, to run repeatedly:
  delay(100);

  if(Serial.available()){
    char s[255];
    int i = 0;
    while(Serial.available()){
      s[i] = Serial.read();
      i++;
    }
    s[i] = '\0';
    Serial.print("Serial read:");
    Serial.print(s);
    String d = String((char*)s);
    // コマンド解析及び実行
    command_ditection(d);
  }

  //Send broadcast
  //udp.broadcast("Anyone here?");
}

void command_ditection(String d){
  // []内のデータを取得
  String d2 = d.substring(d.indexOf("[") + 1, d.indexOf("]"));
  //Serial.println("debug:" + d2);
  // ,で分割
  String cmds[10] = {"\0"};
  String pins[10] = {"\0"};
  String values[10] = {"\0"};
  int count = split(d2, ',', cmds);
  // コマンド分処理
  int cmd_count = 0;
  for(int i = 0; i < count; i++){
    // コマンドのPIN_NOと値を取得
    String pp[2] = {"\0"};
    if((split(cmds[i], ':', pp)) != -1){
      pins[cmd_count] = pp[0];
      values[cmd_count] = pp[1];
      cmd_count++;
      //Serial.println("debug:cmd_" + String(cmd_count - 1) + " pin_" + pins[cmd_count - 1] + "/value_" + values[cmd_count - 1]);
    }
  }
  // GPIO出力
  gpio_output(pins, values, cmd_count);
}

void gpio_output(String pins[], String values[], int cmd_count){
  for(int i = 0; i < cmd_count; i++){
    for(int i2 = 0; i2 < pin_count; i2++){
      if(pins[i].toInt() == gpio_pins[i2]){
        ledcWrite(i2, values[i].toInt());
      }
    }
  }
}

int split(String data, char delimiter, String *dst){
    int index = 0;
    int arraySize = (sizeof(data)/sizeof((data)[0]));  
    int datalength = data.length();
    for (int i = 0; i < datalength; i++) {
        char tmp = data.charAt(i);
        if ( tmp == delimiter ) {
            index++;
            if ( index > (arraySize - 1)) return -1;
        }
        else dst[index] += tmp;
    }
    return (index + 1);
}