Cubli, ein Würfel mit Lageregelung zum Aufrichten und Ballancieren

Aus der Mikrocontroller.net Artikelsammlung, mit Beiträgen verschiedener Autoren (siehe Versionsgeschichte)
Wechseln zu: Navigation, Suche

von M. Dittrich

Fast jeder kennt Ihn, den Würfel welche Forscher des Instituts für dynamische Systeme der ETH Zürich entwickelt haben. Es wurde schon häufig versucht Ihn zu kopieren, jedoch sind ebenso viele daran gescheitert. Die Technik gleicht einem inversen Pendel. Die Mathematik dahinter sind Differentialgleichungen eines nichtlinearem Systems 4. Ordnung.

Ich möchte mit euch zusammen dieses tolle Projekt erneut entwickeln. Der aktuelle Projektstand wird regelmäßig von mir aktualisiert. Letztes Update 04.05.2022

Cubli Konstruktion Cubli Konstruktion

Technik[Bearbeiten]

  • Motoren: Maxon EC45 flat 12V mit Hallsensoren
  • Motorcontroller: MP6532 3ph BLDC
  • CPU: Basierend auf Arduino MKR1000
  • Sensoren: MPU-9250 (Anzahl u. Anordnung TBD)
  • Akku: LiPo 3S 1500mAh

Mainboard[Bearbeiten]

Die ersten Versuche fanden an einem Original Arduino MKR1000 statt. Dieser ist mit einem Atmel SAMD21 Core ausgestattet. Nachdem klar war, dass die Hardware funktioniert, wurde ein Layout für ein komplettes Mainboard erstellt.

Cubli Mainboard TOP Cubli Mainboard BOT

BLDC Endstufe[Bearbeiten]

Die ersten Tests mit der Hardware ergaben ein Problem mit der Drehzahlregelung bei kleinen Drehzahlen (n < 0,3 s^-1). Ursächlich hierfür war, für den Interrupt nur die steigende Flanke von H1 zu nutzen. Somit ergab sich eine Auflösung von 8/rev. (bei 8 Polpaaren) welche für eine saubere Regelung ungenügend war.

Eine mögliche Lösung ist das interrupten auf jede Flanke aller 3 Hallsensoren. Somit ergibt sich eine Auflösung von 48/rev. welche schon ausreichend für eine saubere Regelung ist. Hier die Umsetzung mittels kleinem PIC12F1572 und dessen Software.

Cubli BLDC TOP Cubli BLDC BOT Cubli BLDC Foto

void main(void)
{
    // initialize the device
    SYSTEM_Initialize();
    // Enable the Global Interrupts
    INTCONbits.GIE = 1;
    // Enable the Peripheral Interrupts
    INTCONbits.PEIE = 1;

    while(1)
    {
        // Motor pole pairs: 8 * (6 hall signal edges) = 48 interrupts per rev.
        // Minimum Motor frequency down to 0,2 Hz * 48 = 9,6 Hz = 104,1666 ms period
        // The Motors are capable up to 131 Hz * 48 = 7,874 kHz = 127,0002 us period
        // Check for rotor stall t > 300 ms
        if(dt >= 30000) {
            lasttime = 0;
            // Prevent over- or underflow at uint16 duty
            dt = dt_max;
            // Set 50% PWM signal
            for(i = 0; i < 5; i ++)
                duty[i] = duty_mid;
        }
        // Calculate only at continues movement
        if(hall > 0) {
            hall = 0;
            // Update loop period
            lasttime = time;
            // Prevent over- or underflow at uint16 duty
            if(dt <= dt_min)
                dt = dt_min;
            // Calculate Hall equivalent PWM signal
            // Range of dt [13 .. 424528]
            duty[i ++] = duty_mid + (sign * (dt_max / dt));
        }
        // Writing to registers
        if(i >= 4) {
            // Calculate arithmetic mean
            for(i = 0; i < 4; i ++)
                dc += duty[i];
            // Set calculated DC
            // Bit shift by extra 2 <-> dc /= 4;
            PWM1DCH = dc >> 10;     // Writing 8 MSBs to PWMDCH register
            PWM1DCL = dc >> 2;      // Writing 8 LSBs to PWMDCL register
            // Clear buffer vars
            dc = i = 0;
            // Load settings from Buffer
            PWM1LDCONbits.LDA = 1;
        }
        // Generate a pulse on RA4 (to verify the loop frequency)
        LATAbits.LATA4 = 1;
        LATAbits.LATA4 = 0;
        // Calculate delta t
        dt = time - lasttime;
        // Clear WDT
        CLRWDT();
    }
}

void __interrupt() INTERRUPT_InterruptManager(void)
{
    if(INTCONbits.IOCIE == 1 && INTCONbits.IOCIF == 1) {
        // The IOCIF Flag bit is read-only and cleared when all the Interrupt-On-Change flags
        // in the IOCxF registers have been cleared by software
        // Clear all IOCxF flags
        IOCAF = 0x00;
        // Update field-orientation buffer
        fo <<= 3;
        fo |= PORTA & 0x07;
        // Increment hall signal edges
        hall ++;
        // Determine rotation direction
        switch(fo & 0x3F) {
            // All positive sequences
            case 0x2C: sign = 1; break;
            case 0x26: sign = 1; break;
            case 0x32: sign = 1; break;
            case 0x13: sign = 1; break;
            case 0x19: sign = 1; break;
            case 0x0D: sign = 1; break;
            // All negative sequences
            case 0x0B: sign = -1; break;
            case 0x1A: sign = -1; break;
            case 0x16: sign = -1; break;
            case 0x34: sign = -1; break;
            case 0x25: sign = -1; break;
            case 0x29: sign = -1; break;
        }
    }
    if(PIE1bits.TMR2IE == 1 && PIR1bits.TMR2IF == 1) {
        // Clear the TMR2 interrupt flag
        PIR1bits.TMR2IF = 0;
        // Increment every 10 us
        time ++;
    }
}

Weiterhin wurde das PWM-Signal, was die Geschwindigkeit des Rotors darstellt, auf 50% DC für 0 rpm geändert. Damit ist auch ein Feedback der momentanen Richtung möglich. Ohne dieses Feedback würde der Regelkreis nur das Vorzeichen der Vorsteuerung kennen.

Downloads[Bearbeiten]

Schaltplan MoBo
Schaltplan BLDC