Kettenroboter mit Arduino Steuerung

[StephanG90]

Ich habe seit langem mal wieder meine Arduinos aus dem Schrank geholt und einige kleinere Programme für den Einstieg geschrieben.
Ich habe jetzt als ein etwas größeres und längerfristiges Projekt begonnen meinen alten RP6 Kettenroboter auf Arduino Mega Steuerung umzubauen.
Die Hardware Grundlage ist schon mal fertig und ein kleines erstes Programm drauf.
Ich würde gerne hier meine Fragen und Probleme anbringen um nicht immer wieder neue Themen eröffnen zu müssen.

Hier erstmal die Hardware. Der Ultraschall Sensor wird noch seinen Platz finden bzw. durch andere Sensoren ersetzt.



Das aktuelle Programm kann per Start/ Stopp Taster gestartet und beendet werden. Das heißt er fährt los wenn der Taster betätigt wurde.
Solange kein Hindernis im Weg ist fährt der Roboter gerade aus. Wird etwas durch den Ultraschallsensor erkannt, wird zurück gefahren gedreht und wieder gerade aus gefahren bis zum nächsten Hindernis. Sollte der Ultraschall Sensor etwas nicht erkennen wird der linke bzw. rechte Stoßstangen Taster aktiviert und der Roboter fährt zurück und dreht.
Das Programm läuft soweit bis auf 2 Kleine Probleme: wenn gerade backUP aktiv ist, lässt sicher Roboter nicht Stoppen, er hält dann erst an wenn das Programm durch gelaufen ist, das muss ich noch ändern.
Das andere Problem was ich mir absolut nicht erklären kann ist das der Roboter sporadisch stoppt für kurze zeit und dann ganz Normal weiter läuft. Hat da jemand eine Idee wo hier der Fehler liegt?
Und Grundlegend ist das Programm so in Ordnung oder sollte man es Komplett anders angehen?

Danke und Gruß
Stephan

const int programmStart_taster = 2; // Taster für Programmstart
bool start = false;

#define PIN_TRIGGER 5 // PINs für hc-sr04 Sensor
#define PIN_ECHO    4

int directionPin_A = 12; // A Richtung LOW vorwärts / HIGH rückwärts
int pwmPin_A = 3; // A PWM
//int brakePin_A = 9; // A Bremse
// A Strommessung an PIN A0

int directionPin_B = 13; // B Richtung LOW vorwärts / HIGH rückwärts
int pwmPin_B = 11; // B Richtung
//int brakePin_B = 8; // B Bremse
// B Strommessung an PIN A1
volatile unsigned long alteZeit=0, entprellZeit=50;
const int crash_btn_li = 18; // PINs Stoßstange Taster
const int crash_btn_re = 19;
bool crash_li = false;
bool crash_re = false;
bool crash = false;
int a = 0; // 1 = Crash Ultraschall Sensor 2 = Crash Stoßstange links 3 = Crash Stoßstange rechts

void setup() 
{
  attachInterrupt(digitalPinToInterrupt(programmStart_taster), programmStart, RISING); // Taster Programm Start/ Stopp
  attachInterrupt(digitalPinToInterrupt(crash_btn_re), crashed_re, RISING); // Stoßstange Taster rechts
  attachInterrupt(digitalPinToInterrupt(crash_btn_li), crashed_li, RISING); // Stoßstange Taster rechts
  pinMode(PIN_TRIGGER, OUTPUT); // PINs Ultraschall Sensor
  pinMode(PIN_ECHO, INPUT);
  
  pinMode(directionPin_A, OUTPUT); // PINs Motorsteuerung
  pinMode(pwmPin_A, OUTPUT);
  pinMode(directionPin_B, OUTPUT);
  pinMode(pwmPin_B, OUTPUT);
}

void programmStart() // Abfrage Taster für den Start. Erstes mal drücken Roboter setzt sich in Bewegung, zweites mal, Roboter stoppt
{
  if((millis() - alteZeit) > entprellZeit) 
  { 
    if(start == false)
    {
      start = true;
      crash = false;
    }
    else
    {
      start = false;
    }
    alteZeit = millis();
  }
}

void crashed_li() // Crash am Taster links, per Interrupt
{
  crash_li = true;
}

void crashed_re() // Crash am Taster rechts, per Interrupt
{
  crash_re = true;
}

int getDistance() // Ultraschallsensor auswerten
{
  unsigned long duration_SR04;
  unsigned int distance;
  
  digitalWrite(PIN_TRIGGER, LOW);
  delayMicroseconds(2);
  digitalWrite(PIN_TRIGGER, HIGH);
  delayMicroseconds(10);

  duration_SR04 = pulseIn(PIN_ECHO, HIGH);
  distance = duration_SR04/58;
  return distance;
}

void backUP(int a)
{
  switch(a)
  {
    case 1: // Fahrt bei Hindernis am Ultraschall Sensor
      digitalWrite(directionPin_A, HIGH);
      digitalWrite(directionPin_B, HIGH);
      delay(2000);
      digitalWrite(directionPin_A, LOW);
      digitalWrite(directionPin_B, HIGH);
      analogWrite(pwmPin_A, 100);
      analogWrite(pwmPin_B, 100);
      delay(2000);
      analogWrite(pwmPin_A, 0);
      analogWrite(pwmPin_B, 0);
    break;
    case 2: // Fahrt bei Hindernis am Taster links
      digitalWrite(directionPin_A, HIGH);
      digitalWrite(directionPin_B, HIGH);
      delay(2000);
      digitalWrite(directionPin_A, HIGH);
      digitalWrite(directionPin_B, LOW);
      analogWrite(pwmPin_A, 100);
      analogWrite(pwmPin_B, 100);
      delay(2000);
      analogWrite(pwmPin_A, 0);
      analogWrite(pwmPin_B, 0);
    break;
    case 3: // Fahrt bei Hindernis am Taster rechts
      digitalWrite(directionPin_A, HIGH);
      digitalWrite(directionPin_B, HIGH);
      delay(2000);
      digitalWrite(directionPin_A, LOW);
      digitalWrite(directionPin_B, HIGH);
      analogWrite(pwmPin_A, 100);
      analogWrite(pwmPin_B, 100);
      delay(2000);
      analogWrite(pwmPin_A, 0);
      analogWrite(pwmPin_B, 0);
    break;
  }
}


void goForward(int pwm)
{
  int dist = 0;
  dist = getDistance();
  if(start == false) // Start Taster nicht aktiv
  {
    analogWrite(pwmPin_A, 0);
    analogWrite(pwmPin_B, 0);
  }
  if(dist < 10)
  {
    crash = true;
  }
  if((start == true) && (crash == false)) // kein Crash erkannt, Start Taster aktiv
  {
    digitalWrite(directionPin_A, LOW);
    digitalWrite(directionPin_B, LOW);
    analogWrite(pwmPin_A, pwm);
    analogWrite(pwmPin_B, pwm);
  }
  if((start == true) && (crash == true)) // Crash mit dem Ultraschall Sensor erkannt
  {
    a = 1;
    backUP(a);
    crash = false;
  }
  if((start == true) && (crash_li == true)) // Crash mit dem linken Stoßstangen Taster erkannt
  {
    a = 2;
    backUP(a);
    crash_li = false;
  }
  if((start == true) && (crash_re == true)) // Crash mit dem rechten Stoßstangen Taster erkannt
  {
    a = 3;
    backUP(a);
    crash_re = false;
  }

}

void loop() 
{
  goForward(120); // Programm "Fahren"
}