はじめに
リコーの @KA-2 です。
弊社ではRICOH THETAという全周囲360度撮れるカメラを出しています。
RICOH THETA VやRICOH THETA Z1は、OSにAndroidを採用しています。Androidアプリを作る感覚でTHETAをカスタマイズすることもでき、そのカスタマイズ機能を「プラグイン」と呼んでいます（詳細は本記事の末尾を参照）。
突然ですが、こちらの記事の「まとめ」で、私、こんなこと書いていました。
シリアル通信ができると、今回の事例のような既製品のみならず、電子工作の世界では有名な Arduino をはじめとする廉価なマイコンボードとも連携できるようになります。THETAから手足が生えたかのように、さまざまなセンサーやモーターなどを拡張することも可能です。
というわけで、やってみました。ついにTHETAが動き出します！
作成物は以下のような外観です。
動作している様子は以下をご参照ください。
【開発記事】ついにTHETAが動き始めます！自立移動への道、最初の一歩はTHETAのラジコン化。
— THETAプラグイン開発者コミュニティ (@thetaplugin) 2019年6月27日
↓Qiita記事はこちらhttps://t.co/b43069fyNL#M5Stack #M5Bala #thetaplugin pic.twitter.com/RgWadamtvM
動画ではわかりにくかったですが、webUIボタン操作と移動の関係は以下です。
ひとまずはラジコンにしました。荒削りですがコンパクトに仕上がっています。
これはコトのはじめ、土台部分の確立です。今後「ライントレーサー（映像トリガーで動かす）」→「倒立振子（6軸センサートリガーで動かす）」 という具合にシリーズ化できたらなーと思っています。
ハードウェア構成
ハードウェア構成は以下のようになっています。細かな点は割愛します。
主要な構成物は以下の3つです
- RICOH THETA V
- M5 BALA
- Arduino Pro Micro
以降に、３つの主要構成物の役割などについて説明します。
RICOH THETA Vの役割
今回はラジコンですので、
- 内部にサーバーを立て、webUIからユーザーの指示を受付る
- 受け付けた指示にしたがい駆動命令を出す
というラジコンの頭脳部分を担当させました。
先々は、「映像をトリガーに駆動命令を出す」「姿勢センサーをトリガーに駆動命令を出す」というような変更ができるようにしています。
THETA V は、「GPS/GNSSレシーバー連携記事」、「M5Stack連携記事」、「音声認識モジュール連携記事」の事例のように、USB Host機能を利用して外部機器とシリアル通信を行うことができます。
この通信を利用して外部機器に駆動命令を出します。
M5 BALA の役割
M5 BALA の詳細はこちらをご参照ください。日本では、スイッチサイエンスさんなどでも販売しています。
本来は、M5 Stack FIRE/GO と組み合わせて駆動する倒立振子タイプの2輪車です。
こちらを、THETAの 下駄 駆動部にします。
現時点では補助輪の鉄球をとりつけ3輪車状態としています。補助輪の説明はのちほど。
M5 BALAにはATmega328Pというマイコンが搭載されており、このマイコンがI2C通信で外部からの指示を受け付け、指示にしたがったモータ駆動をします。
I2Cの端子は、天面のPogo pinだけでなく、本体側面のGROVEコネクタ側にも出ています。
I2C通信で必要な情報は、前述のM5 BALAに関するリンクから辿ってください。
サンプルプログラムなどもありますので理解できると思います。
Arduino Pro Micro の役割
駆動指示を受け付ける M5 BALAは I2C通信。一方、駆動指示を出すTHETA Vはシリアル通信。
M5 BALAに搭載されているATmega328Pのファーム書き換えができれば、FTDI等のUSB-シリアル変換ICを介してシリアル通信の接続ができそうだったのですが、ファーム書き換えが行えませんでした。
そこで、通信の仲介役として小型のArduinoマイコンであるPro Microを利用しました。
「HID連携の記事」でも登場しています。ATmega32U4というマイコンが搭載されており、USBシリアル通信がマイコン単体で行える点と小型な点が魅力です。
もちろん M5 Stack を使っても通信の仲介が可能なのですが、THETA Vが頭脳なのか M5 Stackが頭脳なのか、ハタからみている人がわからなくなるのを避けるため、このような構成としています。
Pro MicroとGROVEコネクタの結線は以下としています。
| Pro Micoro シルク印刷のマーク |
ケーブルの色 | GROVEコネクタ I2C通信の割り当て |
|---|---|---|
| GND | 黒 | GND |
| VCC | 赤 | VCC |
| 2 | 白 | SDA |
| 3 | 黄 | SCL |
組み立てのポイント
M5 BALAの補助輪
「鉄球」と「鉄球のホルダー」を探すのがむずかしいのかもしれませんので、品物名と参考リンクを書いておきますね。
あとはLEGOパーツを適当に組み合わせるとくめます。
今回の組み方については、以下画像をご参照ください。
オレンジ色のL字パーツの上方向の出っ張りや、本来はM5Stackと結合するための黒いジョイントパーツは、ミニ三脚を固定するのに都合がよいです。
ちなみに、M5 Stack本家ではより改善された組み方をしています。
Look good? pic.twitter.com/xac5mgzBfV
— M5Stack (@M5Stack) 2018年12月28日
たぶん以下がトリガーとなってスマートな組み立て方を編み出したのかとおもいます。
good idea!!
— M5Stack (@M5Stack) 2018年12月26日
RICOH THETA V の固定
補助輪をつけたM5 BALAにミニ三脚をテープ固定し、TE-1とTHETAを順に立てていくとわりと大丈夫です。（てきとーでスミマセン！）
あとで、M5 BALA に見栄えよくTHETA Vを載せられるパーツを3Dプリンタで誰か（チラっ）に作ってもらおうと思います。
TE-1を解さないとTHETA V底面のUSBポートにもろもろを取り付けられないので、TE-1をネジ留めできる板状のパーツになるのではないかとおもいます。
ソースコード
義体ができましたので、魂込める話になります。
義体の腰から下の「Arduino ProMicro」と、義体の頭脳となる「THETAプラグイン」の2段階にわけて記載します。
Arduino ProMicro
M5 StackからM5 BALAを操るためのサンプルコードを参考に Pro Micro (Arduino IDEからは Arduino Leonardoに見えます)のソースコードを記載しました。
上記サンプルコードの核はC++のソースコードになっている「M5Bala.cpp」です。
「M5Bala.cpp」では M5 Stack FIRE/GOに内蔵されている6軸センサーの値を元にPID制御で倒立振子の姿勢制御を行っているコードもありますが、今回は削除しています。
これらを参考に、今回作成した関数は以下のとおり。
現時点ではエンコーダー値は参考までに読んでいるだけで制御に使用していません。
| No |
関数名称 |
説明 |
|---|---|---|
| 1 | void setup() | Arduinoの定型起動処理 |
| 2 | void loop() | Arduinoの定型メインループ |
| 3 | String serialRead() | シリアル通信の受信をします |
| 4 | int splitParam( String inStr, int *param1, int *param2 ) | シリアル通信で受信したコマンドのパラメータを分割します |
| 5 | void setMotor(int16_t pwm0, int16_t pwm1) | 2つのモーターへ駆動指示をします |
| 6 | void readEncder() | 2つのモーターに取り付けられたエンコーダーの値を読み速度を計算します |
| 7 | void stop() | 2つのモータへの停止指示を出します |
| 8 | void move(int16_t speed, uint16_t duration ) | 2つのモータへ同量、durationで指定した時間[ms]だけ駆動指示を出します speedの正負により正転逆転を指定できます |
| 9 | void turn(int16_t speed, uint16_t duration ) | 引数speedの正負により、片側のモータをdurationで指定した時間[ms]だけ正転駆動します |
| 10 | void turn0(int16_t speed, uint16_t duration ) | モータ0をdurationで指定した時間[ms]だけ駆動します speedの正負により正転逆転を指定できます |
| 11 | void turn1(int16_t speed, uint16_t duration ) | モータ1をdurationで指定した時間[ms]だけ駆動します speedの正負により正転逆転を指定できます |
| 12 | void rotate(int16_t speed, uint16_t duration ) | モータ0と1を同量だけ逆転させる指示をdurationで指定した時間[ms]だけ行わせます speedの正負により回転方向を指定できます |
No3, No4 はシリアル通信に関する処理です。
No5, No6 はメインループから毎回呼び出され M5 BALA とI2C通信をする処理です。
No7～No12 はシリアル通信で受け付けたコマンドに対応する処理です。第一引数の「speed」の絶対値はモーター駆動時PWM値を決める数値で 0～255までの値をとれます。255がデューティー100(最大出力)になります。特にNo8～No12では、duration[ms]で指定した期間、ループで処理を滞留させている簡易的なdelay処理をおこなっています。この期間他の処理（シリアルやI2Cの送受信）が行われません（改善の余地があります）。
シリアル通信のコマンド体系は以下のようになっています。
| No |
コマンド |
説明 |
|---|---|---|
| 1 | go | テスト用コマンド move(80, 1000)を実行します |
| 2 | set 引数1 引数2 | set(引数1,引数2)を実行します 停止指示をしないと回り続けます |
| 3 | move 引数1 引数2 | move(引数1,引数2)を実行します |
| 4 | turn 引数1 引数2 | turn(引数1,引数2)を実行します |
| 5 | turn0 引数1 引数2 | turn0(引数1,引数2)を実行します |
| 6 | turn1 引数1 引数2 | turn1(引数1,引数2)を実行します |
| 7 | rotate 引数1 引数2 | rotate(引数1,引数2)を実行します |
| 8 | その他の入力 | 空白以外の入力で1～7に該当しないものは全てstop()を実行します 空白は無処理です |
ソースコード全文を畳んで掲載しておきます。
ファイル名は「LineTracer_Base.ino」としています。
「LineTracer_Base.ino」ソースコード全文
LineTracer_Base.ino
/**
* Copyright 2018 Ricoh Company, Ltd.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include <Wire.h>
#define M5GO_WHEEL_ADDR 0x56
#define MOTOR_CTRL_ADDR 0x00
#define ENCODER_ADDR 0x04
#define MOTOR_RPM 150
#define MAX_PWM 255
#define DEAD_ZONE 20
int16_t speed_input0=0;
int16_t speed_input1=0;
int motor0 = 0;
int motor1 = 0;
String serialRead();
int splitParam( String inStr, int *param1, int *param2 );
void setMotor(int16_t pwm0, int16_t pwm1);
void readEncder();
void stop();
void move(int16_t speed, uint16_t duration );
void turn(int16_t speed, uint16_t duration );
void turn0(int16_t speed, uint16_t duration );
void turn1(int16_t speed, uint16_t duration );
void rotate(int16_t speed, uint16_t duration );
void setup() {
Serial.begin(115200); // SERIAL
Wire.begin();
Wire.setClock(400000UL); // Set I2C frequency to 400kHz
delay(500);
setMotor( 0, 0 );
}
void loop() {
int speed=0;
int delay_ms=0;
String RcvCmd ="";
RcvCmd = serialRead();
RcvCmd.trim();
if ( !(RcvCmd.equals("")) ) {
Serial.print("rcv=["+RcvCmd + "]\n");
if ( RcvCmd.equals("go") ) {
move(80, 1000); // test move
} else if ( RcvCmd.startsWith("set ") ) {
RcvCmd.replace("set " , "");
splitParam( RcvCmd, &motor0, &motor1);
} else if ( RcvCmd.startsWith("move ") ) {
RcvCmd.replace("move ", "");
splitParam( RcvCmd, &speed, &delay_ms);
move( speed, delay_ms );
} else if ( RcvCmd.startsWith("turn ") ) { //右前 or 左前
RcvCmd.replace("turn ", "");
splitParam( RcvCmd, &speed, &delay_ms);
turn( speed, delay_ms );
} else if ( RcvCmd.startsWith("turn0 ") ) { // 右前 or 右後
RcvCmd.replace("turn0 ", "");
splitParam( RcvCmd, &speed, &delay_ms);
turn0( speed, delay_ms );
} else if ( RcvCmd.startsWith("turn1 ") ) { // 左前 or 左後
RcvCmd.replace("turn1 ", "");
splitParam( RcvCmd, &speed, &delay_ms);
turn1( speed, delay_ms );
} else if ( RcvCmd.startsWith("rotate ") ) {
RcvCmd.replace("rotate ", "");
splitParam( RcvCmd, &speed, &delay_ms);
rotate( speed, delay_ms );
} else {
stop();
}
}
readEncder();
//Serial.print("s0=" + String(speed_input0) + ", s1=" + String(speed_input1) + ", m0=" + String(motor0) + ", m1=" + String(motor1) + "\n");
setMotor( motor0, motor1 );
delay(100);
}
#define SERIAL_BUFF_BYTE 512
String serialRead(){
char sSerialBuf[SERIAL_BUFF_BYTE];
String result = "";
if ( Serial.available() > 0 ) {
int iPos=0;
while (Serial.available()) {
char c = Serial.read();
if ( c == '\n' ) {
break;
} else {
sSerialBuf[iPos] = c;
iPos++;
if (iPos==(SERIAL_BUFF_BYTE-1) ) {
break;
}
}
}
sSerialBuf[iPos] = 0x00;
result = String(sSerialBuf);
}
return result ;
}
int splitParam( String inStr, int *param1, int *param2 ) {
int ret = 0;
inStr.trim();
int len = inStr.length();
int pos = inStr.indexOf(' ', 0);
if ( (pos > 0) && (len>=3) ){
String Param1 = inStr.substring(0, pos);
String Param2 = inStr.substring(pos, len);
//Serial.print("Param1=" + Param1 + ", Param2=" + Param2 +"\n");
*param1 = Param1.toInt();
*param2 = Param2.toInt();
} else {
ret = -1;
}
return ret;
}
void setMotor(int16_t pwm0, int16_t pwm1) {
// Value range
int16_t m0 = constrain(pwm0, -255, 255);
int16_t m1 = constrain(pwm1, -255, 255);
// Dead zone
if (((m0 > 0) && (m0 < DEAD_ZONE)) || ((m0 < 0) && (m0 > -DEAD_ZONE))) m0 = 0;
if (((m1 > 0) && (m1 < DEAD_ZONE)) || ((m1 < 0) && (m1 > -DEAD_ZONE))) m1 = 0;
// Same value
static int16_t pre_m0, pre_m1;
if ((m0 == pre_m0) && (m1 == pre_m1))
return;
pre_m0 = m0;
pre_m1 = m1;
Wire.beginTransmission(M5GO_WHEEL_ADDR);
Wire.write(MOTOR_CTRL_ADDR); // Motor ctrl reg addr
Wire.write(((uint8_t*)&m0)[0]);
Wire.write(((uint8_t*)&m0)[1]);
Wire.write(((uint8_t*)&m1)[0]);
Wire.write(((uint8_t*)&m1)[1]);
Wire.endTransmission();
}
void readEncder() {
static float _speed_input0 = 0, _speed_input1 = 0;
int16_t rx_buf[2];
//Get Data from Module.
Wire.beginTransmission(M5GO_WHEEL_ADDR);
Wire.write(ENCODER_ADDR); // encoder reg addr
Wire.endTransmission();
Wire.beginTransmission(M5GO_WHEEL_ADDR);
Wire.requestFrom(M5GO_WHEEL_ADDR, 4);
if (Wire.available()) {
((uint8_t*)rx_buf)[0] = Wire.read();
((uint8_t*)rx_buf)[1] = Wire.read();
((uint8_t*)rx_buf)[2] = Wire.read();
((uint8_t*)rx_buf)[3] = Wire.read();
// filter
_speed_input0 *= 0.9;
_speed_input0 += 0.1 * rx_buf[0];
_speed_input1 *= 0.9;
_speed_input1 += 0.1 * rx_buf[1];
speed_input0 = constrain((int16_t)(-_speed_input0), -255, 255);
speed_input1 = constrain((int16_t)(_speed_input1), -255, 255);
}
}
void stop(){
motor0 = 0;
motor1 = 0;
}
void move(int16_t speed, uint16_t duration){
motor0 = speed;
motor1 = speed;
setMotor( motor0, motor1 );
if (duration != 0) {
delay(duration);
stop();
}
}
void turn(int16_t speed, uint16_t duration){
if (speed > 0) {
motor0 = speed;
motor1 = 0;
} else if (speed < 0) {
motor0 = 0;
motor1 = -speed;
}
setMotor( motor0, motor1 );
if (duration != 0) {
delay(duration);
stop();
}
}
void turn0(int16_t speed, uint16_t duration){
motor0 = speed;
motor1 = 0;
setMotor( motor0, motor1 );
if (duration != 0) {
delay(duration);
stop();
}
}
void turn1(int16_t speed, uint16_t duration){
motor1 = 0;
motor1 = speed;
setMotor( motor0, motor1 );
if (duration != 0) {
delay(duration);
stop();
}
}
void rotate(int16_t speed, uint16_t duration){
motor0 = speed;
motor1 = -speed;
setMotor( motor0, motor1 );
if (duration != 0) {
delay(duration);
stop();
}
}
THETAプラグイン
以下のソースコードはTHETA plugin SDKをベースとしています。
変更しているのはMainActivity.javaだけです。
・シリアル通信を使うための諸設定は「GPS/GNSSレシーバーの記事」
・webUIを使うための諸設定は「THETAプラグインのWeb UIの実装方法」
を参考にしてください。
シリアル通信をスレッドで行い、webUIも共存させるという大枠のプログラム構成は、「音声認識モジュール連携の記事」と同様です。
今回THETA Vと連携させる Pro Microというマイコンは、シリアル通信ライブラリに「ベンダーID、プロダクトID」が予め登録されているので上記記事と同様にコードが一部簡単になります。
今回、シリアル通信まわりで以下2点の「ハマったポイント」がありました。
(1) シリアル通信ライブラリで、ATmega32U4を利用したArduinoと通信する場合、固有の初期化処理が必要だった。
(2) ATmega32U4がデータを送ってきてないときにread()するとread()から処理が戻ってこない。
(1)については、onResume()内でシリアル通信のポートをopenした後、DTRとRTSの設定が必要でした。コードは以下のようになっています。下2行が固有の初期化処理です。
MainActivity.java
port.open(connection);
port.setParameters(115200, 8, UsbSerialPort.STOPBITS_1, UsbSerialPort.PARITY_NONE);
port.setDTR(true); // for Arduino(ATmega32U4)
port.setRTS(true); // for Arduino(ATmega32U4)
(2)については、まだ解消方法がわかっていません。
他のいくらかのシリアル通信機器でこのライブラリを利用した場合、相手がデータを送ってきてないときにread()をしても、戻り値=読み取りサイズが0で処理が戻ってきたのですが･･･
解消方法がわかる方いらっしゃいましたらおしえてくださいませ。
当面は Pro Micoroからの応答なしで事足りるのでこのまま進んでしまいます。
以下に「MainActivity.java」を全文を畳んで掲載しておきます。
「MainActivity.java」全文
MainActivity.java
/**
* Copyright 2018 Ricoh Company, Ltd.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package com.theta360.thetacar;
import android.content.Context;
import android.content.Intent;
import android.hardware.usb.UsbDeviceConnection;
import android.os.Bundle;
import android.util.Log;
import android.view.KeyEvent;
import com.theta360.pluginapplication.task.TakePictureTask;
import com.theta360.pluginapplication.task.TakePictureTask.Callback;
import com.theta360.pluginlibrary.activity.PluginActivity;
import com.theta360.pluginlibrary.callback.KeyCallback;
import com.theta360.pluginlibrary.receiver.KeyReceiver;
import com.theta360.pluginlibrary.values.LedColor;
import com.theta360.pluginlibrary.values.LedTarget;
//シリアル通信まわりで使用
import android.app.PendingIntent;
import android.hardware.usb.UsbManager;
import com.hoho.android.usbserial.driver.UsbSerialDriver;
import com.hoho.android.usbserial.driver.UsbSerialPort;
import com.hoho.android.usbserial.driver.UsbSerialProber;
import com.hoho.android.usbserial.driver.CdcAcmSerialDriver;
import com.hoho.android.usbserial.driver.ProbeTable;
//web server
import java.io.IOException;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import fi.iki.elonen.NanoHTTPD;
import fi.iki.elonen.NanoHTTPD.Response.Status;
public class MainActivity extends PluginActivity {
private static final String TAG = "LINETRACER";
//シリアル通信関連
private boolean mFinished;
private UsbSerialPort port ;
//USBデバイスへのパーミッション付与関連
PendingIntent mPermissionIntent;
private static final String ACTION_USB_PERMISSION = "com.android.example.USB_PERMISSION";
private TakePictureTask.Callback mTakePictureTaskCallback = new Callback() {
@Override
public void onTakePicture(String fileUrl) {
}
};
@Override
protected void onCreate(Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
setContentView(R.layout.activity_main);
// Set enable to close by pluginlibrary, If you set false, please call close() after finishing your end processing.
setAutoClose(true);
// Set a callback when a button operation event is acquired.
setKeyCallback(new KeyCallback() {
@Override
public void onKeyDown(int keyCode, KeyEvent event) {
if (keyCode == KeyReceiver.KEYCODE_CAMERA) {
/*
* To take a static picture, use the takePicture method.
* You can receive a fileUrl of the static picture in the callback.
*/
//new TakePictureTask(mTakePictureTaskCallback).execute();
sendReq = true;
}
}
@Override
public void onKeyUp(int keyCode, KeyEvent event) {
/**
* You can control the LED of the camera.
* It is possible to change the way of lighting, the cycle of blinking, the color of light emission.
* Light emitting color can be changed only LED3.
*/
notificationLedBlink(LedTarget.LED3, LedColor.BLUE, 1000);
}
@Override
public void onKeyLongPress(int keyCode, KeyEvent event) {
}
});
//webUI用のサーバー開始処理
this.context = getApplicationContext();
this.webServer = new WebServer(this.context);
try {
this.webServer.start();
} catch (IOException e) {
e.printStackTrace();
}
}
@Override
protected void onResume() {
super.onResume();
if (isApConnected()) {
}
//WlanをAPモードから開始する
notificationWlanAp(); // プラグイン起動前からAPモードでVysorのWLANデバッグしていても影響なし
//--------------- added code ---------------
mFinished = true;
// Find all available drivers from attached devices.
UsbManager manager = (UsbManager) getSystemService(Context.USB_SERVICE);
List<UsbSerialDriver> usb = UsbSerialProber.getDefaultProber().findAllDrivers(manager);
//定義済みのArduinoを利用するため、新たな定義を追加する必要がない
//final ProbeTable probeTable = UsbSerialProber.getDefaultProbeTable();
//probeTable.addProduct(0x2341,0x8036,CdcAcmSerialDriver.class);
//List<UsbSerialDriver> usb = new UsbSerialProber(probeTable).findAllDrivers(manager);
if (usb.isEmpty()) {
int usb_num = usb.size();
Log.d(TAG,"usb num =" + usb_num );
Log.d(TAG,"usb device is not connect." );
//return;
//port = null;
} else {
// デバッグのため認識したデバイス数をしらべておく
int usb_num = usb.size();
Log.d(TAG,"usb num =" + usb_num );
// Open a connection to the first available driver.
UsbSerialDriver driver = usb.get(0);
//USBデバイスへのパーミッション付与用（機器を刺したときスルーしてもアプリ起動時にチャンスを与えるだけ。なくても良い。）
mPermissionIntent = PendingIntent.getBroadcast(this, 0, new Intent(ACTION_USB_PERMISSION), 0);
manager.requestPermission( driver.getDevice() , mPermissionIntent);
UsbDeviceConnection connection = manager.openDevice(driver.getDevice());
if (connection == null) {
// You probably need to call UsbManager.requestPermission(driver.getDevice(), ..)
// パーミッションを与えた後でも、USB機器が接続されたままの電源Off->On だとnullになる... 刺しなおせばOK
Log.d(TAG,"M:Can't open usb device.\n");
port = null;
} else {
port = driver.getPorts().get(0);
try {
port.open(connection);
port.setParameters(115200, 8, UsbSerialPort.STOPBITS_1, UsbSerialPort.PARITY_NONE);
port.setDTR(true); // for arduino(ATmega3U4)
port.setRTS(true); // for arduino(ATmega3U4)
port.purgeHwBuffers(true,true);//念のため
Log.d(TAG,"CD - Carrier Detect =" + String.valueOf( port.getCD() ) );
Log.d(TAG,"CTS - Clear To Send =" + String.valueOf( port.getCTS() ) );
Log.d(TAG,"DSR - Data Set Ready =" + String.valueOf( port.getDSR() ) );
Log.d(TAG,"DTR - Data Terminal Ready=" + String.valueOf( port.getDTR() ) );
Log.d(TAG,"RI - Ring Indicator =" + String.valueOf( port.getRI() ) );
Log.d(TAG,"RTS - Request To Send =" + String.valueOf( port.getRTS() ) );
mFinished = false;
start_read_thread();
} catch (IOException e) {
// Deal with error.
e.printStackTrace();
Log.d(TAG, "M:IOException");
//return;
}
}
}
//-----------------------------------------
}
@Override
protected void onPause() {
// Do end processing
//close();
//--------------- added code ---------------
//スレッドを終わらせる指示。終了待ちしていません。
mFinished = true;
//シリアル通信の後片付け ポート開けてない場合にはCloseしないこと
if (port != null) {
try {
port.close();
Log.d(TAG, "M:onDestroy() port.close()");
} catch (IOException e) {
Log.d(TAG, "M:onDestroy() IOException");
}
} else {
Log.d(TAG, "M:port=null\n");
}
//-----------------------------------------
super.onPause();
}
boolean sendReq = false;
int moveCommandNo = 0;
private static final String BALA_STOP = "stop\n";
private static final String BALA_FORWARD = "move 128 1000\n";
private static final String BALA_BACK = "move -128 1000\n";
private static final String BALA_TURN_R = "turn 128 200\n";
private static final String BALA_TURN_L = "turn -128 200\n";
private static final String BALA_TURN_RF = "turn0 128 200\n";
private static final String BALA_TURN_RB = "turn0 -128 200\n";
private static final String BALA_TURN_LF = "turn1 128 200\n";
private static final String BALA_TURN_LB = "turn1 -128 200\n";
private static final String BALA_ROTATE_R = "rotate 128 800\n";
private static final String BALA_ROTATE_L = "rotate -128 800\n";
//=====================================================
//<<< Serial thread >>>
//=====================================================
//シリアル受信スレッド
boolean readFlag = false;
public void start_read_thread(){
new Thread(new Runnable(){
@Override
public void run() {
try {
//notificationLedBlink(LedTarget.LED3, LedColor.MAGENTA, 500);
Log.d(TAG, "Thread Start");
while(mFinished==false){
//Log.d(TAG, "mFinished=" + String.valueOf(mFinished));
//シリアル通信 受信ポーリング部
// ProMicro(ATmega32U4)は 受信していないときに read()をすると戻ってきません。
if ( readFlag ) {
readFlag = false;
byte buff[] = new byte[256];
int num=0;
try {
num= port.read(buff, buff.length);
} catch (IOException e) {
e.printStackTrace();
Log.d(TAG, "T:read() IOException");
}
if ( num > 0 ) {
String rcvStr = new String(buff, 0, num);
rcvStr = rcvStr.trim();
Log.d(TAG, "len=" + rcvStr.length() + ", RcvDat=[" + rcvStr + "]" );
}
}
//シリアル送信
if (sendReq == true){
sendReq = false;
switch (moveCommandNo) {
case 1 :
Log.d(TAG, BALA_FORWARD);
port.write(BALA_FORWARD.getBytes(), BALA_FORWARD.length());
break;
case 2 :
Log.d(TAG, BALA_BACK);
port.write(BALA_BACK.getBytes(), BALA_BACK.length());
break;
case 3 :
Log.d(TAG, BALA_TURN_R);
port.write(BALA_TURN_R.getBytes(), BALA_TURN_R.length());
break;
case 4 :
Log.d(TAG, BALA_TURN_L);
port.write(BALA_TURN_L.getBytes(), BALA_TURN_L.length());
break;
case 5 :
Log.d(TAG, BALA_ROTATE_R);
port.write(BALA_ROTATE_R.getBytes(), BALA_ROTATE_R.length());
break;
case 6 :
Log.d(TAG, BALA_ROTATE_L);
port.write(BALA_ROTATE_L.getBytes(), BALA_ROTATE_L.length());
break;
case 7 :
Log.d(TAG, BALA_TURN_RF);
port.write(BALA_TURN_RF.getBytes(), BALA_TURN_RF.length());
break;
case 8 :
Log.d(TAG, BALA_TURN_RB);
port.write(BALA_TURN_RB.getBytes(), BALA_TURN_RB.length());
break;
case 9 :
Log.d(TAG, BALA_TURN_LF);
port.write(BALA_TURN_LF.getBytes(), BALA_TURN_LF.length());
break;
case 10 :
Log.d(TAG, BALA_TURN_LB);
port.write(BALA_TURN_LB.getBytes(), BALA_TURN_LB.length());
break;
default:
Log.d(TAG, BALA_STOP);
port.write(BALA_STOP.getBytes(), BALA_STOP.length());
break;
}
readFlag = true;
}
//ポーリングが高頻度になりすぎないよう10msスリープする
Thread.sleep(10);
}
Log.d(TAG, "Thread End");
} catch (IOException e) {
// Deal with error.
e.printStackTrace();
Log.d(TAG, "T:IOException");
} catch (InterruptedException e) {
// Deal with error.
e.printStackTrace();
Log.d(TAG, "T:InterruptedException");
}
}
}).start();
}
//-----------------------------------------
String myPluginName = "THETA Car";
//=====================================================
//<<< web server processings >>>
//=====================================================
private Context context;
private WebServer webServer;
protected void onDestroy() {
super.onDestroy();
if (this.webServer != null) {
this.webServer.stop();
}
}
private class WebServer extends NanoHTTPD {
private static final int PORT = 8888;
private Context context;
public WebServer(Context context) {
super(PORT);
this.context = context;
}
@Override
public Response serve(IHTTPSession session) {
Method method = session.getMethod();
String uri = session.getUri();
switch (method) {
case GET:
return this.serveHtml(uri);
case POST:
Map<String, List<String>> parameters = this.parseBodyParameters(session);
Log.d(TAG, "parameters=" + parameters.toString() );
execButtonAction(parameters);
return this.serveHtml(uri);
default:
return newFixedLengthResponse(Status.METHOD_NOT_ALLOWED, "text/plain",
"Method [" + method + "] is not allowed.");
}
}
private Map<String, List<String>> parseBodyParameters(IHTTPSession session) {
Map<String, String> tmpRequestFile = new HashMap<>();
try {
session.parseBody(tmpRequestFile);
} catch (IOException e) {
e.printStackTrace();
} catch (ResponseException e) {
e.printStackTrace();
}
return session.getParameters();
}
private Response serveHtml(String uri) {
String html="";
switch (uri) {
case "/":
html = editHtml();
return newFixedLengthResponse(Status.OK, "text/html", html);
default:
html = "URI [" + uri + "] is not found.";
return newFixedLengthResponse(Status.NOT_FOUND, "text/plain", html);
}
}
public static final String buttonName1 = " Forward " ;
public static final String buttonName2 = " Back " ;
public static final String buttonName3 = "Right Forward" ;
public static final String buttonName4 = "Left Forward" ;
public static final String buttonName5 = "Right Rotation" ;
public static final String buttonName6 = "Left Rotation" ;
public static final String buttonName7 = " Right Back " ;
public static final String buttonName8 = " Left Back " ;
private void execButtonAction( Map<String, List<String>> inParameters ) {
if (inParameters.containsKey("button")) {
List<String> button = inParameters.get("button");
Log.d(TAG, "button=" + button.toString() );
if ( button.get(0).equals(buttonName1) ) {
moveCommandNo = 1;
sendReq = true;
} else if ( button.get(0).equals(buttonName2) ) {
moveCommandNo = 2;
sendReq = true;
} else if ( button.get(0).equals(buttonName3) ) {
moveCommandNo = 7;
sendReq = true;
} else if ( button.get(0).equals(buttonName4) ) {
moveCommandNo = 9;
sendReq = true;
} else if ( button.get(0).equals(buttonName5) ) {
moveCommandNo = 5;
sendReq = true;
} else if ( button.get(0).equals(buttonName6) ) {
moveCommandNo = 6;
sendReq = true;
} else if ( button.get(0).equals(buttonName7) ) {
moveCommandNo = 8;
sendReq = true;
} else if ( button.get(0).equals(buttonName8) ) {
moveCommandNo = 10;
sendReq = true;
}
}
}
private String editHtml() {
String html = "";
html += "<html>";
html += "<head>";
//html += " <meta name='viewport' content='width=device-width,initial-scale=1'>";
html += " <meta name='viewport' content='width=480,initial-scale=0.7'>";
html += " <title>" + myPluginName + " : M5 BALA Controler</title>";
html += " <script type='text/javascript'>";
html += " </script>";
html += "</head>";
html += "<body>";
html += "";
html += "<form action='/' method='post' name='SettingForm'>";
html += " <hr>";
html += " <h2>[" + myPluginName + " : M5 BALA Controler]</h2>";
html += " <hr>";
html += " <table>";
html += " <tr>";
html += " <td> </td>";
html += " <td> </td>";
html += " <td><input type='submit' name='button' value='" + buttonName1 + "'></td>";
html += " <td> </td>";
html += " <td> </td>";
html += " </tr>";
html += " <tr>";
html += " <td> <br> </td>";
html += " <td> </td>";
html += " <td> </td>";
html += " <td> </td>";
html += " <td> </td>";
html += " </tr>";
html += " <tr>";
html += " <td><input type='submit' name='button' value='" + buttonName4 + "'></td>";
html += " <td> </td>";
html += " <td> </td>";
html += " <td> </td>";
html += " <td><input type='submit' name='button' value='" + buttonName3 + "'></td>";
html += " </tr>";
html += " <tr>";
html += " <td> <br><br> </td>";
html += " <td> </td>";
html += " <td> </td>";
html += " <td> </td>";
html += " <td> </td>";
html += " </tr>";
html += " <tr>";
html += " <td><input type='submit' name='button' value='" + buttonName6 + "'></td>";
html += " <td> </td>";
html += " <td> </td>";
html += " <td> </td>";
html += " <td><input type='submit' name='button' value='" + buttonName5 + "'></td>";
html += " </tr>";
html += " <tr>";
html += " <td> <br><br> </td>";
html += " <td> </td>";
html += " <td> </td>";
html += " <td> </td>";
html += " <td> </td>";
html += " </tr>";
html += " <tr>";
html += " <td><input type='submit' name='button' value='" + buttonName8 + "'></td>";
html += " <td> </td>";
html += " <td> </td>";
html += " <td> </td>";
html += " <td><input type='submit' name='button' value='" + buttonName7 + "'></td>";
html += " </tr>";
html += " <tr>";
html += " <td> <br> </td>";
html += " <td> </td>";
html += " <td> </td>";
html += " <td> </td>";
html += " <td> </td>";
html += " </tr>";
html += " <tr>";
html += " <td> </td>";
html += " <td> </td>";
html += " <td><input type='submit' name='button' value='" + buttonName2 + "'></td>";
html += " <td> </td>";
html += " <td> </td>";
html += " </tr>";
html += " </table>";
html += " <hr>";
html += "</form>";
html += "";
html += "</body>";
html += "</html>";
return html;
}
}
}
まとめ
雑な点はたくさんあるものの。。。（汗
わりとお手軽にTHETA Vをラジコン化できました。
モータ制御に関していうと、エンコーダーをみて、「左右回転数を揃える」「移動量を時間でなくパルス数で指示する」「いきなり通電でなく加減速制御する」とかもしたいところではありますが、とりあえずのオープンループな雑制御としています。
こんな状態でも、自身が動いた結果を、映像で捉え、次の動きを決定する、というクローズドループ制御をするとライントレーサーになるサマを次回にお見せしたいと思います。
その際、映像の認識については、「THETAの中でOpenCVを動かす【プレビューフレーム取得編】」が参考になりそうです。
だけど、、、OpenCV必要ないかも、、、のあたりはやりながら探っていこうと思います。
倒立振子については、追い込みの時間がひつようそうなので次回記事以降、ちょっと間があくとおもいます。先日購入したトラ技2019年7月号が役に立ちそうです。
（よ、よみとけるだろうか・・・）
もろもろの雑さをクリアして、THETAのCLモードを利用したwebRTCなどの通信とも絡めると、移動可能なテレイグジスタンスの世界が少ない機材構成で実現できそうなこともお分かりいただけたのではないでしょうか。
THETA SやSCの時代ではもっとコンピューターボードやらマイコンがたくさん必要だったかと思います。
THETAプラグイン、なかなか使えそうでしょ？
THETAプラグイン×M5BALA Part2：ライントレースするへと続きます
RICOH THETAプラグインパートナープログラムについて
THETAプラグインをご存じない方はこちらをご覧ください。
パートナープログラムへの登録方法はこちらにもまとめてあります。
興味を持たれた方はTwitterのフォローとTHETAプラグイン開発コミュニティ(Slack)への参加もよろしくおねがいします｡