Go to Qiita Advent Calendar Top
LoginSignup
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

@Mobu_Kyoto

71歳の挑戦... ロボットカーのカメラをピッチ角だけでなく、ヨー角も変化させられるよう改造

Last updated at Posted at 2025-01-19

　アマゾン等で売られている「raspberry Pi」や「Arduino」を応用したおもちゃに、2軸動作のサーボマウントカメラがついてたりします。私もそれに倣い、以前投稿した
https://qiita.com/Mobu_Kyoto/items/a990cc89603a6dc18265
ウェブリモコンカーに、カメラ横スィープ機能を付け加えようと思いました。そこで、2軸サーボマウントを購入してみたのですが、実動作範囲（角度範囲ではなく）が大きく、私所有のケーブル長75mmのカメラでは届かず動作が苦しい状況でした。なので、色々考えた挙句次のようにコンパクトな機構にし車体前部へ取り付けました。

Sweep1.jpg
タミヤのユニバーサル金具と両面テープを用い出来る限りシンプルにし、カメラコネクター基部前方かつ最も近い場所に回転軸が来るように調整しました。サーボは定番の「Tower Pro Micro Servo SG90」です。
　ESP32S3ボードを立ち上げシリアルモニターに示される「192.168.0.xx」にアクセスすれば 、

camera_sweep.jpg
が現れます。最初はカメラ画像オフにしてますが、「CAMERA START」で表示されます。カメラ初期角度は正面90度水平0度です。「スライダー」で左右に60度から120度の範囲でスィープし、「Camera_UP」「Camera_DOWN」で仰角0度から45度まで変化します。つぎに 「Arduino IDE 2.3.2」でのスケッチを示します。

ESP32S3_Cam_WebControllCar_2.ino
#include "Arduino.h"
#include <WiFi.h>
#include "esp_camera.h"
#include "esp_http_server.h"
#include "esp_timer.h"
#include "img_converters.h"
#include "fb_gfx.h"
#include <ESP32Servo.h>

// Camera Pin Config for "ESP32 Wrover Module"
#define PWDN_GPIO_NUM -1
#define RESET_GPIO_NUM -1
#define XCLK_GPIO_NUM 15
#define SIOD_GPIO_NUM 4
#define SIOC_GPIO_NUM 5
#define Y2_GPIO_NUM 11
#define Y3_GPIO_NUM 9
#define Y4_GPIO_NUM 8
#define Y5_GPIO_NUM 10
#define Y6_GPIO_NUM 12
#define Y7_GPIO_NUM 18
#define Y8_GPIO_NUM 17
#define Y9_GPIO_NUM 16
#define VSYNC_GPIO_NUM 6
#define HREF_GPIO_NUM 7
#define PCLK_GPIO_NUM 13

// Motor Pins
#define MOTOR_1_PIN_1 19
#define MOTOR_1_PIN_2 20
#define MOTOR_2_PIN_1 21
#define MOTOR_2_PIN_2 47

// Replace with your network credentials
const char* ssid = "**************";
const char* password = "*************";

Servo myservo0;  // create servo object to control a servo
const int servoPin0 = 48;
const int pos00 = 0;   // input min in HTML
const int pos0e = 45;  // input max in HTML
int pos0;              // variable to store the servo position
Servo myservo1;        // create servo object to control a servo
const int servoPin1 = 45;
const int pos10 = 60;   // input min in HTML
const int pos1e = 120;  // input max in HTML
int pos1;               // variable to store the servo position

// LED
const int LedPIN = 2;
// setting PWM properties
int bright = 8;
const int freq = 5000;
const int ledChannel = 2;
const int reso = 8;

// Horn
const int HornPIN = 42;
// setting PWM properties
const int loud = 516;
const int Hornfreq = 432;
const int ledChannelH = 3;
const int Hornreso = 10;

#define PART_BOUNDARY "123456789000000000000987654321"
static const char* _STREAM_CONTENT_TYPE = "multipart/x-mixed-replace;boundary=" PART_BOUNDARY;
static const char* _STREAM_BOUNDARY = "\r\n--" PART_BOUNDARY "\r\n";
static const char* _STREAM_PART = "Content-Type: image/jpeg\r\nContent-Length: %u\r\n\r\n";

httpd_handle_t camera_httpd = NULL;
httpd_handle_t stream_httpd = NULL;

static const char PROGMEM INDEX_HTML[] =
  R"rawliteral(
<html>
  <head>
    <title>ESP32-S3_CAM Car</title>
    <meta name="viewport" content="width=device-width, initial-scale=1">
    <style>
      body { font-family: Arial; text-align: center; margin:0px auto; padding-top: 30px;}
      table { margin-left: auto; margin-right: auto; }
      td { padding: 8 px; }
      img {  width: auto ;
        max-width: 100% ;
        height: auto ;
      }
      .button1 {
        background-color: #bf94e4;
        border: none;
        color: black;
        padding: 10px 20px;
        text-align: center;
        text-decoration: none;
        display: inline-block;
        font-size: 18px;
        margin: 6px 3px;
        cursor: pointer;
        -webkit-touch-callout: none;
        -webkit-user-select: none;
        -khtml-user-select: none;
        -moz-user-select: none;
        -ms-user-select: none;
        user-select: none;
        -webkit-tap-highlight-color: rgba(0,0,0,0);
      }
      .button2 {
        background-color: #2f4468;
        border: none;
        color: white;
        padding: 10px 20px;
        text-align: center;
        text-decoration: none;
        display: inline-block;
        font-size: 18px;
        margin: 6px 3px;
        cursor: pointer;
        -webkit-touch-callout: none;
        -webkit-user-select: none;
        -khtml-user-select: none;
        -moz-user-select: none;
        -ms-user-select: none;
        user-select: none;
        -webkit-tap-highlight-color: rgba(0,0,0,0);
      }
    </style>
  </head>
  <body>
    <h1>ESP32-S3-CAM Car</h1>
    <table>
      <tr>
        <td colspan="3" align="center">
          <img id="stream" src="" >
        </td>
      </tr>
      <tr>
        <td colspan="3" align="center">
          <button class="button1"
            id="toggle-stream">CAMERA START
          </button>
        </td>
      </tr>
      <tr>
        <td>
          <input type="range" id="camSweep" min="60" max="120" value="90" />
        </td>
        <td>
          <p class="log">Camera Sweep Angle(L -- R) = 90</p>
        </td>
      </tr>
      <tr>
        <td align="center">
          <button class="button1"
            onmousedown ="toggleCheckbox('cam-up');"
            ontouchstart="toggleCheckbox('cam-up');">Camera_UP
          </button>
        </td>
        <td align="center">
          <button class="button1"
            onmousedown ="toggleCheckbox('cam-dn');"
            ontouchstart="toggleCheckbox('cam-dn');">Camera_DOWN
          </button>
        </td>
      </tr>

      <tr>
        <td align="center">
          <button class="button2"
            onmousedown ="toggleCheckbox('led-up');"
            ontouchstart="toggleCheckbox('led-up');">Led_LIGHTER
          </button>
        </td>
        <td align="center">
          <button class="button2"
            onmousedown ="toggleCheckbox('led-dn');"
            ontouchstart="toggleCheckbox('led-dn');">Led_DARKER
          </button>
        </td>
        <td align="center">
          <button class="button2"
            onmousedown ="toggleCheckbox('horn');"
            onmouseup   ="toggleCheckbox('hstop');"
            ontouchstart="toggleCheckbox('horn');"
            ontouchend  ="toggleCheckbox('hstop');">Horn
          </button>
        </td>
      </tr>

      <tr>
        <td colspan="3" align="center">
          <button class="button2"
            onmousedown ="toggleCheckbox('forward');"
            onmouseup   ="toggleCheckbox('stop');"
            ontouchstart="toggleCheckbox('forward');"
            ontouchend  ="toggleCheckbox('stop');">Forward
          </button>
        </td>
      </tr>
      <tr>
        <td align="center">
          <button class="button2"
            onmousedown ="toggleCheckbox('left');"
            onmouseup   ="toggleCheckbox('stop');"
            ontouchstart="toggleCheckbox('left');"
            ontouchend  ="toggleCheckbox('stop');">Turn Left
          </button>
        </td>
        <td align="center">
          <button class="button2"
            onmousedown ="toggleCheckbox('stop');"
            ontouchstart="toggleCheckbox('stop');">Stop
          </button>
        </td>
        <td align="center">
          <button class="button2"
            onmousedown ="toggleCheckbox('right');"
            onmouseup   ="toggleCheckbox('stop');"
            ontouchstart="toggleCheckbox('right');"
            ontouchend  ="toggleCheckbox('stop');">Turn Right
          </button>
        </td>
      </tr>
      <tr>
        <td  colspan="3" align="center">
          <button class="button2"
            onmousedown ="toggleCheckbox('backward');"
            onmouseup   ="toggleCheckbox('stop');"
            ontouchstart="toggleCheckbox('backward');"
            ontouchend  ="toggleCheckbox('stop');">Backward
          </button>
        </td>
      </tr>
    </table>
    <script>
      const elementSV=document.querySelector('#camSweep');
      elementSV.addEventListener('input', handleChange);
      function handleChange(event){
        const value=event.target.value;
        document.querySelector('.log').innerHTML=`Camera Sweep Angle(L -- R) = ${value}`;

        var xhr = new XMLHttpRequest();
        xhr.open("GET", "/action?go=" + value, true);
        xhr.send();
      }

      function toggleCheckbox(x) {
        var xhr = new XMLHttpRequest();
        xhr.open("GET", "/action?go=" + x, true);
        xhr.send();
      }

      const streamButton = document.getElementById('toggle-stream')
      streamButton.addEventListener("click",
        function(){
          const view = document.getElementById('stream')
          var baseHost = document.location.origin
          var streamUrl = baseHost + ':81'

          const streamEnabled = streamButton.innerHTML === 'CAMERA STOP'
          if(streamEnabled) {
            window.stop();
            streamButton.innerHTML = 'CAMERA START'
          } else {
            view.src = `${streamUrl}/stream`
            streamButton.innerHTML = 'CAMERA STOP'
          }
        }
      );       
      
    </script>
  </body>
</html>
)rawliteral";

static esp_err_t index_handler(httpd_req_t* req) {
  httpd_resp_set_type(req, "text/html");
  return httpd_resp_send(req, (const char*)INDEX_HTML, strlen(INDEX_HTML));
}

static esp_err_t stream_handler(httpd_req_t* req) {
  camera_fb_t* fb = NULL;
  esp_err_t res = ESP_OK;
  size_t _jpg_buf_len = 0;
  uint8_t* _jpg_buf = NULL;
  char* part_buf[64];

  res = httpd_resp_set_type(req, _STREAM_CONTENT_TYPE);
  if (res != ESP_OK) {
    return res;
  }

  while (true) {
    fb = esp_camera_fb_get();
    if (!fb) {
      Serial.println("Camera capture failed");
      res = ESP_FAIL;
    } else {
      if (fb->format != PIXFORMAT_JPEG) {
        bool jpeg_converted = frame2jpg(fb, 80, &_jpg_buf, &_jpg_buf_len);
        esp_camera_fb_return(fb);
        fb = NULL;
        if (!jpeg_converted) {
          Serial.println("JPEG compression failed");
          res = ESP_FAIL;
        }
      } else {
        _jpg_buf = fb->buf;
        _jpg_buf_len = fb->len;
      }
    }
    if (res == ESP_OK) {
      size_t hlen = snprintf((char*)part_buf, 64, _STREAM_PART, _jpg_buf_len);
      res = httpd_resp_send_chunk(req, (const char*)part_buf, hlen);
    }
    if (res == ESP_OK) {
      res = httpd_resp_send_chunk(req, (const char*)_jpg_buf, _jpg_buf_len);
    }
    if (res == ESP_OK) {
      res = httpd_resp_send_chunk(req, _STREAM_BOUNDARY, strlen(_STREAM_BOUNDARY));
    }
    if (fb) {
      esp_camera_fb_return(fb);
      fb = NULL;
      _jpg_buf = NULL;
    } else if (_jpg_buf) {
      free(_jpg_buf);
      _jpg_buf = NULL;
    }
    if (res != ESP_OK) {
      break;
    }
    Serial.printf("MJPG: %uB\n", (uint32_t)(_jpg_buf_len));
  }
  return res;
}

static esp_err_t cmd_handler(httpd_req_t* req) {
  char* buf;
  size_t buf_len;
  char variable[32] = {
    0,
  };

  buf_len = httpd_req_get_url_query_len(req) + 1;
  if (buf_len > 1) {
    buf = (char*)malloc(buf_len);
    if (!buf) {
      httpd_resp_send_500(req);
      return ESP_FAIL;
    }
    if (httpd_req_get_url_query_str(req, buf, buf_len) == ESP_OK) {
      if (httpd_query_key_value(buf, "go", variable, sizeof(variable)) == ESP_OK) {
        // go on
      } else {
        free(buf);
        httpd_resp_send_404(req);
        return ESP_FAIL;
      }
    } else {
      free(buf);
      httpd_resp_send_404(req);
      return ESP_FAIL;
    }
    free(buf);
  } else {
    httpd_resp_send_404(req);
    return ESP_FAIL;
  }

  sensor_t* s = esp_camera_sensor_get();
  int res = 0;

  if (!strcmp(variable, "forward")) {
    Serial.println("Forward");
    digitalWrite(MOTOR_1_PIN_1, 1);
    digitalWrite(MOTOR_1_PIN_2, 0);
    digitalWrite(MOTOR_2_PIN_1, 1);
    digitalWrite(MOTOR_2_PIN_2, 0);
  } else if (!strcmp(variable, "left")) {
    Serial.println("Left");
    digitalWrite(MOTOR_1_PIN_1, 1);
    digitalWrite(MOTOR_1_PIN_2, 0);
    digitalWrite(MOTOR_2_PIN_1, 0);
    digitalWrite(MOTOR_2_PIN_2, 1);
  } else if (!strcmp(variable, "right")) {
    Serial.println("Right");
    digitalWrite(MOTOR_1_PIN_1, 0);
    digitalWrite(MOTOR_1_PIN_2, 1);
    digitalWrite(MOTOR_2_PIN_1, 1);
    digitalWrite(MOTOR_2_PIN_2, 0);
  } else if (!strcmp(variable, "backward")) {
    Serial.println("Backward");
    digitalWrite(MOTOR_1_PIN_1, 0);
    digitalWrite(MOTOR_1_PIN_2, 1);
    digitalWrite(MOTOR_2_PIN_1, 0);
    digitalWrite(MOTOR_2_PIN_2, 1);
  } else if (!strcmp(variable, "stop")) {
    Serial.println("Stop");
    digitalWrite(MOTOR_1_PIN_1, 0);
    digitalWrite(MOTOR_1_PIN_2, 0);
    digitalWrite(MOTOR_2_PIN_1, 0);
    digitalWrite(MOTOR_2_PIN_2, 0);

  } else if (atoi(variable) >= pos10 && atoi(variable) <= pos1e) {
    Serial.print("camSweep=");
    Serial.println(atoi(variable));
    myservo1.write(180 - atoi(variable));

  } else if (!strcmp(variable, "cam-up")) {
    pos0 = pos0 + 15;
    if (pos0 >= pos0e) pos0 = pos0e;
    Serial.printf("CAMERA=%d\n", pos0);
    myservo0.write(pos0);
  } else if (!strcmp(variable, "cam-dn")) {
    pos0 = pos0 - 15;
    if (pos0 <= pos00) pos0 = pos00;
    Serial.printf("CAMERA=%d\n", pos0);
    myservo0.write(pos0);
  } else if (!strcmp(variable, "led-up")) {
    bright += 128;
    if (bright >= 255) bright = 255;
    Serial.printf("LED=%d\n", bright);
    ledcWrite(LedPIN, bright);
  } else if (!strcmp(variable, "led-dn")) {
    bright -= 128;
    if (bright <= 0) bright = 0;
    Serial.printf("LED=%d\n", bright);
    ledcWrite(LedPIN, bright);
  } else if (!strcmp(variable, "horn")) {
    Serial.printf("HORN=%d\n", loud);
    ledcWrite(HornPIN, loud);
    delay(1000);
  } else if (!strcmp(variable, "hstop")) {
    ledcWrite(HornPIN, 0);
  } else {
    res = -1;
  }

  if (res) {
    return httpd_resp_send_500(req);
  }

  httpd_resp_set_hdr(req, "Access-Control-Allow-Origin", "*");
  return httpd_resp_send(req, NULL, 0);
}

void startCameraServer() {
  httpd_config_t config = HTTPD_DEFAULT_CONFIG();
  config.server_port = 80;
  httpd_uri_t index_uri = {
    .uri = "/",
    .method = HTTP_GET,
    .handler = index_handler,
    .user_ctx = NULL
  };
  httpd_uri_t cmd_uri = {
    .uri = "/action",
    .method = HTTP_GET,
    .handler = cmd_handler,
    .user_ctx = NULL
  };
  httpd_uri_t stream_uri = {
    .uri = "/stream",
    .method = HTTP_GET,
    .handler = stream_handler,
    .user_ctx = NULL
  };

  if (httpd_start(&camera_httpd, &config) == ESP_OK) {
    httpd_register_uri_handler(camera_httpd, &index_uri);
    httpd_register_uri_handler(camera_httpd, &cmd_uri);
  }

  config.server_port += 1;
  config.ctrl_port += 1;
  if (httpd_start(&stream_httpd, &config) == ESP_OK) {
    httpd_register_uri_handler(stream_httpd, &stream_uri);
  }
}

void setup() {
  pinMode(MOTOR_1_PIN_1, OUTPUT);
  pinMode(MOTOR_1_PIN_2, OUTPUT);
  pinMode(MOTOR_2_PIN_1, OUTPUT);
  pinMode(MOTOR_2_PIN_2, OUTPUT);

  Serial.begin(115200);
  // camera init
  camera_init();
  // Wi-Fi connection
  connectWiFi(ssid, password);
  // Start streaming web server
  startCameraServer();
  // Camera servo. Angle & Sweep
  myservo0.setPeriodHertz(50);            // standard 50 hz servo
  myservo0.attach(servoPin0, 550, 2350);  // attaches the servo
  myservo0.write(pos00);

  myservo1.setPeriodHertz(50);            // standard 50 hz servo
  myservo1.attach(servoPin1, 550, 2350);  // attaches the servo
  myservo1.write((pos10 + pos1e) / 2);
  // configure LED PWM functionalitites
  ledcAttachChannel(LedPIN, freq, reso, ledChannel);
  ledcWrite(LedPIN, bright);
  // configure Horn PWM functionalitites
  ledcAttachChannel(HornPIN, Hornfreq, Hornreso, ledChannelH);
}

void camera_init() {
  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_sccb_sda = SIOD_GPIO_NUM;
  config.pin_sccb_scl = SIOC_GPIO_NUM;
  config.pin_pwdn = PWDN_GPIO_NUM;
  config.pin_reset = RESET_GPIO_NUM;
  config.xclk_freq_hz = 40000000;
  config.frame_size = FRAMESIZE_VGA;
  config.pixel_format = PIXFORMAT_JPEG;  // for streaming
  config.grab_mode = CAMERA_GRAB_WHEN_EMPTY;
  config.fb_location = CAMERA_FB_IN_DRAM;  // CAMERA_FB_IN_PSRAM;
  config.jpeg_quality = 12;
  config.fb_count = 1;

  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
  s->set_vflip(s, 1);       // flip it back
  s->set_brightness(s, 1);  // up the brightness just a bit
  s->set_saturation(s, 0);  // lower the saturation
  s->set_hmirror(s, 1);
}

void connectWiFi(const char* ssid, const char* pwd) {
  Serial.println("");
  Serial.println("Connecting to WiFi network: " + String(ssid));
  WiFi.disconnect(true, true);
  delay(1000);
  WiFi.begin(ssid, password);
  Serial.println("Waiting for WIFI connection ");
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println(".");
  Serial.println("WiFi connected!");
  Serial.print("Camera Stream Ready! Go to: http://");
  Serial.println(WiFi.localIP());
  delay(1000);
}

void loop() {
}

　最後まで見ていただきありがとうございました。

0
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?