技術的小話【その12】超音波センサをフル活用？自動で中央へ向かってみた

こんにちは、junjunです！
みなさんお盆はどのように過ごされましたか？ 

私は数年ぶりに母方の実家に帰るなどして田舎LIFEを満喫しておりました！
…さて今回は、以前のテーマでも一部使われていた「超音波センサ」を使った制御になります。

↓NEXUS ROBOTの製品情報はこちら
http://www.vstone.co.jp/products/nexusrobot/index.html

NEXUS 10014には3つの超音波センサが等間隔に搭載されています。
今回はこれらを全て使って、自分がいる空間の中心へ自動で向かうような制御をしてみました！

思い付き程度のアルゴリズムなので、動きは見て頂ける通りとても荒いものですが…
参考までに処理手順を以下にまとめておきます。

①3つの超音波センサの値を取得
②3つの中で一番値が大きい(距離が遠い)センサの方向に一瞬進む
③モータを止めて①に戻る

3輪のうち1輪がなんとも調子が良くない状態でしたが、たったこれだけの処理でも結構動いてくれましたね(笑)

この動きをもっとよくするためには…

・離れた距離に応じてモータの出力を変える
・センサのある程度の誤差(ブレ)は許容して止まるようにする

などなど色々考えられると思います。
どうすれば改善されるか、皆さんも制御の流れを想像してみてくださいね！

使用したスケッチはコチラ
———————————————
#include 
#include 
#include 

unsigned long currMillis=0;
SONAR sonar11(0x11),sonar12(0x12),sonar13(0x13);

#define MOTOR1 9
#define MOTOR2 10
#define MOTOR3 7
#define CW 0
#define STOP 1
#define CCW 2

#define S11 distBuf[0]
#define S12 distBuf[1]
#define S13 distBuf[2]

char input[1];
int pos = 0; // サーボのポジション（変数）
int target = 0; // サーボのポジション（変数）
String inString = “”; // 受信文字列用のバッファ

Servo wheels[3];
unsigned char motor1_Ctrl[3] = {70,83,100};
unsigned char motor2_Ctrl[3] = {0,90,180};
unsigned char motor3_Ctrl[3] = {70,88,100};
unsigned short distBuf[3];

void motorDrive(int id, int val){
switch(id){
case 1:
wheels[0].write(val);
break;
case 2:
wheels[1].write(val);
break;
case 3:
wheels[2].write(val);
break;
}
}

int sonarUpdate() {
static unsigned char sonarCurr=1;
if(sonarCurr==3) sonarCurr=1;
else ++sonarCurr;
if(sonarCurr==1) { 
distBuf[1]=sonar12.getDist(); 
sonar12.trigger(); 
sonar12.showDat(); 
} else if(sonarCurr==2) {
distBuf[2]=sonar13.getDist();
sonar13.trigger();
sonar13.showDat();
} else { 
distBuf[0]=sonar11.getDist();
sonar11.trigger();
sonar11.showDat();
}
return sonarCurr;
}

void setup(){
wheels[0].attach(MOTOR1);
wheels[1].attach(MOTOR2);
wheels[2].attach(MOTOR3);

motorDrive(1, motor1_Ctrl[STOP]);
motorDrive(2, motor2_Ctrl[STOP]);
motorDrive(3, motor3_Ctrl[STOP]);

SONAR::init(); //call the init() from SONAR.h;

//Serial.begin(115200);

}

void demowithSosars(){
unsigned char sonarcurrent=0;
if(millis()-currMillis>50/*SONAR::duration*/){ //judge if the time more than SONAR::duration;
currMillis=millis();
sonarcurrent= sonarUpdate(); //if the requirement was ture call the function; 
}
if(sonarcurrent==3){ 
Serial.print(“s11:”);
Serial.print(distBuf[0]);
Serial.print(” s12:”);
Serial.print(distBuf[1]);
Serial.print(” s13:”);
Serial.println(distBuf[2]);
keepCenter();
delay(800);
} 
}

void keepCenter(){
sonarUpdate();
/*
wheels[0].attach(MOTOR1);
wheels[1].attach(MOTOR2);
wheels[2].attach(MOTOR3);
motorDrive(1, motor1_Ctrl[STOP]);
motorDrive(2, motor2_Ctrl[STOP]);
motorDrive(3, motor3_Ctrl[STOP]);
*/
switch( compareSonarValue() ){
case 11:
motorDrive(1, motor1_Ctrl[STOP]);
motorDrive(2, motor2_Ctrl[CW]);
motorDrive(3, motor3_Ctrl[CCW]);
break;
case 12:
motorDrive(1, motor1_Ctrl[CCW]);
motorDrive(2, motor2_Ctrl[STOP]);
motorDrive(3, motor3_Ctrl[CW]);
break;
case 13:
motorDrive(1, motor1_Ctrl[CW]);
motorDrive(2, motor2_Ctrl[CCW]);
motorDrive(3, motor3_Ctrl[STOP]);
break;
default:
break; 
}

delay(200);
/* 
wheels[0].detach();
wheels[1].detach();
wheels[2].detach();
*/

motorDrive(1, motor1_Ctrl[STOP]);
motorDrive(2, motor2_Ctrl[STOP]);
motorDrive(3, motor3_Ctrl[STOP]);
}

int compareSonarValue(){
int addr = 11;
short maxVal = S11;

if( maxVal < S12){
addr = 12;
maxVal = S12;
}

if( maxVal < S13){
addr = 13;
maxVal = S13;
}

if(inMargin(S11,maxVal,1) && inMargin(S12,maxVal,1) && inMargin(S13,maxVal,1)){
addr = 0;
}

Serial.println(addr);

return addr;
}

boolean inMargin(short sample, short target, short margin){
if(sample < (target – margin) )
return false;
else if(sample > (target + margin) )
return false;
else
return true;
}

void loop(){
demowithSosars();
}

—————————————–
ご質問等は受け付けておりませんのであしからず！