Cubli, ein Würfel mit Lageregelung zum Aufrichten und Ballancieren
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
Technik
- 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
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.
BLDC Endstufe
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.
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.