fahrzeug mit vier motoren und vier encodern

**botty** · 11.11.2015, 19:47

Hallo inka,

ich hab mir Deinen Code mal angeguckt und da ich gerade selbst an
meinem ersten Projekt dran bin, versuch ich hier mal Dir Deine Zurückhaltung
gegenüber Interrupts zu nehmen und zeig Dir meinen Code.
Falls es zu ausführlich wird, dann fasse das bitte nicht als Überheblichkeit auf.
Ich weiß halt einfach nicht auf welchem Programmierstand Du bist.

Die Code Beispiele nicht zusammenkopieren, ganz unten ist der ganze Sketch.

Los geht's:
Zuerst hab ich mir einen Datentypen gemacht, der die Infos für einen Motor hält.

Code:

struct motor {
  AF_DCMotor mot;
  uint8_t enc_pin;
  // volatile damit der Compiler keine 'dummen' Optimierung macht.
  volatile uint32_t ticks;  

  unsigned long start_time; 
  unsigned long stop_time;
};

mot ist das Objekt für den PWM Kanal, enc_pin ist der Pin an den der Encoder
kommt und ticks ist die Variable in der wir die Flankenwechsel zählen.
Das "volatile" davor ist wichtig. Ohne könnte der Compiler auf die Idee kommen
was wegzuoptimieren was er nicht soll.

Faustregel: Variablen die Interrupt Service Routinen und Hauptprogramm
benutzt werden sind immer "volatile".

Da Du vier Motoren hast (ich hab nur zwei) nehmen wir ein Array vom Typ
"struct motor" und initialisieren es.

Code:

struct motor motoren[M_MAX] = { 
  { AF_DCMotor(1), 18, 0, 0, 0 }, // M_VL
  { AF_DCMotor(2), 19, 0, 0, 0 }, // M_HL
  { AF_DCMotor(3), 20, 0, 0, 0 }, // M_VR
  { AF_DCMotor(4), 21, 0, 0, 0 }  // M_HR
};

Die Spalten entsprechen den obigen Zeilen. Sprich die "1" ist der PWM Kanal und
die "18" der Pin an den der Endocder vom Motor der mit dem ersten PWM Kanal
gesteuert wird angeschlossen ist.

Da ich lieber mit Namen als mit Zahlen arbeite habe ich noch einen Aufzählungstypen
deklariert:

Code:

enum motoren_e {
  M_VL = 0,   // Motor vorne links
  M_HL,
  M_VR,
  M_HR,
  M_MAX
};

Die Namen werden auf fortlaufende unsigned integer abgebildet M_MAX hat
den Wert "4".

Wenn Du die Zahlen bei der Initialisierung so anpaßt das dein erster PWM Kanal
den vorderen linken Motor antreibt und der Encoder am entsprechenden Pin liegt, läßt
sich der Motor mit

Code:

motoren[M_VL].mot.setSpeed(255/2);
motoren[M_VL].mot.run(FORWARD);

anschalten.

Wollte man das mit allen machen geht das mit:

Code:

for(uint8_t idx = M_VL; idx < M_MAX; idx++) {
  motoren[idx].mot.setSpeed(255/2);
  motoren[idx].mot.run(FORWARD);
}

Jetzt brauchen wir für jedes Element eine Interrupt Service Routine, die aufgerufen wird wenn
sich der Zustand am Pin ändert.
Grundsätzlich haben dies Funktionen diese Signatur:

Code:

void isr_name(void);

Faustregeln für den Inhalt so einer ISR sind:
Sie muß ihre Funktion erfüllen.
Sie soll so kurz wie möglich dabei sein.

Wir wollen unseren "ticks" Zähler raufzählen und zwar für jedes Element in unserem Array.
So brauchen wir vier Stück:

Code:

void motor_isr_m_vl(void) { motoren[M_VL].ticks++; }
void motor_isr_m_vr(void) { motoren[M_VR].ticks++; }
void motor_isr_m_hl(void) { motoren[M_HL].ticks++; }
void motor_isr_m_hr(void) { motoren[M_HR].ticks++; }

Der nächste Schritt ist den Pin zu initialisieren und dem System zu erklären, welcher Pin
zu welcher ISR gehört:

Code:

void setup() {
  Serial.begin(9600);

  for(uint8_t idx = M_VL; idx < M_MAX; idx++) {
    pinMode(motoren[idx].enc_pin, INPUT_PULLUP);
  }

  attachInterrupt(digitalPinToInterrupt(motoren[M_VL].enc_pin), motor_isr_m_vl, CHANGE);
  attachInterrupt(digitalPinToInterrupt(motoren[M_HL].enc_pin), motor_isr_m_hl, CHANGE);
  attachInterrupt(digitalPinToInterrupt(motoren[M_VR].enc_pin), motor_isr_m_vr, CHANGE);
  attachInterrupt(digitalPinToInterrupt(motoren[M_HR].enc_pin), motor_isr_m_hr, CHANGE);
}

"digitalPinToInterrupt()" mapped die Pin Nummer auf das entsprechende Register und
"attachInterrupt()" setzt dafür die ISR als zweiten Parameter. Der letzte Parameter kann
die Werte RISING, FALLING oder CHANGE haben. CHANGE bedeutet bei jedem Wechsel
von HIGH nach LOW und vice versa wird die entsprechende ISR aufgerufen und in unserem Fall
der richtige "ticks" inkrementiert.

Was ist jetzt kritisch? Nun alle Elemente aus der Struktur außer "ticks" sind unkritisch.
Wenn Du auf "ticks" lesend zugreifst kann es schlimmstenfalls passieren das der Interrupt das
Lesen unterbricht und du mit einem Wert um eins verschoben weiterarbeitest.
Beim schreiben kann es passieren, daß wenn Du "ticks" einen neuen Wert zuweißt, der
Interrupt Dich unterbricht und dadurch der alte Wert verwendet wird.
Mag ersteres noch unkritisch sein,müssen wir uns vor Letzterem schützen.
Dafür gibt's im Arduino Framework die Funktionen "noInterrupts()", die alle Interrupts ausschaltet
und "interrupts()", die sie wieder einschaltet. Hier am Beispiel "motor_start()"

Code:

void motor_start(uint8_t idx, uint8_t pwm, uint8_t dir) {
  if(idx >= M_MAX)
    return;

  noInterrupts();
  motoren[idx].ticks = 0;
  interrupts();

  motoren[idx].start_time = motoren[idx].stop_time = millis();
  motoren[idx].mot.setSpeed(pwm);
  motoren[idx].mot.run(dir);
}

letzten Endes muß man jeden Zugriff auf "ticks" so schützen und damit man das beim Lesen
wie beim Schreiben nicht vergißt gibt's die Funktionen

Code:

inline uint32_t motor_hole_ticks(uint8_t idx) { 
  if(idx >= M_MAX)
    return 0;
    
  noInterrupts();
  uint32_t tmp = motoren[idx].ticks;
  interrupts();

  return tmp;
}

inline void motor_setze_ticks(uint8_t idx, uint32_t val) {
  if(idx >= M_MAX)
    return;
    
  noInterrupts();
  motoren[idx].ticks = val;
  interrupts();
}

die Du anstelle direkter Zuweisungen benutzt, so kann nichts passieren.

Zu guter Letzt der ganze Sketch in dem Du die PWM Kanäle und die
Encoder Pins anpassen müßtest. Dann kann's losgehen ob einer eine Umdrehung
macht und wenn das funktionert sind da "setupMulti()" und "loopMulti()" für alle
Motoren.

Code:

#include <AFMotor.h>

/*
 * Benamte Indexe. Wichtig: Alle Schleifen basieren auf der
 * Annahme, dass M_VL das erste Element und mit 0 initialisiert
 * ist. M_MAX muss ausserdem _immer_ das letzte Element sein da
 * es die Groesse des Arrays s.u. bestimmt.
 */
enum motoren_e {
  M_VL = 0,   // Motor vorne links
  M_HL,
  M_VR,
  M_HR,
  M_MAX
};

/* 
 *  Infos zu einem Motor zusammenfassen.
 */
struct motor {
  AF_DCMotor mot;
  uint8_t enc_pin;
  // volatile damit der Compiler keine 'dummen' Optimierung macht.
  volatile uint32_t ticks;  

  unsigned long start_time; 
  unsigned long stop_time;
};

/*
 * Hier werden die PWM Channel - Encoder Pin Paare gesetzt.
 * Restliche Elemente muessen ebenfalls initialisiert werden.
 * Werte pruefen ob sie zu den Namen oben passen.
 * Fuer die externen Interrupts sind die Pins 2,5,18-21
 * erlaubt. Hardware anpassen.
 */
struct motor motoren[M_MAX] = { 
  { AF_DCMotor(1), 18, 0, 0, 0 }, // M_VL
  { AF_DCMotor(2), 19, 0, 0, 0 }, // M_HL
  { AF_DCMotor(3), 20, 0, 0, 0 }, // M_VR
  { AF_DCMotor(4), 21, 0, 0, 0 }  // M_HR
};

 // Anzahl der Encoderstriche fuer eine Umdrehung
const static double ticks_per_rev = 40;
const static double durchmesser  = 6.5; // in cm;
const static double u_per_tick   = (3.1415926535897932384626433832795 * durchmesser) / ticks_per_rev;

/*
 * Interruptroutinen mit der gleichen Namenskonvention wie
 * oben.
 */
void motor_isr_m_vl(void) { motoren[M_VL].ticks++; }
void motor_isr_m_vr(void) { motoren[M_VR].ticks++; }
void motor_isr_m_hl(void) { motoren[M_HL].ticks++; }
void motor_isr_m_hr(void) { motoren[M_HR].ticks++; }

void setup() {
  //Serial.begin(115200);
  Serial.begin(9600);

  pinMode(motoren[M_VL].enc_pin, INPUT_PULLUP);
//  motor_init_pos(M_VL);

  /*
   * Hier wird die Verbindung hergestellt zwischen Pin und Interruptroutine.
   * Die Namen muessen zueinander passen.
   * Durch CHANGE werden beide Uebergaenge beruecksichtigt.
   */
  attachInterrupt(digitalPinToInterrupt(motoren[M_VL].enc_pin), motor_isr_m_vl, CHANGE);
}

void loop() {
  unsigned long log_time;
  String out_s = "";
  
  motor_start(M_VL, 255/2+3, FORWARD);

  log_time = millis();
  while(motor_laeuft(M_VL)) {
    
    if(motor_hole_ticks(M_VL) >= ticks_per_rev)
      motor_stop(M_VL);

    // Ticks und Strecke ausgeben
    static const int wtime = 250;
    if(log_time + wtime <= millis() ) {
      log_time = wtime + millis();
      out_s = "";
      out_s += millis() + ": ";
      out_s += M_VL + " ";
      out_s += motoren[M_VL].ticks + " ";  // Hier koennte jetzt ein Interrupt unterbrechen!
      out_s += motor_strecke_gefahren(M_VL);
      Serial.println(out_s);
    }
  }
  while(1);
  // oder
  // delay(2000);
}

void setupMulti() {
  //Serial.begin(115200);
  Serial.begin(9600);

  for(uint8_t idx = M_VL; idx < M_MAX; idx++) {
    pinMode(motoren[idx].enc_pin, INPUT_PULLUP);
//    motor_init_pos(idx);
  }

  /*
   * Hier wird die Verbindung hergestellt zwischen Pin und Interruptroutine.
   * Die Namen muessen zueinander passen.
   * Durch CHANGE werden beide Uebergaenge beruecksichtigt.
   */
  attachInterrupt(digitalPinToInterrupt(motoren[M_VL].enc_pin), motor_isr_m_vl, CHANGE);
  attachInterrupt(digitalPinToInterrupt(motoren[M_HL].enc_pin), motor_isr_m_hl, CHANGE);
  attachInterrupt(digitalPinToInterrupt(motoren[M_VR].enc_pin), motor_isr_m_vr, CHANGE);
  attachInterrupt(digitalPinToInterrupt(motoren[M_HR].enc_pin), motor_isr_m_hr, CHANGE);
}

/*
 * Alle Motoren an, bis eine Umdrehung erreicht ist. Es wird alle 
 * halbe Sekunde der Zustand der Ticks pro Motor ausgeben.
 */
void loopMulti() {
  unsigned long log_time = 0;
  unsigned long ticks_tmp = 0;
  unsigned long all_ticks_tmp[M_MAX] = { 0, 0, 0, 0 };
  String out_s = "";
  bool alle_aus = false;

  // Alle Motoren an.
  for(uint8_t idx = M_VL; idx < M_MAX; idx++) {
    if( ! motor_laeuft(idx) )
      motor_start(idx, 255/2+3, FORWARD);
  }

  while( ! alle_aus ) {
    // Wenn die Anzahl der Ticks fuer eine Umdrehung
    // erreicht ist - Motor aus.
    for(uint8_t idx = M_VL; idx < M_MAX; idx++) {
      if(motor_laeuft(idx) && motor_hole_ticks(idx) >= ticks_per_rev) {
        motor_stop(idx);
      }
    }

    // Ticks und Strecke ausgeben
    static const int wtime = 250;
    if(log_time + wtime <= millis() ) {
      out_s = "";
      out_s += millis() + ": ";

      for(uint8_t idx = M_VL; idx < M_MAX; idx++) {
        out_s += idx + " ";
        out_s += motoren[idx].ticks + " ";  // Hier koennte jetzt ein Interrupt unterbrechen!
        out_s += motor_strecke_gefahren(idx);

        ticks_tmp = 0;
        motor_ticks_per_milli(idx, &ticks_tmp);
        out_s += " " + ticks_tmp;

        if(idx == M_MAX - 1)
          out_s += "\n";
        else
          out_s += ", ";
      }

      for(uint8_t idx = 0; idx < M_MAX; idx++)
        all_ticks_tmp[idx] = 0;
        
      motor_ticks_per_milli_fuer_alle(all_ticks_tmp, M_MAX);

      for(uint8_t idx; idx < M_MAX; idx++)
        out_s += all_ticks_tmp[idx] + " ";

      Serial.println(out_s);

      log_time = wtime + millis();
    }

    // pruefen ob alle Motoren aus sind.
    for(uint8_t idx = M_VL, alle_aus = true; alle_aus && idx < M_MAX; idx++) {
      alle_aus = alle_aus && ( ! motor_laeuft(idx) );
    }
  }

  while(1);
  // Oder nach 2sec geht's von vorne los.
  // delay(2000);
}

/*
 * Faehrt den Motor auf den ersten HIGH Level des Encoderpins.
 * Falls er schon HIGH ist wird erst in ein LOW gefahren und
 * beim naechsten HIGH gestoppt.
 */
void motor_init_pos(uint8_t idx) {
  if(idx >= M_MAX)
    return;

  if(motor_laeuft(idx))
    motor_stop(idx);
    
  // Wenn wir bereits eine 1 haben, dann fahren wir erstmal bis zur
  // naechsten 0.
  if(digitalRead(motoren[idx].enc_pin)) {
    motoren[idx].mot.setSpeed(255/3);
    motoren[idx].mot.run(FORWARD);
    
    while(digitalRead(motoren[idx].enc_pin));
  }

  // Wir sind in ner 0, wissen aber nicht ob sich der Motor schon dreht.
  if( ! digitalRead(motoren[idx].enc_pin) ) {
    motoren[idx].mot.setSpeed(255/4);
    motoren[idx].mot.run(FORWARD);

    while( ! digitalRead(motoren[idx].enc_pin) );

    motoren[idx].mot.setSpeed(0);
    motoren[idx].mot.run(RELEASE);

    // Bremsen evtl. nicht noetig oder muss angepasst werden.
    /*
    delayMicroseconds(75);
    motoren[idx].mot.setSpeed(255/5);
    motoren[idx].mot.run(BACKWARD);
    delayMicroseconds(100);
    motoren[idx].mot.setSpeed(0);
    motoren[idx].mot.run(RELEASE);
    */
  }
}

/*
 * 
 */
bool motor_laeuft(uint8_t idx) {
  if(idx >= M_MAX)
    return false;

  return motoren[idx].start_time > 0 && motoren[idx].start_time == motoren[idx].stop_time;
}

/*
 * idx - der Motor Index
 * pwm und dir wie in AF_DCMotor.
 */
void motor_start(uint8_t idx, uint8_t pwm, uint8_t dir) {
  if(idx >= M_MAX)
    return;

  motor_setze_ticks(idx, 0);
  motoren[idx].start_time = motoren[idx].stop_time = millis();
  motoren[idx].mot.setSpeed(pwm);
  motoren[idx].mot.run(dir);
}

/*
 * Fuer den Fall das zwischen motor_start und motor_stop die pwm nicht 
 * geaendert wird liesse sich aus der Zeit und der Strecke s.u. grob die
 * Geschwindigkeit ermitteln (Be- und Entschleunigung vernachlaessgt).
 */
void motor_stop(uint8_t idx) {
  if(idx >= M_MAX)
    return;

  motoren[idx].mot.setSpeed(0);
  motoren[idx].mot.run(RELEASE);
  motoren[idx].stop_time = millis();
}

/*
 * Liesst den ticks Wert interrupt safe.
 */
inline uint32_t motor_hole_ticks(uint8_t idx) { 
  if(idx >= M_MAX)
    return 0;
    
  noInterrupts();
  uint32_t tmp = motoren[idx].ticks;
  interrupts();

  return tmp;
}

/*
 * Setzt den ticks Wert interrupt safe.
 */
inline void motor_setze_ticks(uint8_t idx, uint32_t val) {
  if(idx >= M_MAX)
    return;
    
  noInterrupts();
  motoren[idx].ticks = val;
  interrupts();
}

/*
 * 
 */
void motor_ticks_per_milli(uint8_t idx, unsigned long *res) {
  if(idx >= M_MAX) {
    *res = 0;
    return;  
  }
  
  *res = motor_hole_ticks(idx) / (millis() - motoren[idx].start_time);
}

/*
 * Zu _einem_ Zeitpunkt werden alle Ticks/Milli ausgerechnet.
 */
void motor_ticks_per_milli_fuer_alle(unsigned long *res, uint8_t n) {
  unsigned long *b = 0;
  unsigned long tstamp = 0;
  
  if(n != M_MAX)
    return;

  b = res;
  noInterrupts();
  tstamp = millis();
  while(b < res + n) {
    *b = motoren[b - res].ticks;
    b++;
  }
  interrupts();

  for(b = res; b < res + n; b++) {
    *b = *b / (tstamp - motoren[b - res].start_time);
  }
}

/*
 * Rechnet die gefahrenen Zentimeter anhand der Ticks aus.
 */
double motor_strecke_gefahren(uint8_t idx) {
  if(idx >= M_MAX)
    return 0.0;
    
  return motor_hole_ticks(idx) * u_per_tick;
}

/*
 * Rechnet fuer alle Motoren zu _einem_ Zeitpunkt die Strecke 
 * aus.
 */
void motor_strecke_fuer_alle(double *res, int n) {
  if(n != M_MAX)
    return;
  
  double *b = 0;
  unsigned long tmps[M_MAX];
  uint8_t idx = 0;

  noInterrupts();
  while(idx < M_MAX) {
    tmps[idx] = motoren[idx].ticks;
    idx++;
  }
  interrupts();

  for(b = res; b < res + n; b++) {
    *b = tmps[b-res] * u_per_tick;
  }
}

Der nächste Schritt wäre jetzt die Datenstruktur für einen Regler zu erweitern und diesen
zu implementieren.
Soweit bin hier noch nicht, da die Dinger Neuland für mich sind und ich jetzt erstmal
schaue ob mir eine gute Fee einen Drucker auf den heutigen Sperrmüll gestellt hat, den
für meinen Robby ausschlachten kann.

Viele Grüße

Chris