« Arduino UNO で Robot Car 製作 | Main | Arduino で 4-8x8 dotmatrix LED使用方法(いわゆるLチカ) »

06/05/2019

Arduino UNO 超音波センサーロボットカーに後方センサーを追加

前回製作したArduino UNOのロボットカーでは、後退する際に、障害物に衝突してしまうケースが多発した。
それで、超音波距離センサーを後方にも追加(サーボコントロール無し)して、後方への衝突をできる限り回避するようにしてみた。
後方距離センサー用のピンは、トリガーピンをArduinoのD8に、エコーピンをD7に接続した。
スケッチは次のように加筆変更した。

I made my own robot tank that avoids and moves obstacles.
This robot tank uses an ultrasonic distance sensor that mounted in front of it to measure the distance to an obstacle and control the movement to move.
Since the processing power of the microcomputer is Arduino UNO that is not so high performance, it might be difficult to make a further advanced control. I added a sensor at the back of the tank to measure the distance to the obstacle behind it, so the robot tank can move around in a narrow place without collision.

《Sketch》

// Obstacle avoidance Tank Robot program 2019/06/05
// add backward sensor avoid collisions My Spice Cabinet
double Fdistance = 0;
double Rdistance1= 0;
double Ldistance1= 0;
double Rdistance2= 0;
double Ldistance2= 0;
double RRdist=0;
#include  <Servo.h>
int pinLB = 12; // direction control function
int pinLF = 10; // motors' control chip with speed control function
int pinRB = 13; // direction control function
int pinRF = 11; // motors' control chip with speed control function
int echoPin = 5; // define pin for sensor echo
int trigPin = 6; // define pin for sensor trig
int echoPinB = 7; // define pin for back sensor echo
int trigPinB = 8; // define pin for back sensor trig
int motorOnTime1; // motor on time back
int motorOnTime2; // turning motor on time
int directionn = 0; // forward=8 backward=2 left=4 right=6
Servo myServo; // set myservo
int delay_time = 250; // settling time 250 after steering servo motor moving B
int Fgo = 8; // Move Forward
int Rgo = 6; // move to the R
int Lgo = 4; // move to the L
int Bgo = 2; // move Backward
int angle;
void setup()
{
Serial.begin(9600);
pinMode(3, OUTPUT); // Head light
// 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)
// Define Front sensor pin
pinMode(echoPin, INPUT); // define input pin from sensor
pinMode(trigPin,OUTPUT); // define output pin for sensor
// Define Back sensor pin
pinMode(echoPinB, INPUT); // define input pin from sensor
pinMode(trigPinB,OUTPUT); // define output pin for sensor
myServo.attach(9); // Define servo motor output pin to D9 (PWM)
}
void advance()              // move to forward
{
digitalWrite(pinLB,LOW); // left wheel moves forward
digitalWrite(pinRB,LOW); // right wheel moves forward
analogWrite(pinLF,255);
analogWrite(pinRF,255);
}
void stopp()                // all wheel stop
{
digitalWrite(pinLB,HIGH); // left wheel stop
digitalWrite(pinRB,HIGH); // right wheel stop
analogWrite(pinLF,0);
analogWrite(pinRF,0);
}
void right()                // turn right (single wheel)
{
digitalWrite(pinLB,HIGH); // wheel on the left stop
digitalWrite(pinRB, LOW); // wheel on the right moves backward
analogWrite(pinLF,255);
analogWrite(pinRF,255);
}
void left()                 // turn left (single wheel)
{
digitalWrite(pinLB, LOW); // wheel on the left moves backward
digitalWrite(pinRB,HIGH); // wheel on the right stop
analogWrite(pinLF,255);
analogWrite(pinRF,255);
}
void back()                 // move backward
{
digitalWrite(pinLB,HIGH); // motor moves to left rear
digitalWrite(pinRB,HIGH); // motor moves to right rear
analogWrite(pinLF,255);
analogWrite(pinRF,255);
}
void detection()            // measure multi directions
{
angle = 90; // measure front direction
ask_pin_F(); // read the distance ahead
motorOnTime1 = 100;
motorOnTime2 = 200;
// ==========================================================
if(Fdistance < 30) // if distance ahead is <30cm
{
stopp(); // all wheel stop
delay(motorOnTime1);
back(); // move backward for 0.2S
delay(motorOnTime2);
}
// ----------------------------------------------------------
if(Fdistance < 50) // if distance ahead is <50cm
{
stopp(); // all wheel stop
delay(motorOnTime1);
// ==========================================================
angle = 5; // measure left direction
ask_pin_F(); // read distance on the left
Ldistance1 = Fdistance;
motorOnTime1 = 300;
motorOnTime2 = 300; // turn about 100deg, if 120deg --> 600
delay(delay_time); // stabilizing time for servo motor
// ==========================================================
angle = 30; // measure left direction
ask_pin_F(); // read distance on the left
Ldistance2 = Fdistance;
motorOnTime1 = 100;
motorOnTime2 = 200;
delay(delay_time); // stabilizing time for servo motor
// ==========================================================
angle = 120; // measure right direction
ask_pin_F(); // read distance on the right
Rdistance2 = Fdistance;
motorOnTime1 = 100;
motorOnTime2 = 200;
delay(delay_time); // stabilizing time for servo motor
// ==========================================================
angle = 175; // measure right direction
ask_pin_F(); // read distance on the right
Rdistance1 = Fdistance;
motorOnTime1 = 300;
motorOnTime2 = 300; // turn about 100deg, if 120deg --> 600
delay(delay_time); // stabilizing time for servo motor
// ==========================================================
if(Ldistance2 > Rdistance2) // if LH distance is >distance on the right
{
directionn = Lgo; // move to the L
}
// ----------------------------------------------------------
if(Ldistance2 <= Rdistance2) // if LH distance is <= distance on the right
{
directionn = Rgo; // move to the right
}
// ----------------------------------------------------------
if (Ldistance1 < 30 && Rdistance1 < 30) // if distance on LH and RH are both <30cm
{
directionn = Bgo; // move to backward
ask_pin_B();
}
}
// ----------------------------------------------------------
else // if distance ahead is >50cm
{
angle = 90;
ask_pin_F(); // read the distance ahead
directionn = Fgo; // move forward
}
}
void ask_pin_F()        // measure the distance ahead on the angle
{
myServo.write(angle);
delay(delay_time);
digitalWrite(trigPin, LOW); // ultrasonic sensor transmit low level signal 2μs
delayMicroseconds(2);
digitalWrite(trigPin,HIGH); // ultrasonic sensor transmit high level signal 10μs, at least 10μs
delayMicroseconds(10);
digitalWrite(trigPin, LOW); // keep transmitting low level signal
double Duration = pulseIn(echoPin, HIGH); // read the time in between
Fdistance = Duration/2*340*100/1000000; // convert time into distance (unit: cm)
// debugging the angle and its distance
Serial.print(angle);
Serial.print("- Distance ahead ");
Serial.print(Fdistance);
Serial.println(" [cm]");
}
void ask_pin_B()        // measure the distance backward
{
digitalWrite(trigPinB, LOW); // ultrasonic sensor transmit low level signal 2μs
delayMicroseconds(2);
digitalWrite(trigPinB,HIGH); // ultrasonic sensor transmit high level signal 10μs, at least 10μs
delayMicroseconds(10);
digitalWrite(trigPinB, LOW); // keep transmitting low level signal
double Duration = pulseIn(echoPinB, HIGH); // read the time in between
RRdist = Duration/2*340*100/1000000; // convert time into distance (unit: cm)
// debugging the angle and its distance
Serial.print("Distance backward ");
Serial.print(RRdist);
Serial.println(" [cm]");
}
void loop()
{
// Head light
Serial.print("CDS Value = ");
Serial.println(analogRead(0));
digitalWrite(3, LOW);
if(analogRead(0)<400) digitalWrite(3, HIGH);
// driving forward ----------------------------
angle=90;
myServo.write(angle); // home set the servo motor, ready for next measurement from 90
detection(); // measure the angle and determine which direction to move
if(directionn == 2) // if direction => backward
{
if(RRdist > 25)
{
back(); // go backward
delay(motorOnTime1);
left(); // Move slightly to the left (to prevent stuck in dead end)
delay(motorOnTime2);
}
else
{
stopp();
delay(motorOnTime2);
}
}
// ---------------------------------------------
if(directionn == 6) // if direction => right
{
back(); // go backward
delay(motorOnTime1);
left(); // turn left because reverse driving
delay(motorOnTime2);
}
// ---------------------------------------------
if(directionn == 4) // if direction => left
{
back(); // go backward
delay(motorOnTime1);
right(); // turn right because reverse driving
delay(motorOnTime2);
}
// ---------------------------------------------
if(directionn == 8) // if direction => forward
{
advance(); // move forward
delay(motorOnTime1);
}
}

« Arduino UNO で Robot Car 製作 | Main | Arduino で 4-8x8 dotmatrix LED使用方法(いわゆるLチカ) »

Arduino」カテゴリの記事

Comments

Post a comment

Comments are moderated, and will not appear on this weblog until the author has approved them.

(Not displayed with comment.)

« Arduino UNO で Robot Car 製作 | Main | Arduino で 4-8x8 dotmatrix LED使用方法(いわゆるLチカ) »

July 2019
Sun Mon Tue Wed Thu Fri Sat
  1 2 3 4 5 6
7 8 9 10 11 12 13
14 15 16 17 18 19 20
21 22 23 24 25 26 27
28 29 30 31      

Recent Trackbacks

無料ブログはココログ