Projekt Nr:2/3 Arduino Auto/Car-Roboter Ultraschall-Sensor-HC-SR04 Motordriver L298N Umbau Programm

Das Programm/Sketch findest Du im ersten Kommentar
*********************************************************
Damit sich mein Enkelkind für das Programmieren interessierte, beschloss ich, die Grundlagen von Arduino zu erlernen und einige Projekte dafür vorzubereiten. Zuerst werde ich ins Thema selbst einzuarbeiten, und dann werde ich anfangen, meinen Enkelkin einzubeziehen.
Dazu habe ich im Internet ein "Smart Car Kit" bestellt.
Ich habe die Arduino-Entwicklungsumgebung (Arduino Software (IDE) 2.0.3.) installiert.
Dann habe ich mir eine große Anzahl von YouTube-Videos zum Thema "Arduino für Anfänger" angesehen.
Daher werde ich ein paar Projekte zum Thema Roboterautos vorbereiten.
Projekt #1: Roboter mit Infrarotsensoren
Projekt #2: Roboter mit Ultraschallsensoren
Projekt #3: Roboter mit automatischer und manueller Steuerung (Bluetooth)
Hier ist Projekt #2.

#ultrasonic
#ultrasonicsensor
#ultrasonicsensorrobot
#ultrasonicsensorcar
#ultrasonictechnology
#ultraschallsensor

[sergegal]

#include <Servo.h>
Servo srvDirection;
//Motor pins
#define mot_LefFW 5 //left side forward
#define mot_LefBW 6 //left side backward
#define mot_RightFW 9 //right side forward
#define mot_RightBW 10 //right side backward
//Ultra sonic
#define UltSon_Snd 7 //UltraSonic Tr
#define UltSon_Rcv 8 //UltraSonic Echo
//servo motor pins with PWM (Pulse-Width-Modulation).
#define ServoPin 3 //Servo
//minimum distance (cm) to the object
#define distStop 50 //Speed is to hight(Akku-12V)
//for ImpulsTimer()
bool sekImp, ms200Imp;
unsigned long int timeMillis, timePrev1sec, timePrev200ms;
//for distance calculation and direction
unsigned long int dur;
int dist, curDist;

void setup() {
Serial.begin(9600);
//set motor pins
pinMode(mot_LefFW, OUTPUT);
pinMode(mot_LefBW, OUTPUT);
pinMode(mot_RightFW, OUTPUT);
pinMode(mot_RightBW, OUTPUT);
//set ultrasonic pins
pinMode(UltSon_Snd, OUTPUT);
pinMode(UltSon_Rcv, INPUT);
//set Servo pins

delay(2000);
}//end of setup()

void loop() {
//always set at loop()-start pulse sekImp and ms200Imp
ImpulsTimer();

autoDrive();

//always at loop()-end reset pulse
sekImp = 0;
ms200Imp = 0;

}// end of loop()

void ImpulsTimer() {
timeMillis= millis();
if(timeMillis - timePrev1sec >= 1000)//One second. ATTENTION: If too much "delay()" in program, then the value of 1000 will differ!!!!
{
timePrev1sec = millis();//Save Timer Stand
sekImp = 1;
}
if(timeMillis - timePrev200ms >= 200)
{
timePrev200ms = millis();//Save Timer Stand
ms200Imp = 1;
}
}//end of ImpulsTimer()

void autoDrive() {//UltraSonic control
//Query distance 5 time per second
if (ms200Imp) curDist = UltrSncDist();
if(sekImp){ Serial.print("Distance : "); Serial.println(curDist); }

if(curDist <= distStop) {
stop();
delay(10);

int dirDrive = 0;
dirDrive = findDirection();//Scan the environment and determinate direcktion
Serial.print("Return Value : "); Serial.println(dirDrive);
switch (dirDrive)
{
//the delay-time should be correspond to the speed.
case 0:
backward();
delay(500);//Do nothing as long as driving back
right();
delay(300);//Do nothing as long as it turns to the left
break;
case 1:
backward();
delay(250);//Do nothing as long as driving back
right();
delay(300);//Do nothing as long as it turns to the right
break;
case 2:
right();
delay(150);//Do nothing as long as it turns to the right
break;
case 3:
left();
delay(150);//Do nothing as long as it turns to the left
break;
case 4:
backward();
delay(250);//Do nothing as long as driving back
left();
delay(300);//Do nothing as long as it turns to the left
break;
}
}
else if (curDist > distStop) {
forward();
delay(10);
}
} //end of void autoDrive()

int UltrSncDist(){ //Determine distance to barrier
digitalWrite(UltSon_Snd, LOW);
delayMicroseconds(2);
digitalWrite(UltSon_Snd, HIGH);
delayMicroseconds(10);
digitalWrite(UltSon_Snd, LOW);
dur = pulseIn(UltSon_Rcv, HIGH); //time
dist = dur / 58;//Soundspeed:343, 2m/s => 0, 03432cm/µs => 2*distance=duration*0, 03432 => distance=duration/58, 3
delay(10);
return dist;
}//end of UltrSncDist()

byte findDirection(){
byte retDirValue = 0, posMaxDist = 0, posSvr;
int maxDist = 0;

//check left
for(posSvr = 90; posSvr < 170; posSvr+=5) {

srvDirection.write(posSvr);
delay(30);
//Query distance
curDist = UltrSncDist();
//Determine the greatest distance
if(curDist > maxDist) {
maxDist = curDist;
posMaxDist = posSvr;
Serial.print("maxDist: "); Serial.print(maxDist); Serial.print(" posMaxDist: "); Serial.println(posMaxDist);
}
}
//check right
for(posSvr = 90; posSvr > 10; posSvr-=5) {

srvDirection.write(posSvr);
delay(30);
//Query distance
curDist = UltrSncDist();
//Determine the greatest distance
if(curDist > maxDist) {
maxDist = curDist;
posMaxDist = posSvr;
Serial.print("maxDist: "); Serial.print(maxDist); Serial.print(" posMaxDist: "); Serial.println(posMaxDist);
}
}
srvDirection.write(90);
delay(15);
//Determine the direction of movement
if(maxDist <= distStop) {
retDirValue = 0; //backwards
}
else if(maxDist > distStop) {
if(posMaxDist > 0 && posMaxDist <= 45) retDirValue = 1; //right
else if(posMaxDist > 45 && posMaxDist <= 90) retDirValue = 2; //half right
else if(posMaxDist > 90 && posMaxDist <= 135) retDirValue = 3; //half left
else if(posMaxDist > 135) retDirValue = 4; //left
}
return retDirValue;
}//end of findDirection()

void forward() {
if(sekImp) Serial.println("forward()");
runMotWSpeed(1, 0, 1, 0);} //(byte leftFW, byte leftBW, byte rightFW, byte rightBW)
void backward() {
Serial.println("backward()");
runMotWSpeed(0, 1, 0, 1);}//(byte leftFW, byte leftBW, byte rightFW, byte rightBW)
void left() {
Serial.println("left()");
runMotWSpeed(0, 1, 1, 0);}//(byte leftFW, byte leftBW, byte rightFW, byte rightBW)
void right() {
Serial.println("right()");
runMotWSpeed(1, 0, 0, 1);}//(byte leftFW, byte leftBW, byte rightFW, byte rightBW)
void stop() {
Serial.println("stop()");
runMotWSpeed(0, 0, 0, 0);}//(byte leftFW, byte leftBW, byte rightFW, byte rightBW)
void runMotWSpeed(byte leftFW, byte leftBW, byte rightFW, byte rightBW) {
digitalWrite(mot_LefFW, leftFW);
digitalWrite(mot_LefBW, leftBW);
digitalWrite(mot_RightFW, rightFW);
digitalWrite(mot_RightBW, rightBW);}