C++ Interrupt Service Routine / Debugging (AVR C/C++)

[Vulpecula]

Hallo zusammen!

Ich habe hier einen selbstbalancierenden Roboter, der auch gut funktioniert. Die Balance wird von einem PID-Regler gesteuert; den Teil habe ich soweit auch verstanden. Der PID Regler spuckt alle 4ms (250Hz) einen neuen Wert zur Ansteuerung der Motoren aus (throttle_left_motor bzw. throttle_right_motor).

Mein eigentliches Problem liegt beim Verständnis der ISR, in der die eigentliche Ansteuerung der Motoren stattfindet. Die ISR wird alle 20µs (50kHz) aufgerufen. Hier mal der Code dazu (sorry für die miese Formatierung - ich hab es so aus dem Beispielcode übernommen um die originalen Kommentare zu erhalten):

ISR(TIMER2_COMPA_vect){
  //Left motor pulse calculations
  throttle_counter_left_motor ++;                                  //Increase the throttle_counter_left_motor variable by 1 every time this routine is executed
  if(throttle_counter_left_motor > throttle_left_motor_memory){    //If the number of loops is larger then the throttle_left_motor_memory variable
    throttle_counter_left_motor = 0;                               //Reset the throttle_counter_left_motor variable
    throttle_left_motor_memory = throttle_left_motor;              //Load the next throttle_left_motor variable
    if(throttle_left_motor_memory < 0){                            //If the throttle_left_motor_memory is negative
      PORTD &= 0b11110111;                                         //Set output 3 low to reverse the direction of the stepper controller
      throttle_left_motor_memory *= -1;                            //Invert the throttle_left_motor_memory variable
    }
    else PORTD |= 0b00001000;                                      //Set output 3 high for a forward direction of the stepper motor
  }
  else if(throttle_counter_left_motor == 1)PORTD |= 0b00000100;    //Set output 2 high to create a pulse for the stepper controller
  else if(throttle_counter_left_motor == 2)PORTD &= 0b11111011;    //Set output 2 low because the pulse only has to last for 20us
 
  //right motor pulse calculations
  throttle_counter_right_motor ++;                                 //Increase the throttle_counter_right_motor variable by 1 every time the routine is executed
  if(throttle_counter_right_motor > throttle_right_motor_memory){  //If the number of loops is larger then the throttle_right_motor_memory variable
    throttle_counter_right_motor = 0;                              //Reset the throttle_counter_right_motor variable
    throttle_right_motor_memory = throttle_right_motor;            //Load the next throttle_right_motor variable
    if(throttle_right_motor_memory < 0){                           //If the throttle_right_motor_memory is negative
      PORTD |= 0b00100000;                                         //Set output 5 low to reverse the direction of the stepper controller
      throttle_right_motor_memory *= -1;                           //Invert the throttle_right_motor_memory variable
    }
    else PORTD &= 0b11011111;                                      //Set output 5 high for a forward direction of the stepper motor
  }
  else if(throttle_counter_right_motor == 1)PORTD |= 0b00010000;   //Set output 4 high to create a pulse for the stepper controller
  else if(throttle_counter_right_motor == 2)PORTD &= 0b11101111;   //Set output 4 low because the pulse only has to last for 20us
}

Ich möchte die Funktion besser verstehen, komme aber durch reines Draufschauen nicht weiter. Das Ganze auf dem Papier nachzuvollziehen wird dazu schnell unübersichtlich. Daher frage ich mich, wie man derartige Funktonen am besten debuggen kann. Es würde mir für das Verständnis nämlich schon helfen, das Verhalten einzelner Variablen beobachten zu können. Vielleicht gibt es aber auch Tools zur Simulation, die einem helfen, solche Funktonen zu beobachten?

In der Regel würde ich Variablen einfach über UART ausgeben und könnte so deren Veränderungen nachvollziehen, aber das ist hier nicht praktikabel da serielle Ausgaben in einer ISR auf einem Mikrocontroller ein No-Go sind. Dazu kommt, dass die Frequenz, mit der die Funktion aufgerufen wird, kaum noch eine Auswertung via UART zulassen würde.

Vielleicht hat ja jemand einen Tipp für mich. Vielen Dank und Grüße - Vulpecula

P.S.: Falls jemand auf den ersten Blick erkennen kann, wie genau diese Funktion arbeitet, dann lasst es mich gerne wissen.

 

[BlackMark]

Die Kommentare von diesem Beispielcode sind ein gutes Beispiel dafür, wie man nicht kommentieren sollte. throttle_counter_left_motor ++; //Increase the throttle_counter_left_motor variable by 1 every time this routine is executed Ja, danke, die ++ Syntax sollte bekannt sein und muss nicht kommentiert werden. Zusätzlich ist PORTD &= 0b11110111; so ziemlich die schlimmste Art das hinzuschreiben. Üblicherweise schreibt man hier PORTD &= ~(1 << PD3) und kann sofort sehen, dass es um Pin-D3 geht, ohne vorher die Bits abzählen zu müssen.

Den Code sauber zu schreiben hilft auf jeden Fall beim Verstehen. Hier gibt es nämlich sehr viel refactoring Potential. Code duplication eliminieren (left und right sind einfach nur copy-paste), statt den Kommentaren kann man die Sachen auch in Funktionen auslagern (die der Compiler dann eh wieder inlined), dann macht der Name der Funktion das Kommentar überflüssig. Zum Beispiel left_motor.invert() statt throttle_left_motor_memory *= -1.

Wie sinnvoll es ist den Code zu refactoren ohne genau verstanden zu haben was er macht hängt davon ab, ob man zumindest versteht was der Code macht, auch wenn man nicht versteht warum er das macht.

Sinnvoller ist es vermutlich zuerst herauszufinden was und warum der Code das macht, was er macht. Das ist mit Microcontrollern deutlich schwerer als mit normalen PC Programmen, aber es gibt trotzdem einige Möglichkeiten.

Hardware Debugger:
Es gibt für praktisch alle Microcontroller spezielle Debugging-Hardware, die recht teuer sein kann, je nach Chip. Für Arduino Uno/Pro Mini aka den ATmega328P Chip, gibt es zum Beispiel diesen Programmer/Debugger: https://www.amazon.de/dp/B013S5ANOQ
Mit Atmel Studio kann man dann debuggen wie man auch ein normales PC Programm debuggen würde. Breakpoints, durch den Code steppen, Inhalt von Variablen (und Hardware Register) ansehen wird alles damit möglich.
Atmel Studio hat zusätlich auch einen Simulator, mit dem man auch debuggen kann, das funktioniert aber logischerweise nur, wenn man nicht auf Hardware Interaktionen angewiesen ist.

Software Debugger:
Solange man nur Software debuggen will und gar nicht auf die Hardware angewiesen ist, kann man auch ein normales PC Programm schreiben und dieses debuggen. Hardware Kompenenten wie GPIO Pins kann man ganz einfach durch globale Variablen modellieren. Aber sobald man mit komplizierterer Hardware interagiert, oder side-effects der Hardware benötigt, funktioniert dieser Ansatz nicht mehr.

Printf debugging:
Der Klassiker funktioniert auch auf Microcontrollern und auch in Interrupts, man muss aber ein paar Sachen beachten. Wie du schon richtig gesagt hast, sollte man prinzipiell keine UART Ausgaben in ISRs machen. Das gilt aber nicht für Debugging. Bei Debugging ist alles erlaubt, es geht ja nicht darum den Debugging Code dann auch so beizubehalten, sondern nur vorübergehend hinzuschreiben um besser zu verstehen was überhaupt passiert.
Bei UART in ISRs muss man dann aber auch darauf achten, dass (auf AVR) es zwei Modi für UART gibt. Einmal blocking und ohne Interrupts, das ist immer Safe, auch innerhalb ISRs. Und einmal non-blocking mit Interrupts, was auch in ISRs funktionieren kann, aber nicht muss, je nachdem was man macht. Es gibt für Arduino leider keine blocking Serial Implementierung, aber es ist nicht schwer das selbst zu implementieren:

static inline void initUart(const uint32_t baudRate)
{
    UBRR0 = static_cast<uint16_t>((F_CPU + 8 * baudRate) / (16 * baudRate) - 1);
    UCSR0A = 0;
    UCSR0C = (1 << UCSZ01) | (1 << UCSZ00);
    UCSR0B = (1 << RXEN0) | (1 << TXEN0);
}
static inline void txUart(uint8_t byte)
{
    while (!(UCSR0A & (1 << UDRE0)))
        ;
    UDR0 = byte;
}
static inline void txString(const char *str)
{
    while (char ch = *str++)
        txUart(ch);
}
void uartTest()
{
    initUart(115200);
    txString("Hello World!");
}

Und wegen der Frequenz der ISR die dir zu viel Output produzieren würde, du kannst ja einen Zähler einbauen, der nur alle 1000 mal Output produziert.

Ich hoffe du kannst mit diesen Tipps etwas anfangen und kannst damit dann nachvollziehen was diese ISR macht.

Gruß
BlackMark

 

[Vulpecula]

Erstmal vielen Dank für Deine Antwort!

Ja, der Code ist leider nicht der beste. Er funktioniert anscheinend, was schonmal gut ist, aber außer der grauenhaften Kommentaren (teilweise sogar fehlerhaft dank Copy&Paste) stolpert man ständig über solche Sachen wie z.B. if(pid_output > 0)self_balance_pid_setpoint -= 0.0015;, was es schwer macht, irgendwas zu verstehen (if-Statement mit Anweisung in einer Zeile). Dazu kommt, dass der Autor sich an vielen stellen Magic Numbers aus dem ... zieht, ohne weiter darauf einzugehen. Ich kann allerdings verstehen, dass er zum Setzen der digitalen Pins über Port-Manipulation geht und den Pin nicht via Bit-Shifting setzt, da letzteres scheinbar doppelt so lange dauert. Zumindest an dieser Stelle macht es Sinn.

Bezüglich des Debuggings:
Ein Programm zur Simulation zu schreiben schießt wohl etwas über das ziel Hinaus bzw. ist mir persönlich der Zeitaufwand zu groß. Ich werde wahrscheinlich den Weg über das Debugging via UART-Ausgabe gehen; das erscheint mir zumindest aktuell am praktikabelsten zu sein. Tatsächlich liebäugele ich aber schon länger mit dem Atmel ICE, wobei ich dann weg von VS Code müsste (habe mich gerade erst mühsam von der grauenhaften Arduino IDE lösen können). Andererseits wäre es aber auch DIE Gelegenheit, weiter in die Materie einzutauchen, da Atmel Studio ja durchaus umfangreicher ist und viel mehr Funktionen bietet, als der 08/15 Arduio-User jemals benötigen würde.

 

[BlackMark]

Ich hab mir mal die Mühe gemacht den Code zu refactoren, und versucht die ganzen Kommentare als self-documenting Code hinzuschreiben. Ich weiß die Datentypen der ganzen Variablen nicht, also hab ich jetzt einfach mal int angenommen. Code ist nicht getestet, aber sollte prinzipiell (mit minimalen Anpassungen) funktionieren.

Spoiler

template <uint8_t DrivePin, uint8_t PulsePin>
class MotorThrottle {
  public:
    MotorThrottle(const volatile int &throttle) : m_throttle(throttle), m_counter(0), m_memory(0) {}

    inline void pulseHandler()
    {
        if (++m_counter > m_memory) {
            m_counter = 0;
            m_memory = drive(m_throttle);
        } else if (m_counter == 1) {
            pulseOn();
        } else if (m_counter == 2) {
            pulseOff();
        }
    }

  private:
    const volatile int &m_throttle; // External state, modified somewhere else, and only ever read from
    int m_counter;
    int m_memory;

    inline void pulseOn() const { PORTD |= (1 << PulsePin); }
    inline void pulseOff() const { PORTD &= ~(1 << PulsePin); }

    inline void forward() const { PORTD |= (1 << DrivePin); }
    inline void backward() const { PORTD &= ~(1 << DrivePin); }
    inline int drive(int throttle) const
    {
        if (throttle < 0) {
            throttle = -throttle;
            backward();
        } else {
            forward();
        }

        return throttle;
    }
};

ISR(TIMER2_COMPA_vect)
{
    static MotorThrottle<PD3, PD2> s_leftMotorThrottle(throttle_left_motor);
    static MotorThrottle<PD5, PD4> s_rightMotorThrottle(throttle_right_motor);

    s_leftMotorThrottle.pulseHandler();
    s_rightMotorThrottle.pulseHandler();
}

Ich kann allerdings verstehen, dass er zum Setzen der digitalen Pins über Port-Manipulation geht und den Pin nicht via Bit-Shifting setzt, da letzteres scheinbar doppelt so lange dauert. Zumindest an dieser Stelle macht es Sinn.

Ja, die ganze Arduino Core Library ist nicht auf Performance ausgelegt und hat teilweise extrem viel Runtime-Overhead. Man könnte aber auf jeden Fall mit modernem C++ eine Arduino Core Library ohne Runtime-Overhead implementieren, aber das ist dann mit backwards-compatability schwer.

Bezüglich Atmel Studio vs VSCode. Das ist wirklich eine schwere Entscheidung. Ich verwende beides in Kombination, um jeweils die Stärken nutzen zu können, was aber auch unnötig kompliziert ist.

Gruß
BlackMark

 

[Vulpecula]

Super, vielen Dank für den ganzen Input. Ich werde den Code ausprobieren, sobald ich dazu komme. Ich bin der Funktionsweise schon ein bisschen näher gekommen. Der PID-Regler schmeißt zwar einen Soll-Winkel raus, aber das Ganze wird in Puls-Zeiten bzw. eine Frequenz für das Auslösen eines Schrittes umgerechnet. Die ISR macht dann nichts anderes, als einen Puls auszulösen und zu warten, bis ein neuer Schritt fällig ist. Eigentlich ganz clever.

VSCode nutze ich vor allem wegen IntelliSense. Aber Atmel Studio sollte das ja auch können, oder? Immerhin ist es ein angepasstes Visual Studio.

 

[BlackMark]

Die ISR macht dann nichts anderes, als einen Puls auszulösen und zu warten, bis ein neuer Schritt fällig ist. Eigentlich ganz clever.

Die ISR macht noch etwas. Zusätzlich zum Puls wird der Motor auf vorwärts oder rückwärts gesetzt, bis zum nächsten Schritt.

VSCode nutze ich vor allem wegen IntelliSense. Aber Atmel Studio sollte das ja auch können, oder? Immerhin ist es ein angepasstes Visual Studio.

Ja, Atmel Studio kann IntelliSense, aber bei weitem nicht so gut wie VSCode oder das aktuelle Visual Studio. Das liegt daran, dass es auf Visual Studio 2015 aufbaut, und somit einige Features fehlen. Außerdem ist Atmel Studio für C ausgelegt, und das IntelliSense hat so seine Probleme mit C++, vor allem modernem C++.

Gruß
BlackMark

 

[Vulpecula]

Achso, ja... das Umschalten der Laufrichtung passiert aufgrund des Vorzeichens, was über die Throttle-Variable reinkommt. Danach wird negiert, da der Zähler ja nur nach oben zählt.
Links und rechts unterscheiden sich in der originalen Version übrigens dahingehend, dass das setzen des Bits für die Richtung jeweils vertauscht ist. Liegt daran, dass einer der Motoren ja um 180° gedreht eingebaut ist. Anstatt die verdrahtung zu ändern hat er das in Software gelöst. Wäre bei großen Stückzahlen solcher Roboter in der Praxis vielleicht way to go, aber für so ein Hobby-Projekt finde ich es fast besser, die Verdrahtung zu ändern (Spule 1 und 2 des Motors tauschen) und dafür den Code etwas zu reduzieren.

 

[BlackMark]

Links und rechts unterscheiden sich in der originalen Version übrigens dahingehend, dass das setzen des Bits für die Richtung jeweils vertauscht ist.

Du hast vollkommen recht, das ist mir nicht aufgefallen. Wieder ein Argument warum diese Art von copy-paste code-duplication sehr gefährlich ist und vermieden werden sollte.
Hier wäre eine Möglichkeit das in Software zu lösen, ohne die Hardware verändern zu müssen:

Spoiler

template <uint8_t DrivePin, uint8_t PulsePin>
class MotorThrottle {
  public:
    MotorThrottle(const volatile int &throttle, bool reverseDirection)
        : m_throttle(throttle), m_counter(0), m_memory(0), m_reverseDirection(reverseDirection)
    {
    }

    inline void pulseHandler()
    {
        if (++m_counter > m_memory) {
            m_counter = 0;
            m_memory = drive(m_throttle);
        } else if (m_counter == 1) {
            pulseOn();
        } else if (m_counter == 2) {
            pulseOff();
        }
    }

  private:
    const volatile int &m_throttle; // External state, modified somewhere else, and only ever read from
    int m_counter;
    int m_memory;
    const bool m_reverseDirection;

    inline void pulseOn() const { PORTD |= (1 << PulsePin); }
    inline void pulseOff() const { PORTD &= ~(1 << PulsePin); }

    inline void setDirection(bool forwardDir) const
    {
        if (forwardDir) PORTD |= (1 << DrivePin);
        else PORTD &= ~(1 << DrivePin);
    }
    inline void forward() const { setDirection(!m_reverseDirection); }
    inline void backward() const { setDirection(m_reverseDirection); }
    inline int drive(int throttle) const
    {
        if (throttle < 0) {
            throttle = -throttle;
            backward();
        } else {
            forward();
        }

        return throttle;
    }
};

ISR(TIMER2_COMPA_vect)
{
    static MotorThrottle<PD3, PD2> s_leftMotorThrottle(throttle_left_motor, false);
    static MotorThrottle<PD5, PD4> s_rightMotorThrottle(throttle_right_motor, true);

    s_leftMotorThrottle.pulseHandler();
    s_rightMotorThrottle.pulseHandler();
}

Ich stimme dir aber soweit zu, dass es in diesem Fall mehr Sinn macht die Hardware zu verändern, sofern der Motor das erlaubt. Ich würde sogar sagen, dass das auch bei großen Stückzahlen der bessere Weg ist. Wie der Motor angeschlossen wird sollte für die Fertigung keinen Unterschied in den Kosten machen. Wenn man natürlich ein NOT-Gate in Hardware brauchen würde, wäre das etwas anderes. Das wären zusätzliche Kosten, die man in Software billiger lösen kann.

Gruß
BlackMark

 

[Vulpecula]

Danke nochmal für Deinen Input. Man lernt doch eine ganze Menge bei solchen Hobby-Projekten. Ich werde mal versuchen, das in den Code einzupflegen. Tatsächlich bereitet mir die ISR aber gar nicht mehr so große Kopfschmerzen. Ich bin an dem Punkt angekommen, wo ich seine magic numbers verstehen muss. Ich habe gerade noch so gar keinen Plan, woher die Zahlen für die Kompensation der Nicht-Linearität der Motoren kommen. Ich frage mich auch, wieso der PID-Regler mit einer Frequenz von 250Hz arbeitet/aktualisiert wird, aber die Ansteuerung deutlich schneller erfolgt.

Ich würde sogar sagen, dass das auch bei großen Stückzahlen der bessere Weg ist. Wie der Motor angeschlossen wird sollte für die Fertigung keinen Unterschied in den Kosten machen. Wenn man natürlich ein NOT-Gate in Hardware brauchen würde, wäre das etwas anderes.

Hier reicht tatsächlich das Umpolen eines Steckers. Aber selbst das kann schon einen Einfluss auf die Kosten haben - je nachdem wie die Arbeitsschritte im Detail aussehen. Aber das ist ein Thema für den Treffpunkt.

 

[BlackMark]

Ich bin an dem Punkt angekommen, wo ich seine magic numbers verstehen muss. Ich habe gerade noch so gar keinen Plan, woher die Zahlen für die Kompensation der Nicht-Linearität der Motoren kommen.

Das Excel Spreadsheet ist auf seiner Website zum Download: http://www.brokking.net/yabr_downloads.html
Im Video siehst du auch die Excel Formeln die zum generieren der Magic Numbers verwendet wurden. Und in den Kommentaren gibt es auch Antworten auf deine Fragen.

Gruß
BlackMark

 

[Vulpecula]

Den Spreadsheet habe ich schon gesehen. Mir ist auch klar, warum er den Output mit der Inversen multipliziert. Nur der Offset von 405 und der Faktor 5500 sind mir ein Rätsel.

 

[BlackMark]

Wie gesagt, die Kommentare lesen:

The 405, 5500, 9 are values to counteract the non linear behavior. It's not calculated via some interesting method. I just puzzled with Excel until I got a straight line. Same with the PID values. I't just a matter of experience I think.

Gruß
BlackMark

 

[Vulpecula]

Ach, du ahnst es nicht... wie konnte ich DAS übersehen...?! Vielen Dank!

Okay... er hat an der Funktion rumgebastelt, bis sie einigermaßen linear ausgesehen hat. Großartig. Nicht.

 

[kuddlmuddl]

Das erinnert mich irgendwie an das hier

https://stackoverflow.com/questions/1349542/john-carmacks-unusual-fast-inverse-square-root-quake-iii

https://en.m.wikipedia.org/wiki/Fast_inverse_square_root