画像認識による自律走行Vehicle

Spresense のカメラ画像により、画像を認識し、Vehicleを自律走行させるのが目標です。
現状、充分な仕上がりとは言えませんが、自律走行の技術要素の確認はできました。（達成したとは言えませんが・・・・・）

Spresense Main Board
Spresense 拡張Board
Spresense カメラBoard
ツイストクローラー　（タミヤ）
DCモータードライバ　DRV8835（秋月電子通商）
USB用バッテリー　（Spresense 駆動用）
単3電池＊２　（DCモータ駆動用）

Youtubeのチュートリアルにて、学習。
Macは、PC版が無いので、クラウド版にて行う。
殆どチュートリアルのコピペ。
【number_recognition.sdcproj】参照

NNCを適用するためには、Aruduino IDEが必要。VSCodeでもできるのだろうが、圧倒的に情報が足りないため、Aruduino IDEをインストール。
画像処理の部分は、チュートリアルSpresense | 14. Neural Network Consle を使って組込みAIを体験する（画像編）【ソニー公式】から転用。
【Spresense_number_recognition.ino】をベースに、0〜9の数字を認識して、『4』だけをターゲットとしてサーチします。

Vehicle自律走行
#include <Camera.h>
#include <DNNRT.h>
#include <SDHCI.h>

SDClass theSD;

#define DNN_IMG_W 28
#define DNN_IMG_H 28
#define CAM_IMG_W 320
#define CAM_IMG_H 240
#define CAM_CLIP_X 104
#define CAM_CLIP_Y 0
#define CAM_CLIP_W 112
#define CAM_CLIP_H 224

uint8_t buf[DNN_IMG_W*DNN_IMG_H];

DNNRT dnnrt;
DNNVariable input(DNN_IMG_W*DNN_IMG_H);
  
static uint8_t const label[11] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10};

// io pin define  //全大文字を使うのが嫌なので・・・・      
int R_Forward = 5;      // RightMotor Forward　// PWM0
int R_Backward = 6;     // RightMotor back    // PWM1
int L_Forward = 3;      // Left Forward       // PWM2
int L_Backward = 9;     // Left back           // PWM3
int speed = 160;

// IO remote操作のときに使う
int cameraShutter = 2;  // cameraShutter
int speedDown     = 0;      // Speed down
int speedUp       = 1;      // Speed Up
int moveCCW       = 4;
int moveCW        = 7;
int moveForward   = 10;
int moveBackward  =  8;
int moveStop      = 11;

int findCountSum = 0;
unsigned long previousMillis = 0;
bool firstDirectionCW = true;
int cwCCWdelay = 100;

void CamCB(CamImage img) {
  if (!img.isAvailable()) {
    Serial.println("Image is not available. Try again");
    return;
  }

  CamImage small;
  CamErr err = img.clipAndResizeImageByHW(small
                     , CAM_CLIP_X, CAM_CLIP_Y
                     , CAM_CLIP_X + CAM_CLIP_W -1
                     , CAM_CLIP_Y + CAM_CLIP_H -1
                     , DNN_IMG_W, DNN_IMG_H);
  if (!small.isAvailable()){
    Serial.println("Clip and Reize Error:" + String(err));
    return;
  }

  small.convertPixFormat(CAM_IMAGE_PIX_FMT_RGB565);
  uint16_t* tmp = (uint16_t*)small.getImgBuff();

  float *dnnbuf = input.data();
  float f_max = 0.0;
  for (int n = 0; n < DNN_IMG_H*DNN_IMG_W; ++n) {
    dnnbuf[n] = (float)((tmp[n] & 0x07E0) >> 5);
    if (dnnbuf[n] > f_max) f_max = dnnbuf[n];
  }
  
  /* normalization */
  for (int n = 0; n < DNN_IMG_W*DNN_IMG_H; ++n) {
    dnnbuf[n] /= f_max;
  }
  
  String gStrResult = "?";
  dnnrt.inputVariable(input, 0);
  dnnrt.forward();
  DNNVariable output = dnnrt.outputVariable(0);
  int index = output.maxIndex();
  
  if (index < 10) {
    gStrResult = String(label[index]) + String(":") + String(output[index]);
    switch (index) {
      case 3:       //
        // Serial.println("************* 3");
        break;
      case 4:       //
        findCountSum ++;
        Serial.print("Find  ");
        Serial.print(gStrResult);
        Serial.print("  ");
        Serial.println(findCountSum);
       break;

      case 5:       //
        // Serial.println("************* 5");
        break;

      default:
        break;
    }
  } else {
    gStrResult = String("?:") + String(output[index]);
  }
  // Serial.println(gStrResult);
}

void setup() {
  Serial.begin(115200);
  pinMode(R_Forward, OUTPUT);
  pinMode(R_Backward, OUTPUT);
  pinMode(L_Forward, OUTPUT);
  pinMode(L_Backward, OUTPUT);
  analogWrite(R_Forward, 0);
  analogWrite(R_Backward, 0); 
  analogWrite(L_Forward, 0); 
  analogWrite(L_Backward, 0); 

  // 外部リモートで操作する場合
  pinMode(cameraShutter, INPUT_PULLUP);   
  pinMode(speedDown, INPUT_PULLUP);
  pinMode(speedUp, INPUT_PULLUP);
  pinMode(moveCCW, INPUT_PULLUP);
  pinMode(moveCW, INPUT_PULLUP);
  pinMode(moveBackward, INPUT_PULLUP);
  pinMode(moveForward, INPUT_PULLUP);
  pinMode(moveStop, INPUT_PULLUP);

  while (!theSD.begin()) {  }
  
  // Neural Network Console クラウド版にて学習した結果
  File nnbfile = theSD.open("result_v1.nnb");
  int ret = dnnrt.begin(nnbfile);
  if (ret < 0) {
    Serial.println("dnnrt.begin failed" + String(ret));
    return;
  }

  theCamera.begin();
  theCamera.startStreaming(true, CamCB);
}

void loop() {
  unsigned long currentMillis = millis();
  if (cwCCWdelay > 200) {
    cwCCWdelay = 100;
  }
  if (currentMillis - previousMillis >= 1000) {
    findCountSum = 0;
    previousMillis = currentMillis;
    // remoteIO_Check();
  }
  if (findCountSum > 3) {
    motorForward();       //方向が正しいので進め
    cwCCWdelay = 100;
  } else {
    motorStop();
    if (firstDirectionCW) {
      motorCCW();
      firstDirectionCW = false;      
    } else {
      motorCW();
      firstDirectionCW = true;
    }
    cwCCWdelay += 5;
    delay(cwCCWdelay);
    motorStop();
  }

}

// Motor Control Analog(PWM)
void motorStop() {
  Serial.println("++++Stop");
  analogWrite(R_Forward, 0);
  analogWrite(R_Backward, 0); 
  analogWrite(L_Forward, 0); 
  analogWrite(L_Backward, 0); 
}

void motorForward() {
  Serial.println("++++Forward");
  analogWrite(R_Forward, speed);
  analogWrite(L_Forward, speed);   
  analogWrite(R_Backward, 0);
  analogWrite(L_Backward, 0);   
}

void motorBackward() {
  Serial.println("++++Backward");
  analogWrite(R_Backward, speed);
  analogWrite(L_Backward, speed);   
  analogWrite(R_Forward, 0);
  analogWrite(L_Forward, 0);   
}

void motorCW() {
  Serial.println("++++CW");
  analogWrite(R_Forward, 0);
  analogWrite(R_Backward, 0); 
  analogWrite(L_Forward, speed+10); 
  analogWrite(L_Backward, 0); 
}

void motorCCW() {
  Serial.println("++++CCW");
  analogWrite(R_Forward, speed);
  analogWrite(R_Backward, 0); 
  analogWrite(L_Forward, 0); 
  analogWrite(L_Backward, 0); 
}

void motorSpeedUp() {
  speed += 10;  
}

void motorSpeedDown() {
  speed -= 10;
}

void remoteIO_Check() {
  int val;
   val = digitalRead(cameraShutter);
  if (val == 0) {
    Serial.println("cameraShutter On");    
  }
  val = digitalRead(speedDown);
  if (val == 0) {
    speed -= 10;
    Serial.print("speedDown Speed = ");    
    Serial.println(speed);
  }
  val = digitalRead(speedUp);
  if (val == 0) {
    speed += 10;
    Serial.print("speedUp Speed = "); 
    Serial.println(speed);
  }
  val = digitalRead(moveCCW);
  if (val == 0) {
    Serial.println("moveCCW On");    
    analogWrite(R_Forward, speed);
    analogWrite(L_Backward, speed);   
  }
  val = digitalRead(moveCW);
  if (val == 0) {
    Serial.println("moveCW On");    
    analogWrite(R_Backward, speed);
    analogWrite(L_Forward, speed);   
  }
  val = digitalRead(moveBackward);
  if (val == 0) {
    Serial.println("moveBackward On");    
    analogWrite(R_Backward, speed);
    analogWrite(L_Backward, speed);   
  }
  val = digitalRead(moveForward);
  if (val == 0) {
    Serial.println("moveForward On");
    analogWrite(R_Forward, speed);
    analogWrite(L_Forward, speed);   
  }
  val = digitalRead(moveStop);
  if (val == 0) {
    analogWrite(R_Forward, 0);
    analogWrite(R_Backward, 0); 
    analogWrite(L_Forward, 0); 
    analogWrite(L_Backward, 0); 
  }
}

調整中の段階ですが、現状の動作状況の動画を御覧ください。
@youtube

当初の構想が甘すぎ、紆余曲折してしまいました。
- NNCの理解が不十分で、チュートリアルから外れると手が出ない。
- そもそも画像認識はかなり高度な技術な技術なのに、NNCの見た目の簡便さで勉強不足だった。
- 本来は、物体追跡で画像認識を行いたかったのだが、そのレベルには程遠い状況です。大学入試に中学生レベルで挑戦したようなものです。
- 現状では、光検知センサー数個でライントレースする方がはるかにスムーズかつ、容易に実現できます。
- それでも、これだけの画像処理ができるSpresense の能力は魅力的ですね。
VSCode＋PlatformIOの環境になれすぎて、Arduino IDE がなかなか思う様に使いこなせませんでした。

このサイズで、マルチコア、GPS、オーディオ、カメラ等の魅力たっぷり、かつ、画像処理を楽々こなせる能力が素晴らしい。
ただし、外部との通信機能が弱い点が気になる。
（利用方法次第とも言えるが・・・）
NNCをワンボードで活用できるのには驚いた。ただあまりにも分野が広いので学習コストが高そう。

マルチコアをシングルで使っているためか、タイマー割り込みが異様に遅く使えなかった。（画像処理、Serial.printを排除しても・・・）
同じ処理をタイマー割り込み内で使うのと、void loop内で使うのと速度が大幅に異なる。
誰か教えて下さい。
本ソフトでは、タイマー割り込みを使わず、millis()を使っています。

タイマー割り込み確認テスト
int cnt=1;
void timerWup() {
  // Serial.print("Wup ");
  // Serial.println(cnt);
  //cnt ++;
  if (cnt == 1 ) {
    digitalWrite(11, HIGH);
    cnt = 0;
  } else {
    digitalWrite(11, LOW);
    cnt = 1;
  }
  // Serial.println("Wup");
}

void setup() {
  Serial.begin(115200);
  pinMode(11, OUTPUT);
  attachTimerInterrupt(timerWup, 200000);

}

void loop() {
  // if (cnt == 1 ) {
  //   digitalWrite(11, HIGH);
  //   cnt = 0;
  // } else {
  //   digitalWrite(11, LOW);
  //   cnt = 1;
  // }
  // delay(200);
}

eMMCライブラリは、SDKでは動くが、Arduino IDE ではうまく動きませんでした。あまり深く追求していませんが・・・・

HakoHiro

函館在住のOldエンジニアです。マイコンは　Z80ワンボードの頃から使っています。なにせあの頃は、ハンドアセンブルでプログラム作成していました。（ニーモニックを手書きで書き、それを表を見ながら機械語コードに落とします）おかげで　リターンコード（RTN=0xC9)はいまだに覚えています。RTNまで来るとサブルーチンが一段落でホッとするので・・・現在は、電子工作の他に錫で色々作って楽しんでいます。