SPRESENSEで倒立振子に挑戦してみた話

elchikaでSPRESENSEを使った倒立振子作品がなかったので、挑戦してみました。

・倒立振子のハード製作
・SPRESENSEでのモータの駆動
・カメラ映像の表示/撮影
・IMUセンサで姿勢制御
~~・WIfi/LTEで遠隔操作~~（次回）

部品は安く手に入るものを中心に構成を組み立てました

部品名	型式など
SPRESENSEメインボード
SPRESENSE LTE拡張ボード
SPRESENSE HDRカメラボード
WiFiAddonボード　is110B
Lipoバッテリ	402030
バッテリ充電ボード	TP4056
エンコーダ付きギアードモータ	JGA25-371
モータドライバ	TB6612FNG
加速度センサ	GY-521

倒立振子でよくあるパターンがユニバーサル基板をフレームに、タミヤのギアモータなどを取り付けるものでした。
外装もなく縦長で見た目が微妙なので、簡易に3Dプリンタでケース・フレーム設計を行います。
全高を低く抑えたものを目標とします。

内蔵する各基板サイズも形状も違い、上部からピンヘッダで配線をするので、全高を抑えるには少し工夫が必要そうです。
機能ごとにケースを分け、上部に積み上げて取り付ける方式としました。

これで、機能を追加したい場合も新たなモジュールケースを挟み込むことで、簡単に追加できる構造とします。
前後方向と上下方向を固定するために、左右から爪で押さえつける構造とすることで、ケースごとのねじなどの締結部品の数を省いています。

使用するギアードモータは定格12Vなのですが、12V電源を持ってくるのは結構大変です。
3~5Vで駆動ができればLipoバッテリやモバイルバッテリなどでも問題がないので、モータを低電圧で駆動ができるか確認してみます。

モータ電圧を変化させて、電流がどの程度必要かをプロットしました。

動き出しの電圧は1.3Vくらいでした。
この結果であればLipoバッテリなどでもいけそうです。

IMUpin	SPRESENSE
VCC	JP1 10
GND	JP2 10
SCL	JP2 11
SDA	JP2 12

モータドライバは、SPRESENSEとモータ用外部電源（バッテリ）、モータと接続します。

モータドライバpin
PWMA	CN9 9
AIN2	CN9 7
AIN1	CN9 8
STBY	JP1 10
BIN1	CN9 10
BIN2	CN9 12
PWMB	CN9 11
GND
VM	外部電源＋
VCC	JP1 10
GND	外部電源GND
AO1	モータ1　赤
AO2	モータ1　白
BO2	モータ2　赤
BO1	モータ2　白
GND	CN9 2

3Dプリンタで印刷しました。

モジュールのピンヘッダのはんだづけと、充電モジュールにLipoを繋ぎます。

加速度センサ・モータドライバも同様にピンをはんだ付けします

モータとベース部品を繋げます。

1段目のケースを取り付けます。
IMUは前方中心に設置しました。

ケースを積み上げて、全部品を組付けました。

SPRESENSEには、電力供給＋通信用に脇と正面から有線でアクセス可能にしました。

以下のコードでカメラ撮影が可能なことを確認しました。非常に簡単です。


カメラ撮影
#include <SDHCI.h>
#include <stdio.h> 
#include <Camera.h>

#define BAUDRATE                (115200)
#define TOTAL_PICTURE_COUNT     (10)

SDClass  theSD;
int take_picture_count = 0;

void printError(enum CamErr err)
{
  Serial.print("Error: ");
  switch (err)
    {
      case CAM_ERR_NO_DEVICE:
        Serial.println("No Device");
        break;
      case CAM_ERR_ILLEGAL_DEVERR:
        Serial.println("Illegal device error");
        break;
      case CAM_ERR_ALREADY_INITIALIZED:
        Serial.println("Already initialized");
        break;
      case CAM_ERR_NOT_INITIALIZED:
        Serial.println("Not initialized");
        break;
      case CAM_ERR_NOT_STILL_INITIALIZED:
        Serial.println("Still picture not initialized");
        break;
      case CAM_ERR_CANT_CREATE_THREAD:
        Serial.println("Failed to create thread");
        break;
      case CAM_ERR_INVALID_PARAM:
        Serial.println("Invalid parameter");
        break;
      case CAM_ERR_NO_MEMORY:
        Serial.println("No memory");
        break;
      case CAM_ERR_USR_INUSED:
        Serial.println("Buffer already in use");
        break;
      case CAM_ERR_NOT_PERMITTED:
        Serial.println("Operation not permitted");
        break;
      default:
        break;
    }
}

void CamCB(CamImage img)
{

  /* Check the img instance is available or not. */

  if (img.isAvailable())
    {

      img.convertPixFormat(CAM_IMAGE_PIX_FMT_RGB565);

      Serial.print("Image data size = ");
      Serial.print(img.getImgSize(), DEC);
      Serial.print(" , ");

      Serial.print("buff addr = ");
      Serial.print((unsigned long)img.getImgBuff(), HEX);
      Serial.println("");
    }
  else
    {
      Serial.println("Failed to get video stream image");
    }
}

void setup()
{
  CamErr err;

  Serial.begin(BAUDRATE);
  while (!Serial)
    {
      ;
    }

  while (!theSD.begin()) 
    {
      Serial.println("Insert SD card.");
    }

  Serial.println("Prepare camera");
  err = theCamera.begin();
  if (err != CAM_ERR_SUCCESS)
    {
      printError(err);
    }

  Serial.println("Start streaming");
  err = theCamera.startStreaming(true, CamCB);
  if (err != CAM_ERR_SUCCESS)
    {
      printError(err);
    }

  Serial.println("Set Auto white balance parameter");
  err = theCamera.setAutoWhiteBalanceMode(CAM_WHITE_BALANCE_DAYLIGHT);
  if (err != CAM_ERR_SUCCESS)
    {
      printError(err);
    }
 
  Serial.println("Set still picture format");
  err = theCamera.setStillPictureImageFormat(
     CAM_IMGSIZE_QUADVGA_H,
     CAM_IMGSIZE_QUADVGA_V,
     CAM_IMAGE_PIX_FMT_JPG);
  if (err != CAM_ERR_SUCCESS)
    {
      printError(err);
    }
}

void loop()
{
  sleep(1); 

  if (take_picture_count < TOTAL_PICTURE_COUNT)
    {
  
      Serial.println("call takePicture()");
      CamImage img = theCamera.takePicture();

      if (img.isAvailable())
        {    
          char filename[16] = {0};
          sprintf(filename, "PICT%03d.JPG", take_picture_count);
    
          Serial.print("Save taken picture as ");
          Serial.print(filename);
          Serial.println("");

          theSD.remove(filename);
          File myFile = theSD.open(filename, FILE_WRITE);
          myFile.write(img.getImgBuff(), img.getImgSize());
          myFile.close();
        }
      else
        {

          Serial.println("Failed to take picture");
        }
    }
  else if (take_picture_count == TOTAL_PICTURE_COUNT)
    {
      Serial.println("End.");
      theCamera.end();
    }

  take_picture_count++;
}