/**
Author: Omar Draidrya
Date: 2024/07/03
This code controls the forward and backward movement of a motor using an H-bridge.
*/
#include <AFMotor.h>
#include <Arduino.h>
#include <Servo.h>
byte triggerPin = A4;
byte echoPin = A5;
long duration, distance;
int number = 0;
byte star = 1;
Servo myservo;
unsigned long lastServoAction;
long cm, cml, cmr;
int pos = 30;
bool richtung = true;
int val;
constexpr uint8_t minPoint{ 45 };
constexpr uint8_t maxPoint{ 135 };
constexpr uint8_t servo{ 10 };
AF_DCMotor motor1(1); // Create motor #1 using M1 connector
AF_DCMotor motor2(2); // Create motor #2 using M2 connector
AF_DCMotor motor3(3); // Create motor #3 using M3 connector
AF_DCMotor motor4(4); // Create motor #4 using M4 connector
char BT_input;
void setup() {
Serial.begin(9600); // Start serial communication at 9600 baud rate
motor1.setSpeed(150); // Set initial motor speeds
motor2.setSpeed(150);
motor3.setSpeed(150);
motor4.setSpeed(150);
pinMode(triggerPin, OUTPUT);
pinMode(echoPin, INPUT);
myservo.attach(servo);
myservo.write(minPoint);
}
void loop() {
connection();
uint32_t myMillis = millis();
if (myMillis - lastServoAction > 15) // Pause für Echounterdrückung
{
if (lastServoAction != 0)
{
Serial.print(F("timing: "));
Serial.println(myMillis - lastServoAction);
}
lastServoAction = myMillis;
constexpr uint8_t fieldTicks{ (maxPoint - minPoint) / 3 }; // (40) gleichmässig Bereiche über den gesamten Arbeitsbereich verteilen
static uint32_t tempData = 0;
uint16_t d = ultra();
tempData += d;
/*
Serial.print(F("Dura: "));
Serial.print(d);
Serial.print(F(" Summe: "));
Serial.println(tempData);
*/
switch (pos)
{
case minPoint: // (30) ganz links
if (!richtung) // kommt von rechts
{
cml = tempData / (fieldTicks - 1);
}
Serial.println(F("Servo links"));
richtung = HIGH; // Richtungsumkehr
tempData = 0;
break;
case minPoint + fieldTicks: // (30+40 = 70)Ende linke Ausleuchtung - Anfang Mitte
if (richtung) // Auf dem Weg nach rechts
{
cml = tempData / (fieldTicks - 1);
}
else // auf dem Weg nach links
{
cm = tempData / (fieldTicks - 1);
}
Serial.println(F("tick 70"));
tempData = 0;
break;
case maxPoint - fieldTicks: // (150-40 = 110) Ende Mitte - Anfang rechte Ausleuchtung
if (!richtung) // Auf dem Weg nach links
{
cmr = tempData / (fieldTicks - 1);
}
else // Auf dem Weg nach rechts
{
cm = tempData / (fieldTicks - 1);
}
Serial.println(F("110 tick"));
tempData = 0;
break;
case maxPoint: // (150) ganz rechts
if (richtung)
{
cmr = tempData / (fieldTicks - 1);
}
Serial.println(F("Servo rechts"));
richtung = LOW;
tempData = 0;
break;
}
if (richtung)
{
pos+=8;
}
else
{
pos-=8;
}
// Fehlerbehandlung
if (pos > maxPoint)
{
pos = maxPoint;
Serial.println(F("maxPosFail"));
}
if (pos < minPoint)
{
pos = minPoint;
Serial.println(F("minPosFail"));
}
myservo.write(pos); // Servo setzen
}
digitalWrite(triggerPin, LOW);
delayMicroseconds(4);
digitalWrite(triggerPin, HIGH);
delayMicroseconds(10);
digitalWrite(triggerPin, LOW);
duration = pulseIn(echoPin, HIGH);
return duration / 60;
}
void connection() {
if (Serial.available() > 0) {
BT_input = Serial.read();
Serial.println(BT_input); // Read the incoming BT_input
switch (BT_input) {
case 'F':
forward();
break;
case 'B':
backward();
break;
case 'L':
turnLeft();
break;
case 'R':
turnRight();
break;
case 'S':
stop();
break;
case 'X':
static unsigned long lastAction = 0;
static uint8_t State = 0;
switch (State) {
case 0: //Foreward
if ((cm < 30) || ((cm < 20) && (cmr < 20) && (cm < 20))) {
Serial.println(F("zu eng...Stop"));
stop();
drive(); // erstmal anhalten
State = 1;
lastAction = millis();
} else {
forward();
}
break;
case 1: //Stop abwarten
if (millis() - lastAction > 20) {
lastAction = millis();
Serial.println(F("Zurück"));
backward(); // Rückwärts raus
State = 2;
}
break;
case 2: //Zurück abwarten
if (millis() - lastAction > 1000) {
lastAction = millis();
Serial.println(F("Stop"));
stop(); // anhalten
State = 3;
}
break;
case 3: //wohin?
if (millis() - lastAction > 20) {
lastAction = millis();
if (cml < cmr) { // Richtungsbewegung
Serial.println(F("turnRight"));
turnRight();
} else {
Serial.println(F("turnLeft"));
turnLeft();
}
State = 4;
}
break;
case 4: //Stop abwarten
if (millis() - lastAction > 20) {
lastAction = millis();
State = 0;
}
break;
}
break;
}
}
}
void drive() {
number = random(3);
if (number == 1) {
turnLeft();
delay(1500);
} else {
turnRight();
delay(1500);
}
}
void forward() {
motor1.run(FORWARD);
motor2.run(FORWARD);
motor3.run(FORWARD);
motor4.run(FORWARD);
}
void backward() {
motor1.run(BACKWARD);
motor2.run(BACKWARD);
motor3.run(BACKWARD);
motor4.run(BACKWARD);
}
void turnLeft() {
motor1.run(BACKWARD);
motor2.run(BACKWARD);
motor3.run(FORWARD);
motor4.run(FORWARD);
}
void turnRight() {
motor1.run(FORWARD);
motor2.run(FORWARD);
motor3.run(BACKWARD);
motor4.run(BACKWARD);
}
void stop() {
motor1.run(RELEASE);
motor2.run(RELEASE);
motor3.run(RELEASE);
motor4.run(RELEASE);
}
/*void automatic() {
static unsigned long lastAction = 0;
static uint8_t State = 0;
switch (State) {
case 0: //Foreward
if ((cm < 30) || ((cm < 20) && (cmr < 20) && (cm < 20))) {
Serial.println(F("zu eng...Stop"));
stop();
drive(); // erstmal anhalten
State = 1;
lastAction = millis();
} else {
forward();
}
break;
case 1: //Stop abwarten
if (millis() - lastAction > 20) {
lastAction = millis();
Serial.println(F("Zurück"));
backward(); // Rückwärts raus
State = 2;
}
break;
case 2: //Zurück abwarten
if (millis() - lastAction > 1000) {
lastAction = millis();
Serial.println(F("Stop"));
stop(); // anhalten
State = 3;
}
break;
case 3: //wohin?
if (millis() - lastAction > 20) {
lastAction = millis();
if (cml < cmr) { // Richtungsbewegung
Serial.println(F("turnRight"));
turnRight();
} else {
Serial.println(F("turnLeft"));
turnLeft();
}
State = 4;
}
break;
case 4: //Stop abwarten
if (millis() - lastAction > 20) {
lastAction = millis();
State = 0;
}
break;
}
}*/
void setServoGetDist() {
uint32_t myMillis = millis();
if (myMillis - lastServoAction > 15) // Pause für Echounterdrückung
{
if (lastServoAction != 0)
{
Serial.print(F("timing: "));
Serial.println(myMillis - lastServoAction);
}
lastServoAction = myMillis;
constexpr uint8_t fieldTicks{ (maxPoint - minPoint) / 3 }; // (40) gleichmässig Bereiche über den gesamten Arbeitsbereich verteilen
static uint32_t tempData = 0;
uint16_t d = ultra();
tempData += d;
/*
Serial.print(F("Dura: "));
Serial.print(d);
Serial.print(F(" Summe: "));
Serial.println(tempData);
*/
switch (pos)
{
case minPoint: // (30) ganz links
if (!richtung) // kommt von rechts
{
cml = tempData / (fieldTicks - 1);
}
Serial.println(F("Servo links"));
richtung = HIGH; // Richtungsumkehr
tempData = 0;
break;
case minPoint + fieldTicks: // (30+40 = 70)Ende linke Ausleuchtung - Anfang Mitte
if (richtung) // Auf dem Weg nach rechts
{
cml = tempData / (fieldTicks - 1);
}
else // auf dem Weg nach links
{
cm = tempData / (fieldTicks - 1);
}
Serial.println(F("tick 70"));
tempData = 0;
break;
case maxPoint - fieldTicks: // (150-40 = 110) Ende Mitte - Anfang rechte Ausleuchtung
if (!richtung) // Auf dem Weg nach links
{
cmr = tempData / (fieldTicks - 1);
}
else // Auf dem Weg nach rechts
{
cm = tempData / (fieldTicks - 1);
}
Serial.println(F("110 tick"));
tempData = 0;
break;
case maxPoint: // (150) ganz rechts
if (richtung)
{
cmr = tempData / (fieldTicks - 1);
}
Serial.println(F("Servo rechts"));
richtung = LOW;
tempData = 0;
break;
}
if (richtung)
{
pos+=8;
}
else
{
pos-=8;
}
// Fehlerbehandlung
if (pos > maxPoint)
{
pos = maxPoint;
Serial.println(F("maxPosFail"));
}
if (pos < minPoint)
{
pos = minPoint;
Serial.println(F("minPosFail"));
}
myservo.write(pos); // Servo setzen
}
}
long ultra() {
digitalWrite(triggerPin, LOW);
delayMicroseconds(4);
digitalWrite(triggerPin, HIGH);
delayMicroseconds(10);
digitalWrite(triggerPin, LOW);
duration = pulseIn(echoPin, HIGH);
return duration / 60;
}
mit +=8 und -=8 bin ich schon nah dran, wahrscheinlich eher 10. Jetzt will er den ersten Befehl ausführen und bleibt dann dadrin hängen. <--- per Tastendruck kommt man aber raus
