４方向のHC-SR04超音波距離センサーで障害物を発見するとオムニホイールによる異なる退避動作を行います。
●メカナムホイール ロボットカーシャーシ 4WD 80mm DC12Vモーター スマートロボット ミニ四駆 台車ロボットSTEM
●同ブログ関連記事： 【全方向移動ロボットカー MEGA2560 RemoteXY操作】
動画
オムニホイール
【https://www.amazon.co.jp/gp/product/B07Z19HDXP/ref=ppx_yo_dt_b_search_asin_image?ie=UTF8&th=1】
.png)
 |
| モーターシールド HW-130 |
●ELEGOO Arduino用 MEGA2560 R3ボード ATmega2560
●HW-130 L293D モータ ドライバー拡張ボード 基板モーター モータ制御シールド【アマゾン購入】
回路図
Arduino IDE
.png)
プログラム
#include <AFMotor.h>
#include <pitches.h>//Melody
#define LED 13
int EchoB = 22;
int TrigB = 23;
int EchoL = 24;
int TrigL = 25;
int EchoF = 26;
int TrigF = 27;
int EchoR = 28;
int TrigR = 29;
int TonePIN = 30;
int distB;
int distL;
int distF;
int distR;
bool state = LOW;
int count=0;
AF_DCMotor motor1(1);
AF_DCMotor motor2(2);
AF_DCMotor motor3(3);
AF_DCMotor motor4(4);
void forward(){
motor1.run(FORWARD);
motor2.run(FORWARD);
motor3.run(FORWARD);
motor4.run(FORWARD);
}
void back(){
motor1.run(BACKWARD);
motor2.run(BACKWARD);
motor3.run(BACKWARD);
motor4.run(BACKWARD);
}
void stop(){
motor1.run(RELEASE);
motor2.run(RELEASE);
motor3.run(RELEASE);
motor4.run(RELEASE);
}
void right(){
motor1.run(FORWARD);
motor2.run(BACKWARD);
motor3.run(FORWARD);
motor4.run(BACKWARD);
}
void left(){
motor1.run(BACKWARD);
motor2.run(FORWARD);
motor3.run(BACKWARD);
motor4.run(FORWARD);
}
void R_forward(){
motor1.run(FORWARD);
motor2.run(RELEASE);
motor3.run(FORWARD);
motor4.run(RELEASE);
}
void L_forward(){
motor1.run(RELEASE);
motor2.run(FORWARD);
motor3.run(RELEASE);
motor4.run(FORWARD);
}
void R_back(){
motor1.run(RELEASE);
motor2.run(BACKWARD);
motor3.run(RELEASE);
motor4.run(BACKWARD);
}
void L_back(){
motor1.run(BACKWARD);
motor2.run(RELEASE);
motor3.run(BACKWARD);
motor4.run(RELEASE);
}
void R_roll(){
motor1.run(BACKWARD);
motor2.run(BACKWARD);
motor3.run(FORWARD);
motor4.run(FORWARD);
}
void L_roll(){
motor1.run(FORWARD);
motor2.run(FORWARD);
motor3.run(BACKWARD);
motor4.run(BACKWARD);
}
void distanceB() {
digitalWrite(TrigB, LOW);
delayMicroseconds(2);
digitalWrite(TrigB, HIGH);
delayMicroseconds(20);
digitalWrite(TrigB, LOW);
distB = pulseIn(EchoB, HIGH);
distB= distB/58;
//Serial.print("B ");
//Serial.println(distB);
}
void distanceL() {
digitalWrite(TrigL, LOW);
delayMicroseconds(2);
digitalWrite(TrigL, HIGH);
delayMicroseconds(20);
digitalWrite(TrigL, LOW);
distL = pulseIn(EchoL, HIGH);
distL= distL/58;
//Serial.print("L ");
//Serial.println(distL);
}
void distanceF() {
digitalWrite(TrigF, LOW);
delayMicroseconds(2);
digitalWrite(TrigF, HIGH);
delayMicroseconds(20);
digitalWrite(TrigF, LOW);
distF = pulseIn(EchoF, HIGH);
distF= distF/58;
//Serial.print("F ");
//Serial.println(distF);
}
void distanceR() {
digitalWrite(TrigR, LOW);
delayMicroseconds(2);
digitalWrite(TrigR, HIGH);
delayMicroseconds(20);
digitalWrite(TrigR, LOW);
distR = pulseIn(EchoR, HIGH);
distR= distR/58;
//Serial.print("R ");
//Serial.println(distR);
}
void Led_Change(){
state = !state;
digitalWrite(LED, state);
Serial.println("Light");
}
// Mario
int melody[] = {
NOTE_E7, NOTE_E7, 0, NOTE_E7,
0, NOTE_C7, NOTE_E7, 0,
NOTE_G7, 0, 0, 0,
NOTE_G6, 0, 0, 0,
};
int noteDurations[] = {
12, 12, 12, 12,
12, 12, 12, 12,
12, 12, 12, 12,
12, 12, 12, 12,
};
void Sound_mario() {
for (int thisNote = 0; thisNote < 16; thisNote++) {
int noteDuration = 1000/noteDurations[thisNote];
tone(TonePIN, melody[thisNote],noteDuration);
int pauseBetweenNotes = noteDuration * 1.50;
delay(pauseBetweenNotes);
}
}
// Mario_1
int melody1[] = {
NOTE_E4, NOTE_E4,0,NOTE_E4,0,
NOTE_C4, NOTE_E4,0,NOTE_G4,0,0,0,NOTE_G3,};
int noteDurations1[] = {
8,8,8,8,8,8,8,8,8,8,8,8,8,};
void Sound_mario1() {
for (int thisNote = 0; thisNote < 13; thisNote++) {
int noteDuration1 = 1000/noteDurations1[thisNote];
tone(TonePIN, melody1[thisNote],noteDuration1);
int pauseBetweenNotes = noteDuration1 * 1.00;
delay(pauseBetweenNotes);
}
}
// Mario_2
int melody2[] = {
NOTE_G4, NOTE_F4,0,NOTE_F4,NOTE_F4,
NOTE_E4,0,NOTE_D4,NOTE_C4,0,NOTE_G4,0,NOTE_C4,};
int noteDurations2[] = {
8,8,8,8,8,8,8,8,8,8,8,8,8,4,};
void Sound_mario2() {
for (int thisNote = 0; thisNote < 13; thisNote++) {
int noteDuration2 = 1000/noteDurations2[thisNote];
tone(TonePIN, melody2[thisNote],noteDuration2);
int pauseBetweenNotes = noteDuration2 * 1.20;
delay(pauseBetweenNotes);
}
}
//Darth Vader
int melody3[] = {
NOTE_G3, NOTE_G3, NOTE_G3, NOTE_DS3, NOTE_AS3, NOTE_G3, NOTE_DS3, NOTE_AS3, NOTE_G3,
NOTE_D4, NOTE_D4, NOTE_D4, NOTE_DS4, NOTE_AS3, NOTE_FS3, NOTE_DS3, NOTE_AS3, NOTE_G3
};
int noteDurations3[] = {
4, 4, 4, 6, 8, 4, 6, 8, 2,
4, 4, 4, 6, 8, 4, 6, 8, 2
};
void Sound_darh() {
for (int thisNote = 0; thisNote < 18; thisNote++) {
int noteDuration3 = 1000/noteDurations3[thisNote];
tone(TonePIN, melody3[thisNote],noteDuration3);
int pauseBetweenNotes = noteDuration3 * 1.20;
delay(pauseBetweenNotes);
}
}
void auto_run(){
delay(10);
Led_Change();
count=count+1;
distanceB();
if(distB < 15){
forward();
count=0;
}
distanceL();
if(distL < 15){
right();
count=0;
}
distanceF();
if(distF < 15){
back();
count=0;
}
distanceR();
if(distR < 15){
left();
count=0;
}
if(count > 50 && count < 100 ){
R_roll();
}
else if(count >= 100 && count < 150){
left();
}
else if(count > 150 && count < 200 ){
right();
}
else if(count >= 200 && count < 250){
L_forward();
}
else if(count >= 250 && count < 300){
R_forward();
}
else if(count >= 300){
forward();
}
}
void setup() {
Serial.begin(9600);
pinMode(EchoB, INPUT);
pinMode(TrigB, OUTPUT);
pinMode(EchoL, INPUT);
pinMode(TrigL, OUTPUT);
pinMode(EchoF, INPUT);
pinMode(TrigF, OUTPUT);
pinMode(EchoR, INPUT);
pinMode(TrigR, OUTPUT);
pinMode(LED, OUTPUT);
motor1.setSpeed(200);
motor2.setSpeed(200);
motor3.setSpeed(200);
motor4.setSpeed(200);
motor1.run(RELEASE);
motor2.run(RELEASE);
motor3.run(RELEASE);
motor4.run(RELEASE);
delay(100);
}
void loop() {
auto_run();
}
0 件のコメント:
コメントを投稿