M5BALAでライントレース（M5Camera-MicroPython編）

概要

M5BALAでライントレース、今回は M5Camera の画像を M5Stack FIRE 上の MicroPython で処理し M5BALA を制御する方式です。

• M5Camera は https://github.com/m5stack/m5stack-cam-psram を元に以下のように修正

  • 画像フォーマットはグレースケール
  • 画像サイズは QQVGA（160x120）
  • WIFI はステーションモード
  • 固定IPアドレス
  • 画像データはソケット通信で応答
  • 応答データは横１行分のみ

• M5Stack FIRE は MicroPython で動作し、M5Camera の画像データを取得、線の位置を認識して M5BALA を制御する

環境

• M5Camera
• M5BALA
• M5Stack FIRE
• ESP-IDF v3.3-beta1-64-gd8b625a32
• M5UI.Flow (Firmware 1.0.2 で確認)

実行例

• M5Camera を M5BALA+M5Stack FIRE の上に載せ前方の線を撮影
• M5Camera のGROVEポートを M5Stack FIRE のポートA に接続して電源供給

#M5BALA と遊ぼう、#M5Camera の画像を #M5Stack の MicroPython で処理して M5BALAを制御する方式の実装です。
「M5BALAでライントレース（M5Camera-MicroPython編）」https://t.co/Y6cWMBmsGu
少々複雑なコースでも（何度かに一度ですが）完走することができました。 pic.twitter.com/Pu8ceYIUVW

— 稲澤祐一 (@inasawa) 2018年12月22日

M5Cameraプログラム

https://github.com/m5stack/m5stack-cam-psram を取得し以下の２ファイルを修正。

• main/Kconfig.projbuild
• main/main.c

WiFi 接続および Socket 通信用に以下をコンフィグにて設定

• WiFi AP の SSID
• WiFi AP の PASSWORD
• 固定IPアドレス
• ネットワークゲートウェイ
• ネットワークマスク
• Socket 通信用ポート番号

Kconfig.projbuild

menu "Example Configuration"

config M5CAMERA_WIFI_SSID
    string "WiFi SSID"
    default ""
    help
        SSID (network name) for the example to connect to.

config M5CAMERA_WIFI_PASSWORD
    string "WiFi Password"
    default ""
    help
        WiFi password (WPA or WPA2) for the example to use.

config M5CAMERA_WIFI_IP
    string "WiFi IP"
    default ""
    help
        WiFi IP Address for M5Camera

config M5CAMERA_WIFI_GW
    string "WiFi GW"
    default "192.168.0.1"
    help
        WiFi GW

config M5CAMERA_WIFI_NETMASK
    string "WiFi Network Mask"
    default "255.255.255.0"
    help
        WiFi Network Mask

config M5CAMERA_LISTEN_PORT
    int "Listen Port"
    default "9000"
    help
        Socket Listen Port

endmenu

main.c

#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/event_groups.h"
#include "freertos/semphr.h"
#include "freertos/task.h"

#include "driver/gpio.h"
#include "sdkconfig.h"
#include "esp_log.h"

#include "esp_wifi.h"
#include "nvs_flash.h"
#include "esp_timer.h"
#include "esp_camera.h"
#include "esp_event_loop.h"
#include <lwip/sockets.h>

static const char* TAG = "camera";

//M5STACK_CAM PIN Map
#define CAM_PIN_RESET   15 //software reset will be performed
#define CAM_PIN_XCLK    27
#define CAM_PIN_SIOD    22 //25
#define CAM_PIN_SIOC    23

#define CAM_PIN_D7      19
#define CAM_PIN_D6      36
#define CAM_PIN_D5      18
#define CAM_PIN_D4      39
#define CAM_PIN_D3      5
#define CAM_PIN_D2      34
#define CAM_PIN_D1      35
#define CAM_PIN_D0      32

#define CAM_PIN_VSYNC   25 //22
#define CAM_PIN_HREF    26
#define CAM_PIN_PCLK    21

#define CAM_XCLK_FREQ   20000000

static EventGroupHandle_t s_wifi_event_group;
static ip4_addr_t s_ip_addr;
const int CONNECTED_BIT = BIT0;

static camera_config_t camera_config = {
    .pin_reset = CAM_PIN_RESET,
    .pin_xclk = CAM_PIN_XCLK,
    .pin_sscb_sda = CAM_PIN_SIOD,
    .pin_sscb_scl = CAM_PIN_SIOC,

    .pin_d7 = CAM_PIN_D7,
    .pin_d6 = CAM_PIN_D6,
    .pin_d5 = CAM_PIN_D5,
    .pin_d4 = CAM_PIN_D4,
    .pin_d3 = CAM_PIN_D3,
    .pin_d2 = CAM_PIN_D2,
    .pin_d1 = CAM_PIN_D1,
    .pin_d0 = CAM_PIN_D0,
    .pin_vsync = CAM_PIN_VSYNC,
    .pin_href = CAM_PIN_HREF,
    .pin_pclk = CAM_PIN_PCLK,

    //XCLK 20MHz or 10MHz
    .xclk_freq_hz = CAM_XCLK_FREQ,
    .ledc_timer = LEDC_TIMER_0,
    .ledc_channel = LEDC_CHANNEL_0,

    .pixel_format = PIXFORMAT_GRAYSCALE,//PIXFORMAT_JPEG,//YUV422,GRAYSCALE,RGB565,JPEG
    .frame_size = FRAMESIZE_QQVGA,//FRAMESIZE_QVGA,//QQVGA-UXGA Do not use sizes above QVGA when not JPEG
    //    FRAMESIZE_QQVGA,    // 160x120
    //    FRAMESIZE_QQVGA2,   // 128x160
    //    FRAMESIZE_QCIF,     // 176x144
    //    FRAMESIZE_HQVGA,    // 240x176
    //    FRAMESIZE_QVGA,     // 320x240
    //    FRAMESIZE_CIF,      // 400x296
    //    FRAMESIZE_VGA,      // 640x480
    //    FRAMESIZE_SVGA,     // 800x600
    //    FRAMESIZE_XGA,      // 1024x768
    //    FRAMESIZE_SXGA,     // 1280x1024
    //    FRAMESIZE_UXGA,     // 1600x1200

    .jpeg_quality = 2, //0-63 lower number means higher quality
    .fb_count = 3 //if more than one, i2s runs in continuous mode. Use only with JPEG
};

static void wifi_init_station();
static void tcp_server_task(void *pvParameters);

void app_main()
{
    esp_log_level_set("wifi", ESP_LOG_INFO);

    esp_err_t err = nvs_flash_init();
    if (err != ESP_OK) {
        ESP_ERROR_CHECK( nvs_flash_erase() );
        ESP_ERROR_CHECK( nvs_flash_init() );
    }

    err = esp_camera_init(&camera_config);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "Camera Init Failed");
    }

    wifi_init_station();
    vTaskDelay(100 / portTICK_PERIOD_MS);
    xTaskCreate(tcp_server_task, "tcp_server", 4096, NULL, 5, NULL);
}

static esp_err_t event_handler(void* ctx, system_event_t* event) 
{
  switch (event->event_id) {
    case SYSTEM_EVENT_STA_START:
      esp_wifi_connect();
      break;
    case SYSTEM_EVENT_STA_GOT_IP:
      ESP_LOGI(TAG, "got ip:%s", ip4addr_ntoa(&event->event_info.got_ip.ip_info.ip));
      s_ip_addr = event->event_info.got_ip.ip_info.ip;
      xEventGroupSetBits(s_wifi_event_group, CONNECTED_BIT);
      break;
    case SYSTEM_EVENT_AP_STACONNECTED:
      ESP_LOGI(TAG, "station:" MACSTR " join, AID=%d", MAC2STR(event->event_info.sta_connected.mac),
               event->event_info.sta_connected.aid);
      xEventGroupSetBits(s_wifi_event_group, CONNECTED_BIT);
      break;
    case SYSTEM_EVENT_AP_STADISCONNECTED:
      ESP_LOGI(TAG, "station:" MACSTR "leave, AID=%d", MAC2STR(event->event_info.sta_disconnected.mac),
               event->event_info.sta_disconnected.aid);
      xEventGroupClearBits(s_wifi_event_group, CONNECTED_BIT);
      break;
    case SYSTEM_EVENT_STA_DISCONNECTED:
      esp_wifi_connect();
      xEventGroupClearBits(s_wifi_event_group, CONNECTED_BIT);
      break;
    default:
      break;
  }
  return ESP_OK;
}

static void wifi_init_station()
{
    tcpip_adapter_init();
    tcpip_adapter_dhcpc_stop(TCPIP_ADAPTER_IF_STA); // Don't run a DHCP client
    tcpip_adapter_ip_info_t ipInfo;

    inet_pton(AF_INET, CONFIG_M5CAMERA_WIFI_IP, &ipInfo.ip);
    inet_pton(AF_INET, CONFIG_M5CAMERA_WIFI_GW, &ipInfo.gw);
    inet_pton(AF_INET, CONFIG_M5CAMERA_WIFI_NETMASK, &ipInfo.netmask);

    tcpip_adapter_set_ip_info(TCPIP_ADAPTER_IF_STA, &ipInfo);

    s_wifi_event_group = xEventGroupCreate();
    ESP_ERROR_CHECK( esp_event_loop_init(event_handler, NULL) );
    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
    ESP_ERROR_CHECK( esp_wifi_init(&cfg) );
    ESP_ERROR_CHECK( esp_wifi_set_storage(WIFI_STORAGE_RAM) );
    wifi_config_t wifi_config = {
        .sta = {
            .ssid = CONFIG_M5CAMERA_WIFI_SSID,
            .password = CONFIG_M5CAMERA_WIFI_PASSWORD,
        },
    };
    ESP_ERROR_CHECK( esp_wifi_set_mode(WIFI_MODE_STA) );
    ESP_ERROR_CHECK( esp_wifi_set_config(WIFI_IF_STA, &wifi_config) );
    ESP_ERROR_CHECK( esp_wifi_start() );
    ESP_ERROR_CHECK( esp_wifi_set_ps(WIFI_PS_NONE) );
    ESP_LOGI(TAG, "Connecting to \"%s\"", wifi_config.sta.ssid);
    xEventGroupWaitBits(s_wifi_event_group, CONNECTED_BIT, false, true, portMAX_DELAY);
    ESP_LOGI(TAG, "Connected");
}

static void tcp_server_task(void *pvParameters)
{
    char rx_buffer[128];
    char addr_str[128];
    int addr_family;
    int ip_protocol;

    struct sockaddr_in destAddr;
    destAddr.sin_addr.s_addr = htonl(INADDR_ANY);
    destAddr.sin_family = AF_INET;
    destAddr.sin_port = htons(CONFIG_M5CAMERA_LISTEN_PORT);
    addr_family = AF_INET;
    ip_protocol = IPPROTO_IP;
    inet_ntoa_r(destAddr.sin_addr, addr_str, sizeof(addr_str) - 1);

    int listen_sock = socket(addr_family, SOCK_STREAM, ip_protocol);
    if (listen_sock < 0) {
        ESP_LOGE(TAG, "Unable to create socket: errno %d", errno);
        goto END;
    }
    ESP_LOGI(TAG, "Socket created");
    int err = bind(listen_sock, (struct sockaddr *)&destAddr, sizeof(destAddr));
    if (err != 0) {
        ESP_LOGE(TAG, "Socket unable to bind: errno %d", errno);
        goto END;
    }
    ESP_LOGI(TAG, "Socket binded");

    while (1) {
        err = listen(listen_sock, 1);
        if (err != 0) {
            ESP_LOGE(TAG, "Error occured during listen: errno %d", errno);
            break;
        }
        ESP_LOGI(TAG, "Socket listening");

        struct sockaddr_in6 sourceAddr; // Large enough for both IPv4 or IPv6
        uint addrLen = sizeof(sourceAddr);
        int sock = accept(listen_sock, (struct sockaddr *)&sourceAddr, &addrLen);
        if (sock < 0) {
            ESP_LOGE(TAG, "Unable to accept connection: errno %d", errno);
            break;
        }
        ESP_LOGI(TAG, "Socket accepted");

        while (1) {
            int len = recv(sock, rx_buffer, sizeof(rx_buffer) - 1, 0);
            // Error occured during receiving
            if (len < 0) {
                ESP_LOGE(TAG, "recv failed: errno %d", errno);
                break;
            }
            // Connection closed
            else if (len == 0) {
                ESP_LOGI(TAG, "Connection closed");
                break;
            }
            rx_buffer[len] = 0;
            ESP_LOGI(TAG, "Camera capture request from %s", rx_buffer);

            camera_fb_t * fb = NULL;
            //esp_err_t res = ESP_OK;
            size_t fb_len = 0;
            int64_t fr_start = esp_timer_get_time();

            fb = esp_camera_fb_get();
            if (!fb) {
                ESP_LOGE(TAG, "Camera capture failed");
                break;
            }
            fb_len = fb->len;

            // 線の認識用途であるため、画像の１行分のみ送信
            if (fb_len > fb->width) {
                fb_len = fb->width;
            }
            err = send(sock, (const char *)fb->buf, fb_len, 0);
            if (err < 0) {
                ESP_LOGE(TAG, "Error occured during sending: errno %d", errno);
                break;
            }
            esp_camera_fb_return(fb);

            int64_t fr_end = esp_timer_get_time();
            ESP_LOGI(TAG, "Data Sent %uB %ums", (uint32_t)(fb_len), (uint32_t)((fr_end - fr_start)/1000));
        }
        ESP_LOGE(TAG, "Shutting down socket and restarting...");
        shutdown(sock, 0);
        close(sock);
    }
    END:
    vTaskDelete(NULL);
}

M5BALA 制御プログラム

M5 UI.Flowから以下のプログラムを実行しました。

m5camera_line_trace.py

from m5stack import *
from m5bala  import M5Bala
import i2c_bus
import wifisetup
import time
import gc
import usocket

TURN_MAX = 100
MOVE_OFFSET = 40
LINE_DETECTION_THRESHOLD = -10

M5CAMERA_IP   = 'xxx.xxx.xxx.xxx'  # M5Camera側のコンフィグに合わせて設定
M5CAMERA_PORT = 9000               # M5Camera側のコンフィグに合わせて設定
M5CAMERA_DATA_LEN = 160 # QQVGA (160x120) の横ピクセル数


wifisetup.auto_connect()

lcd.font(lcd.FONT_DejaVu18)
lcd.clear()

m5bala = M5Bala(i2c_bus.get(i2c_bus.M_BUS))

# フレームの処理レート表示用
last_fps_time = time.time()
fps_counter = 0

client = usocket.socket()

control_interval = 0
while True:
    client.connect(usocket.getaddrinfo(M5CAMERA_IP, M5CAMERA_PORT)[0][-1])
    while True:
        if time.ticks_diff(time.ticks_ms(), control_interval) > 0:
            control_interval = time.ticks_add(time.ticks_ms(), 100)
            try:
                client.send(b'M5BALA')
                img = client.recv(M5CAMERA_DATA_LEN)
            except Exception as e:
                print(e)
                client.close()
                break

            # 輝度の差分を計算しの最小値のインデックスを取得
            # 最小値：白から黒になる部分（線の左端）
            p=img[0]
            min_d = 0
            min_i = 0
            imd_xh = len(img) / 2 # 中央位置
            for i in range(1, len(img)):
                d = img[i]-p
                if d < min_d:
                    min_i = i
                    min_d = d
                p = img[i]

            # 輝度の差分の最小値が閾値より小さい時に線を認識したとして回転の計算をする
            # 閾値以上の場合は線を見失ったものとして最大の回転を行う
            if min_d < LINE_DETECTION_THRESHOLD:
                turn = int((min_i - imd_xh) / imd_xh * TURN_MAX)
            else:
                turn = TURN_MAX

            if turn > 0:
                m5bala.left  = MOVE_OFFSET + turn
                m5bala.right = MOVE_OFFSET
            else:
                m5bala.left  = MOVE_OFFSET
                m5bala.right = MOVE_OFFSET - turn

            lcd.print('L:{:4d}  R:{:4d}  '.format(
                    int(m5bala.left), int(m5bala.right)),
                    10, 190)

            # フレームの処理レート表示
            fps_counter += 1
            now = time.time()
            if int(now) > int(last_fps_time):
                lcd.print('{:5.2f} fps  '.format((fps_counter) / (now - last_fps_time)), 10, 210)
                last_fps_time = now
                fps_counter = 0

        m5bala.balance()
        gc.collect()