BALANCIERER (Gruppe C) Robotikpraktikum für Fortgeschrittene Sommersemester 2009 Codebeispiel: Hauptroutine int main(void) { init(); uart_puts("init\n\r"); DDRB |= (1 << PB1) | (1 << PB3); // B1 (Diode) und B3 (Servo) als Ausgänge DDRD |= (1 << PD0) | (1 << PD1); // RXD (D0) und TXD (D1) zur PC- Kommunikation DAS TEAM: #define FRAME_TIME 20 // msec //Servo-Motor initialisieren ICR1 = FRAME_TIME * 2000; Johannes Ferencik Informatik/ Chemie/ Englisch Lehramt (7) // OC1A/B: clear on Compare Match, set at TOP, TOP = ICR1 TCCR1A = (1 << COM1A1) | (1 << COM1B1) | (1 << WGM11); TCCR1B = (1 << WGM13) | (1 << WGM12) TCCR1B |= (1 << CS11); // clk = sysclk/8 (= 1/8 * 16 MHz = 2 MHz → 1/2 µsec) TCNT1 = 0; // Timer zurücksetzen Tamara Rothengaß Mathematik/ Informatik Lehramt (8) OCR1A = SERVO_BALANCE; // initiale Position ballsize = 400; // Balldurchmesser 4cm /* test_friction(); */ // Balleigenschaften messen Christoph Karcher Mathematik Diplom (9) while (1) { if (is_ball_present()) // nur wenn ein Ball auf der Wippe liegt { if (!is_ball_in_center()) // wenn Ball nicht in der Mitte liegt, in die Mitte manövrieren { Betreuer: Benjamin Reh int distance = get_average_distance(0) - SENSOR_DISTANCE / 2; // Abstand von der Mitte double scale = (distance < 0 ? -0.5 : 0.5) + 1.0 * distance / SENSOR_DISTANCE; // Skalierungsfaktor für die Wippenneigung // Neigungswinkel der Wippe über Servoposition bestimmen uint16_t servopos = min(SERVO_BOTTOM, max(SERVO_TOP, fabs(scale) * (SERVO_TOP - SERVO_BALANCE) + SERVO_BALANCE)); double alpha = 0.10426 * sin((1.35 * (SERVO_BALANCE - servopos)) / (SERVO_BALANCE - SERVO_TOP)); DAS PROJEKT: uint32_t t1, t2; get_duration(abs(distance), alpha, distance > 0 ? FAR : NEAR, &t1, &t2); Entwurf, Bau und Programmierung einer Wippe. // beschleunigen OCR1A = min(SERVO_BOTTOM, max(SERVO_TOP, scale * (SERVO_TOP - SERVO_BALANCE) + SERVO_BALANCE)); wait(t1); // bremsen OCR1A = min(SERVO_BOTTOM, max(SERVO_TOP, scale * (SERVO_BOTTOM - SERVO_BALANCE) + SERVO_BALANCE)); wait(t2); UNSERE MILESTONES: UNSERE IDEEN: // anhalten OCR1A = SERVO_BALANCE; // Wippe in Gleichgewichtslage stellen wait(3000); } else { /* test_ballsize(); */ // wenn der Ball in der Mitte liegt, Ballgröße bestimmen } 1. Konzept der Wippe: } 2. Das Legotechnik- Konzept: ● sehr flexibel ● schnell erweiterbar ● leicht ● kostengünstig ● wieder verwendbar 3. Die Programmierung: so wenig Aktion wie möglich d.h. möglichst nur eine Beschleunigungs- und Bremsbewegung HARDWARE: ● ● ● ● ● ● ● Legotechnik- Bausteine Metallschiene Servo-Motor Controller mit Atmega8-Prozessor 2 Infrarot-Sensoren Kabel Ball DER BAU: } } Codebeispiel: Ball- Präsenz- Test int is_ball_present() { int value = 0; int n; for (n = 0; n < 15; n++) { unsigned int v1 = getADCValue(NEAR); unsigned int v2 = getADCValue(FAR); if (v1 < 80 && v2 < 80) //kein Ball in der Wippe { value++; } wait(10); } return value < 5; } DIE BALL-PHYSIK: Ansatz über Energieerhaltungssatz: Zeitpunkt 1: Ball beginnt auf schiefer Wippe zu rollen (potentielle Energie) Zeitpunkt 2: Ball hat beim Beginn des Bremsmanövers die maximale Geschwindigkeit (kinetische und Rotationsenergie) Zeitpunkt 3: Ball kommt zum Stillstand (potentielle Energie) Den vergleichsweise geringen Energieverlust durch Luftreibung haben wir außer Acht gelassen, um die Rechnung einfacher zu halten. Beschleunigungs-Phase: Vergleicht man nun die Zeitpunkte 1 und 2, erhält man – unter Berücksichtigung des Energieverlustes durch Rollreibung – folgende Gleichung: Epot = Ekin + Erot + Err und nach Vereinfachen und Zusammenfassen: (g ⋅ sin(α) − frr ⋅ g ⋅ cos(α)) ⋅ s = (1/2 + 1/2 ⋅ 2/5 ⋅ (R/r)2) ⋅ v2 Hierbei ist g ≈ 9,81 m/s2 die Fallbeschleunigung, α der Neigungswinkel der Wippe, frr ≈ 0,002 der Rollreibungsfaktor, s die zurückgelegte Strecke, R der Radius des Balles, r = (R2 − (0,016 m)2)1/2 der Radius der beiden Kreise, die aus den Berührpunkten des Balles mit der Schiene gebildet wird, und v die momentane Geschwindigkeit. Nun ist die Geschwindigkeit bekanntlich die Ableitung der Strecke nach der Zeit, also v(t) = s′(t). Man erhält also eine Differentialgleichung mit einer der Lösung der Form s1(t) = a/2 ⋅ x2. Für den Parameter a erhält man dann: a = g ⋅ (sin(α) − f ⋅ cos(α)) ⋅ (1 + A ⋅ (R/r)2)-1 Brems-Phase: Vergleicht man nun entsprechend die Zeitpunkte 2 und 3, erhält man die Gleichung: Ekin + Erot = Epot + Err Analog erhält man eine Differentialgleichung mit einer Lösung der Form s2(t) = b/2 ⋅ x2 .Für den Parameter b erhält man: b = g ⋅ (sin(α) + f ⋅ cos(α)) ⋅ (1 + A ⋅ (R/r)2)-1 DIE SOFTWARE: Programmierung: programmiert mit C Besonderheiten: ● startet, sobald ein Ball in die Schiene gelegt wird ● stoppt, wenn der Ball entfernt wird ● erkennt, wenn der Ball in der Mitte liegt, und stoppt ● sobald der Ball nicht mehr in der Mitte liegt, fängt das Programm automatisch wieder von vorne an Berechnung von t1 und t2: Bezeichnet man nun den Abstand des Balles zur Mitte der Wippe mit d, kann man unsere Aufgabe beschreiben durch: Nach einer Beschleunigung der Dauer t1, die durch s1(t) und v1(t) = s1′(t) beschrieben wird, sowie einer Bremsbewegung der Dauer t2, beschrieben durch s2(t) und v2(t) = s2′(t), soll der Ball genau die Strecke d zurücklegen und dort zur Ruhe kommen. In Formeln ausgedrückt heißt das: s1(t1) + s2(t2) = d v1(t1) = v2(t2) Aus diesen beiden Formeln ergibt sich schließlich t1 = (2d ⋅ (a ⋅ (a/b + 1))-1)1/2 t2 = (2d ⋅ (b ⋅ (b/a + 1))-1)1/2 SCHWIERIGKEITEN: ● ● ● ● Stabilität Ungenauigkeit der Sensoren nicht für jeden Ball geeignet Komplexität der Ball-Physik