Arduino Tank Robot

100均セリアで Arduino UNOとMotor Shield が丁度入る カードケースを見つけたので、小型のロボットタンクのシャシーの上に載せてみた。
シャシーとケースとの間は、M3のネジで接合してある。
ボディの前方は、超音波測定器(HC-SR04)を設置し、前方の障害物との距離を計測する。
HC-SR04は、ミニブレッドボードを使い取り付けた。ミニブレッドボードの底面の突起を削って平らにして、サーボモータのアーム部分に両面テープで接着し、表側のピン穴を使ってHC-SR04を差し込み取り付けたのだ。
距離の測定には、「NewPing」ライブラリーを使った。

ロボットが動いている時に「距離をどのように計測し、それから何を判断したか」を、僕が見て分かるようにしたいと思い、TM1637　４桁7セグメント表示器を付けた。 表示器は４桁なので、基本的に、左１桁は、進行方向に対する判断結果表示を、残り３桁は、計測した方向にある障害物との距離をcmで表示するようにした。
このような仕掛けを組み込んだので、不具合発生時、問題の調査が行いやすかった。 

表示内容は、７セグメントでのASCII文字表示に従い表示するようにした。
①前方障害物との距離・・・計測した距離の単位はｃｍで、3桁で表示
②モーター制御の方向
　前進（１桁目に「F」表示 及び 前方障害物との距離を３桁で表示）、停止（「HALt」と表示）、後退（「ｂAc」と表示）、左方向へ後退（「LbAc」と表示）、右方向へ後退（「ｒｂAc」と表示）
③左右方向の余裕距離・・・計測した距離の単位はｃｍで表示
　左側より右側が広く空いている場合は、１桁目に ＜ のASCII文字表示で示し、距離を3桁で表示。
　右側より左側が広く空いている場合には、１桁目に ＞ のASCII文字で示し、同様に距離を３桁で表示した。

　注意）事例として、ロボットが一旦停止して表示する１桁目が「＜」の表示の場合は、右側が左側より広く空いているので、左方向にバックしてから直進し、右方向に進む。

部品構成
　Arduino：UNO
　Motor Shield：Rasbee L298P  @591
　Sensor：HC-SR04 超音波センサーモジュール
　ServoMotor：Lemcos MG 996R(メタルギヤ デジタルRCサーボモーター) @480
　Battery：KEEPPOWER IMR 18650サイズ互換 2600mAh リチウムマンガンバッテリー 
　Display：0.36インチ(42X24X12mm) ４桁7セグメント (ドライブチップはTM1637 )  @188
　本体ケース：100均セリアのカードケース
　シャシー：KKmoon ロボットタンク クローラーシャーシ 　@2499
　Switch：オン/オフSPDTミニ2ポジションラッチトグルスイッチ
その他
　Battery充電器：XTAR VC2S USB 急速 電池 マルチサイズ対応 充電器
　基板の固定用として、六角ナットねじスペーサ(長さ10mmのものを使用)
など

本体に入れた頭脳となる基板は、 arduino UNO の上に arduino専用の L298P motor shield を載せた２層構造にしてある。
電池は、カードケースの裏蓋の中に収めることが出来た(両面テープで電池フォルダーを接着)。
カードケース内の高さは、それほど余裕が無いので、ピン端子と干渉しないように注意して配置する必要がある。
接続ピンは、Motor Shieldカードの左右に配置されると思うので、Battery Holder はなるべく中央部に配置するのが良いと思う。
USB端子と電源の両方を使えるように、ケースに穴を開け、ポートアクセスが可能なようにした。
バッテリ電源用ON/OFFスイッチとして、スナップスイッチを設置した。


配線接続について
１）Motor Shield を使う場合には、PWMピンは使用方法に指定があるので、そのように使わないと制御できない。
　（参考：L298P　2-A Dual H-Bridge Motor Shield for Arduino .pdf)
　①詳細は、上記pdfを確認する事。表にして記載されている。
　②2つのブラシ付きDCモーターを、本シールドのモーターAとモーターBの端子に接続する事で使用できる。
　③方向制御AはD12ピン、方向制御BはD13ピンをHIGHまたはLOWにすることで、モーターの方向を制御できる。
　④D10ピンとD11ピンにより、モーターの速度を制御できる。
　⑤従って、D10～D13ピンは内部で使われるので、別の目的のピンをD10~D13には接続しない事。



２）センサー用サーボモーターの接続は、Motor Shield の「SERVO(G +5 ９)」と書かれたピン(Pin9)に刺す。
　　写真では、G（GND)は、茶色の配線
　　　　　　　＋５は、赤色の配線
　　　　　　　９　は、オレンジ色の配線


３）超音波センサー(HC-SR04)のピン接続は、Echo－Pin5、Trig－Pin6 に刺す。Motor Shield 側のピン接続部分は、オス用・メス用両方が用意されているのでどちらを使ってもよい。+5V電源とGNDピンは、接続しやすく、バッテリーなどと干渉しない部分を使えばよい。
　　写真では、GND は　青色の配線
　　　　　　　Echo は　黄色の配線
　　　　　　　Trig は　緑色の配線
　　　　　　　Vcc は　赤色の配線

４）４桁7セグメント表示器の接続は、
　CLK　－　Pin７(写真では黄色の配線)
　DIO　－　Pin８(写真ではオレンジの配線)
　VCC　－ 接続しやすい５V端子へ接続（写真では赤色の配線）
　GND － 接続しやすいGND端子へ接続（写真では茶色の配線）

　４桁7セグメント表示器のケースへの取り付けは、上の写真のように２本のボルトで止めてある。

５）バッテリー、スイッチ、Motor Shield 外部電源端子間を接続

《Sketch》

// Obstacle avoidance Tank Robot program 2019/08/03
// New algorithm version My Spice Cabinet

#include <NewPing.h>
const int PIN_SONAR_ECHO = 5; // define pin for sensor echo
const int PIN_SONAR_TRIGGER = 6; // define pin for sensor trig
const int MAX_DISTANCE = 200;
NewPing sonar(PIN_SONAR_TRIGGER, PIN_SONAR_ECHO, MAX_DISTANCE);
#include <Servo.h>

#include <TM1637Display.h>
#define CLK 7 //can be any digital pin
#define DIO 8 //can be any digital pin
double Fdistance = 0;
double Dlh;
double Drh;
double Ls = 30; // short range threshold
double Ll = 55; // long range threshold
int angle;
int pinSRB= 9; // servo motor output pin (PWM)

// next pins(10~13) are depended on L298P motorshield
int pinLB = 12; // direction control function
int pinLF = 10; // motors' control chip with speed control function from 10
int pinRB = 13; // direction control function
int pinRF = 11; // motors' control chip with speed control function

int FullSpeed = 255; // max 255
int HalfSpeed = 233;
Servo myServo; // set myservo

TM1637Display display(CLK, DIO);
const uint8_t Letter_F[] = { SEG_A | SEG_E | SEG_F | SEG_G };              // F
const uint8_t Letter_b[] = { SEG_C | SEG_D | SEG_E | SEG_F | SEG_G }; // b
const uint8_t Letter_H[] = { SEG_B | SEG_C | SEG_E | SEG_F | SEG_G }; // H
const uint8_t Letter_r[] = { SEG_E | SEG_G };              // r
const uint8_t Letter_L[] = { SEG_D | SEG_E | SEG_F }; // L
const uint8_t Letter_A[] = { SEG_A | SEG_B | SEG_C | SEG_E | SEG_F | SEG_G }; // A
const uint8_t Letter_c[] = { SEG_D | SEG_E | SEG_G };             // c
const uint8_t Letter_t[] = { SEG_D | SEG_E | SEG_F | SEG_G }; // t
const uint8_t Letter_LT[] = { SEG_A | SEG_F | SEG_G }; // <
const uint8_t Letter_GT[] = { SEG_A | SEG_B | SEG_G }; // >

void setup()
{
Serial.begin(9600);
display.setBrightness(0x0f);   // 0x01 << dark　bright >> 0x0f
display.clear();
// Define motor output pin
pinMode(pinLB,OUTPUT);  // pin 12 direction control function
pinMode(pinLF,OUTPUT);   // pin 10 (PWM)
pinMode(pinRB,OUTPUT);  // pin 13 direction control function
pinMode(pinRF,OUTPUT);   // pin 11 (PWM)
myServo.attach(pinSRB);   // Define servo motor output pin(PWM)
}

void Advance() // move to forward
{
display.setSegments(Letter_F,1,0);
digitalWrite(pinLB,LOW); // left wheel moves forward
digitalWrite(pinRB,LOW); // right wheel moves forward
analogWrite(pinLF,FullSpeed);
analogWrite(pinRF,FullSpeed);
}

void Back() // move backward
{
display.clear();
display.setSegments(Letter_b,1,1);
display.setSegments(Letter_A,1,2);
display.setSegments(Letter_c,1,3);
delay(50);
digitalWrite(pinLB,HIGH);   // motor moves to left rear
digitalWrite(pinRB,HIGH);   // motor moves to right rear
analogWrite(pinLF,FullSpeed);
analogWrite(pinRF,FullSpeed);
}

void MidSpeed() // move to forward
{
display.setSegments(Letter_F,1,0);
digitalWrite(pinLB,LOW);   // left wheel moves forward
digitalWrite(pinRB,LOW);   // right wheel moves forward
analogWrite(pinLF,HalfSpeed);
analogWrite(pinRF,HalfSpeed);
}

void Stopp() // all wheel stop
{
display.setSegments(Letter_H,1,0);
display.setSegments(Letter_A,1,1);
display.setSegments(Letter_L,1,2);
display.setSegments(Letter_t,1,3);
digitalWrite(pinLB,HIGH);   // left wheel stop
digitalWrite(pinRB,HIGH);   // right wheel stop
analogWrite(pinLF,0);
analogWrite(pinRF,0);
}

void BackRight() // turn right
{
display.setSegments(Letter_r,1,0);
display.setSegments(Letter_b,1,1);
display.setSegments(Letter_A,1,2);
display.setSegments(Letter_c,1,3);
digitalWrite(pinLB,HIGH);
digitalWrite(pinRB, LOW);       // wheel on the right moves backward
analogWrite(pinLF,FullSpeed); // from FullSpeed
analogWrite(pinRF,FullSpeed); // from FullSpeed
}

void BackLeft() // turn left
{
display.setSegments(Letter_L,1,0);
display.setSegments(Letter_b,1,1);
display.setSegments(Letter_A,1,2);
display.setSegments(Letter_c,1,3);
digitalWrite(pinLB, LOW);   // wheel on the left moves backward
digitalWrite(pinRB,HIGH);
analogWrite(pinLF,FullSpeed);
analogWrite(pinRF,FullSpeed);
}

void loop()
{
labelF:
// --- Forward obstacle detection and moving ---
angle = 90;
ask_pin_F();   // Fdistance
if(Fdistance <= Ls)
{
Stopp();         // stop for a while
goto label2;
}
if((Fdistance > Ls) & (Fdistance <= Ll))
{
MidSpeed();
goto labelF;
}
if(Fdistance > Ll)
{
Advance();
goto labelF;
}

label2:
angle = 90;   // Check the distance ahead again
ask_pin_F();
if(Fdistance < Ls )
{
  Back();         // go back a little
  if(Fdistance < 30 ) delay( 50);
  if(Fdistance < 20 ) delay(100);
  if(Fdistance < 10 ) delay(100);
  Stopp();
}
// Measure left and right space
angle = 3;
ask_pin_F();
Dlh=Fdistance;
angle = 177;
ask_pin_F();
Drh=Fdistance;

if(Dlh < Drh)
{
display.setSegments(Letter_LT,1,0);
delay(1000);
BackLeft();
delay(600);
Stopp();
delay(400);
goto labelF;
}
//
else
{
display.setSegments(Letter_GT,1,0);
delay(1000);
BackRight();
delay(600);
Stopp();
delay(400);
goto labelF;
}
}

void ask_pin_F()   // Measure the distance at the specified angle
{
myServo.write(angle);
delay(10);
unsigned long microSec = sonar.ping_median();
unsigned long distance = sonar.convert_cm(microSec);
Fdistance = distance;
display.showNumberDec(distance / 100,true, 1, 1);
display.showNumberDec(int( distance - int(distance/100)*100 )/10,false, 1, 2);
display.showNumberDec(int( distance - int(distance/10)*10 ),false, 1, 3);
delay(30);
}

《備考》
参考としたブログなど
１）http://www.handsontec.com/dataspecs/arduino-shield/L298P%20Motor%20Shield.pdf
　　L298P Motor Shield の使い方のマニュアル

２）https://iot.keicode.com/arduino/arduino-sonar-hc-sr04.php
　　超音波センサーの使い方　NewPing の使い方など

《あとがき》
　前回のブログでは、Motor Shield に問題があって中断とした。既に廃却してしまったので検証は出来ないが、もしかすると、モータを低速で回転させたいと思って選んだ制御用数値が、使えない範囲のものを選んだ可能性が高い。また、L298P Motor Shield はPWMピンとして使えるものが、前述の配線接続についての項目の最初の絵にある表のように指定されているので、それを守らないと制御できない。
　超音波センサーの位置が高い場所に設置してしまった事と、シャシーの幅が広いので･･･（低い位置に設置する良いアイデアが思い浮かばなかったので･･･）時々、低い場所にある障害物に衝突して、動けなくなることがある。Sketchを改善するかセンサーの位置を変更するなどする必要がありそう
　Sketchの中で「ｌ(Lの小文字)」と「｜(理論和の記号)」の違いが分かりにくいが、Sketch の中で「｜(理論和の記号)」を使っているのは、TM1637のDisplay設定の中だけである。他の部分は全てLの小文字「l」である。
　LsとLlの閾値を変えると、タンクの挙動も大きく変わってくる。遊ばれる環境で数値を変えてみるといい。
　前回記載のタンクロボットは、Sketchも含めて今回のものに改新したので、前回のブログも抹消する事にした。