初版

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

製作品

# 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バッテリなどでもいけそうです。 ## 配線 ### IMU | 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には、電力供給＋通信用に脇と正面から有線でアクセス可能にしました。  # カメラ撮影 以下のコードでカメラ撮影が可能なことを確認しました。非常に簡単です。  ```arduino:カメラ撮影 #include <SDHCI.h> #include <stdio.h> /* for sprintf */ #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++; } ``` # 制御 I2Cで取得したIMUのデータをもとにモータの回転を制御します。 傾き量に応じてモータのPWMを調整してバランスを取ります。 # 今後の課題 姿勢制御のパラメータ調整と 遠隔操作に次は挑戦したいと思います