技術的小話【その7】Nexus Robot 3WDオムニホイールロボットをぶつからない車にしてみた

どうもこんにちは！
babaです！！
毎週木曜日の技術的小話のコーナー、本日のテーマは「Nexus Robot 3WDオムニホイールロボットをぶつからない車にしてみた」です。
商品についての情報はこちらをご覧ください。

このNexus Robot 3WDオムニホイールロボット、Arduino(ATMega328)を搭載しておりプログラムの作成が簡単、オムニホイールにより姿勢を変えずに様々な方向へ移動することができるなどの特徴があることは先週ご紹介しましたが、さらなる特徴があります。
それは超音波センサが最初から搭載されているということです！
超音波センサは超音波の反射を利用することで物体との距離を測定することができるセンサです。コウモリやイルカが、超音波を使って周囲の状況を把握しているのは有名な話じゃないでしょうか。
この超音波センサを使って距離を測ろうとすると、音の速度や音が跳ね返ってくるまでの時間などを使って計算する必要があります。しかしこのロボットに搭載されているセンサ、なんと距離まで計算して出してくれます！これは楽ちん。

というわけで超音波センサを使って近年話題のぶつからない車（っぽいもの）を作ってみました。

動画のようにSotaにぶつからずに止まることができました！
超音波センサですが、このロボットには3個搭載されているので、オムニホイールと組み合わせてさらに面白い動きもできそうです。
興味がある方はぜひチャレンジしてください！！

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

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

#define MOTOR1_E 9
#define MOTOR2_E 10
#define MOTOR3_E 11
#define FORWORD 0
#define STOP 1
#define BACK 2
//**********************************************//
unsigned char motor1_Ctrl[3] = {60,45,30};
unsigned char motor2_Ctrl[3] = {60,45,30};
unsigned char motor3_Ctrl[3] = {60,45,30};
unsigned short distBuf[3];

Servo wheels;

int sonarUpdate() {
static unsigned char sonarCurr=1;
if(sonarCurr==3) sonarCurr=1;
else sonarCurr=0;
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 goAhead(){
analogWrite(MOTOR1_E, motor1_Ctrl[STOP]);
analogWrite(MOTOR2_E, motor2_Ctrl[BACK]);
analogWrite(MOTOR3_E, motor3_Ctrl[FORWORD]); // Revese
}
//*************************************************//
void getBack(){
analogWrite(MOTOR1_E,motor1_Ctrl[STOP]);
analogWrite(MOTOR2_E,motor2_Ctrl[FORWORD]);
analogWrite(MOTOR3_E,motor3_Ctrl[BACK]);
}
//************************************************//
void turnLeft(){
analogWrite(MOTOR1_E,motor1_Ctrl[FORWORD]); //Revese
analogWrite(MOTOR2_E,motor2_Ctrl[BACK]); //forward
analogWrite(MOTOR3_E,motor3_Ctrl[STOP]); //forward
} 
void turnRight(){
analogWrite(MOTOR1_E,motor1_Ctrl[BACK]); //forward
analogWrite(MOTOR2_E,motor2_Ctrl[STOP]); //Revese 
analogWrite(MOTOR3_E,motor3_Ctrl[FORWORD]); //Revese
}
//************************************************//
void RotateRight(){
analogWrite(MOTOR1_E,motor1_Ctrl[BACK]);
analogWrite(MOTOR2_E,motor2_Ctrl[BACK]);
analogWrite(MOTOR3_E,motor3_Ctrl[BACK]); 
}
//*************************************************//
void RotateLeft(){
analogWrite(MOTOR1_E,motor1_Ctrl[FORWORD]);
analogWrite(MOTOR2_E,motor2_Ctrl[FORWORD]);
analogWrite(MOTOR3_E,motor3_Ctrl[FORWORD]); 
}
//**************************************************//
void judge(){
if(distBuf[0]>=30){
if(distBuf[1]<=10 && distBuf[2]>10) turnRight(); 
else if(distBuf[2]<=10 && distBuf[1]>10) turnLeft();
else if(distBuf[1]<=10 && distBuf[2]<=10) RotateLeft();
else goAhead(); 
}else RotateLeft();
}
//**************************************************//
void allStop(){
analogWrite(MOTOR1_E, motor1_Ctrl[STOP]);
analogWrite(MOTOR2_E, motor2_Ctrl[STOP]);
analogWrite(MOTOR3_E, motor3_Ctrl[STOP]);
}
//*************************************************//
//void (*motion[8])()={ goAhead,RotateLeft,turnRight,RotateLeft,turnLeft,RotateLeft,judge,allStop}; //change the 
void (*motion[8])()={ goAhead,RotateLeft,turnRight,RotateLeft,turnLeft,RotateLeft,goAhead,allStop}; //change the 

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; 
//Serial.println(distBuf[sonarcurrent]) ;
}
sonarUpdate(); 
//if(sonarcurrent==3){
if(distBuf[0] > 10){
analogWrite(MOTOR2_E, motor2_Ctrl[BACK]);
analogWrite(MOTOR3_E, motor3_Ctrl[FORWORD]);
//wheels.write(83);
}
else{
analogWrite(MOTOR2_E, motor2_Ctrl[STOP]);
analogWrite(MOTOR3_E, motor3_Ctrl[STOP]);
//wheels.write(83);
}
//delay(200);
/*unsigned char bitmap = (distBuf[0] < 20);//front
bitmap |= (distBuf[1]<20) <<1; //left
bitmap |= (distBuf[2]<20) <<2; //right:
Serial.print(“bitmap=”);
Serial.println(bitmap,DEC);
(*motion[bitmap])();*/ 
//} 
}

void demoaction(){
for(int i=0;i<7;i++){
(*motion[i])();
Serial.println(i);
delay(10000);

}
}
//*************************************************//

void setup() {

TCCR1B=TCCR1B&0xf8|0x04;
TCCR2B=TCCR2B&0xf8|0x06;

//pinMode(MOTOR1_E, OUTPUT);
pinMode(MOTOR2_E, OUTPUT);
pinMode(MOTOR3_E, OUTPUT);
//wheels.attach(MOTOR1_E);

//wheels.write(83);p
delay(2000);
SONAR::init(); //call the init() from SONAR.h;

//Serial.begin(9600) ;
}
void loop(){
demowithSosars();
//demoaction();
}
—————————————————
ご質問等は受け付けておりませんのであしからず！