/**************************************************************************** ** ** ** Programm-Datei: SERVO.NQC ** ** ** ** Autor: Martin Schmidt Erstellt: 07.03.2006 ** ** ** ** Version: 0.10 Letzte Änderung: 09.03.2006 ** ** ** ***************************************************************************** ** ** ** Zugriffsfunktionen für Servomotor-Steuerung ** ** ** ** Beim Nachbau eines eigenen Servomotors müssen alle Messwerte in ** ** Funktion servo_init() nochmal überprüft und nachgemessen werden. ** ** ** ** Alle Funktionen gehen davon aus, dass der Sensor mit aufsteigendem ** ** Winkel auch aufsteigende Messwerte liefert. ** ** ** ****************************************************************************/ #define SERVO_SENSOR SENSOR_1 /* Hier ist das Servo-Poti angeschlossen */ #define SERVO_MOTOR OUT_A /* Hier ist der Servo-Motor angeschlossen */ #define OUT_SLOW_2 2 /* Noch weniger als OUT_HALF */ #define OUT_SLOW_1 1 #define WINK_MIN -135 /* Niedrigster erreichbarer Winkel */ #define WINK_MAX 135 /* Höchster erreichbarer Winkel */ #define WINK_SCHRITT 15 /* Winkel-Schrittweite in der Messwert-Tabelle */ #define MESSTAB_MAX 19 /* Anzahl Einträge in der Messwert-Tabelle */ int wink_soll; /* Winkel, den das Servo erreichen soll */ int wink_disp; /* Winkel, der auf dem Display angezeigt wird */ int mess_soll; /* Messwert, den das Servo erreichen soll */ int mess_ist; /* Messwert, auf dem das Servo gerade steht */ int messtab[MESSTAB_MAX]; /* Tabelle von Messwerten über alle Winkel */ /**************************************************************************** ** ** ** Unterprogramm: servo_init ** ** ** ** Parameter: keine ** ** ** ** Beschreibung: Dieses Unterprogramm initialisiert die Messwert- ** ** Tabelle. Sie muss auf jeden Fall einmalig ** ** aufgerufen worden sein, bevor irgend eine andere ** ** der Servo-Funktionen benutzt wird! ** ** ** ****************************************************************************/ sub servo_init () { Off (SERVO_MOTOR); messtab[0] = 53; /* -135 ° */ messtab[1] = 59; /* -120 ° */ messtab[2] = 73; /* -105 ° */ messtab[3] = 92; /* - 90 ° */ messtab[4] = 130; /* - 75 ° */ messtab[5] = 180; /* - 60 ° */ messtab[6] = 234; /* - 45 ° */ messtab[7] = 273; /* - 30 ° */ messtab[8] = 314; /* - 15 ° */ messtab[9] = 355; /* 0 ° */ messtab[10] = 381; /* + 15 ° */ messtab[11] = 414; /* + 30 ° */ messtab[12] = 445; /* + 45 ° */ messtab[13] = 468; /* + 60 ° */ messtab[14] = 489; /* + 75 ° */ messtab[15] = 501; /* + 90 ° */ messtab[16] = 505; /* +105 ° */ messtab[17] = 506; /* +120 ° */ messtab[18] = 507; /* +135 ° */ wink_soll = 0; wink_disp = 0; } /**************************************************************************** ** ** ** Inline-Funktion: servo_mess2wink ** ** ** ** Parameter: Sensor-Messwert (0 .. 1023) ** ** ** ** Rückgabe: Winkel (WINK_MIN .. WINK_MAX) ** ** ** ** Beschreibung: Diese Inline-Funktion berechnet den Winkel des ** ** Servos aus einem vorgegebenen Sensor-Messwert. ** ** ** ** Alle Winkel werden intern mit dem zehnfachen Wert verarbeitet, damit ** ** die Rundungsfehler beim Rechnen nicht so stark ins Gewicht fallen. ** ** ** ****************************************************************************/ void servo_mess2wink (int mess, int &wink) { int messtab_index; int mess_1; int mess_2; int mess_diff_1; int mess_diff_2; int wink_1; int wink_diff; if (mess <= messtab[0]) /* Messwert ist (zu) klein */ { wink = WINK_MIN; return; } if (mess >= messtab[MESSTAB_MAX - 1]) /* Messwert ist (zu) groß */ { wink = WINK_MAX; return; } /* Erhöhung der internen Rechen-Genauigkeit: Alles 10fach nehmen */ mess = mess * 10; mess_1 = messtab[0] * 10; wink_1 = WINK_MIN; /* Durch die Messwert-Tabelle gehen, bis der Messwert erreit ist */ for (messtab_index = 1; messtab_index < MESSTAB_MAX; messtab_index ++) { mess_2 = messtab[messtab_index] * 10; if (mess == mess_2) /* Messwert genau getroffen */ { wink = wink_1 + WINK_SCHRITT; /* Winkel ist bekannt */ return; } if (mess < mess_2) /* Meswert überschritten */ { /* Verhältnis des Messwerts zwischen den 2 Eckwerten berechnen */ mess_diff_1 = mess_2 - mess_1; mess_diff_2 = mess - mess_1; wink_diff = WINK_SCHRITT * 10 * mess_diff_2 / mess_diff_1; /* Winkel berechnen (mit kaufmännischer Rundung) */ wink = wink_1 + ((wink_diff + 5) / 10); return; } mess_1 = mess_2; /* 1 Schritt weiter in der Messwert-Tabelle */ wink_1 += WINK_SCHRITT; /* Winkel einen Schritt weiterdrehen */ } /* Hier sollte der RCX eigentlich nie hinkommen */ wink = WINK_MAX; } /**************************************************************************** ** ** ** Inline-Funktion: servo_wink2mess ** ** ** ** Parameter: Winkel (WINK_MIN .. WINK_MAX) ** ** ** ** Rückgabe: Sensor-Messwert (0 .. 1023) ** ** ** ** Beschreibung: Diese Inline-Funktion berechnet den Sensor-Messwert ** ** aus einem vorgegebenen Winkel. ** ** ** ****************************************************************************/ void servo_wink2mess (int wink, int &mess) { int messtab_index; int mess_1; int mess_2; int mess_diff; int wink_1; int wink_rel; int wink_diff; if (wink <= WINK_MIN) /* Winkel ist (zu) klein */ { mess = messtab[0]; return; } if (wink >= WINK_MAX) /* Winkel ist (zu) groß */ { mess = messtab[MESSTAB_MAX - 1]; return; } wink_rel = wink - WINK_MIN; /* Winkel relativ zum Minimum */ messtab_index = wink_rel / WINK_SCHRITT; /* Index in Messwert-Tabelle */ mess_1 = messtab[messtab_index]; /* Eckwerte aus Tabelle holen */ mess_2 = messtab[messtab_index + 1]; mess_diff = mess_2 - mess_1; /* Winkeldifferenz zum unteren Eckwert berechnen */ wink_1 = WINK_MIN + messtab_index * WINK_SCHRITT; wink_diff = wink - wink_1; if (wink_diff == 0) /* Unterer Eckwert genau getroffen */ { mess = mess_1; /* Messwert ist bekannt */ return; } /* Verhältnis des Winkels zwischen den 2 Eckwerten berechnen */ mess = mess_1 + (mess_diff * wink_diff / WINK_SCHRITT); } /**************************************************************************** ** ** ** Unterprogramm: servo_fahr2wink ** ** ** ** Parameter: Sollwinkel in globaler Variable wink_soll ** ** ** ** Beschreibung: Diese Inline-Funktion fährt das Servo möglichst genau ** ** auf einen vorgegebenen Winkel. ** ** ** ****************************************************************************/ sub servo_fahr2wink () { int wink; /* Interne Arbeitskopie des Sollwinkels */ int power; /* Motor-Geschwindigkeit */ int passt; /* Sollposition getroffen */ int mess_diff; /* Differenz zur Sollposition */ wink = wink_soll; if (wink < WINK_MIN) /* Winkel ist zu klein */ { wink = WINK_MIN; } if (wink > WINK_MAX) /* Winkel ist zu groß */ { wink = WINK_MAX; } servo_wink2mess (wink, mess_soll); /* Soll-Messwert berechnen */ passt = 0; while (passt < 10) /* ######## Servopositions-Regelschleife ######## */ { /* Servo-Messwert holen und Differenz zur Sollposition berechnen */ mess_ist = SERVO_SENSOR; mess_diff = mess_ist - mess_soll; /* Servomotor verstellen, wenn nötig */ if (mess_diff < -1) { if (mess_diff < -10) { SetPower (SERVO_MOTOR, OUT_FULL); } else if (mess_diff < -3) { SetPower (SERVO_MOTOR, OUT_SLOW_2); } else { SetPower (SERVO_MOTOR, OUT_SLOW_1); } OnRev (SERVO_MOTOR); passt = 0; } else if (mess_diff > 1) { if (mess_diff > 10) { SetPower (SERVO_MOTOR, OUT_FULL); } else if (mess_diff > 3) { SetPower (SERVO_MOTOR, OUT_SLOW_2); } else { SetPower (SERVO_MOTOR, OUT_SLOW_1); } OnFwd (SERVO_MOTOR); passt = 0; } else { Off (SERVO_MOTOR); /* Sollposition erreicht */ passt ++; } Wait (3); } } /**************************************************************************** ** ** ** Task: servo_task ** ** ** ** Parameter: keine ** ** ** ** Beschreibung: Dieser Task soll parallel zum Task main laufen. ** ** Er versucht, den Servomotor ständig möglichst ** ** genau auf wink_soll zu halten. ** ** Um das Servo zu verstellen, reicht es also, dass ** ** Task main die Variable wink_soll verändert. ** ** ** ****************************************************************************/ task servo_task () { int wink_alt; /* Bisheriger Sollwinkel */ int wink; /* Interne Kopie des Sollwinkels */ int mess_diff; /* Differenz zur Sollposition */ wink_alt = -9999; /* absichtlich jenseits des erlaubten Rahmens */ servo_init (); while (1) /* ######## Endlose Servopositions-Regelschleife ######## */ { wink = wink_soll; /* Aktuellen Wert des Sollwinkels holen */ if (wink_alt != wink) /* Hat sich der Sollwinkel geändert? */ { wink_alt = wink; /* Ja: Neuen Winkel merken, Wert prüfen */ if (wink < WINK_MIN) /* Winkel ist zu klein */ { wink = WINK_MIN; } if (wink > WINK_MAX) /* Winkel ist zu groß */ { wink = WINK_MAX; } servo_wink2mess (wink, mess_soll); /* Soll-Messwert berechnen */ } /* Servo-Messwert holen und Differenz zur Sollposition berechnen */ mess_ist = SERVO_SENSOR; mess_diff = mess_ist - mess_soll; /* Servomotor verstellen, wenn nötig */ if (mess_diff < -1) { if (mess_diff < -10) { SetPower (SERVO_MOTOR, OUT_FULL); } else if (mess_diff < -3) { SetPower (SERVO_MOTOR, OUT_SLOW_2); } else { SetPower (SERVO_MOTOR, OUT_SLOW_1); } OnRev (SERVO_MOTOR); } else if (mess_diff > 1) { if (mess_diff > 10) { SetPower (SERVO_MOTOR, OUT_FULL); } else if (mess_diff > 3) { SetPower (SERVO_MOTOR, OUT_SLOW_2); } else { SetPower (SERVO_MOTOR, OUT_SLOW_1); } OnFwd (SERVO_MOTOR); } else { Off (SERVO_MOTOR); /* Sollposition erreicht */ } Wait (3); } } /**************************************************************************** ** ** ** Inline-Funktion: servo_fahrtest ** ** ** ** Parameter: keine ** ** ** ** Beschreibung: Diese Inline-Funktion fährt das Servo auf verschiedene ** ** Winkel. Das Display zeigt dabei an, welchen Winkel ** ** das Servo als nächstes erreichen soll. ** ** Gut geeignet zum Testen, ob die Funktion ** ** servo_fahr2wink zuverlässig funktioniert. ** ** ** ****************************************************************************/ void servo_fahrtest (void) { /* Aktuellen Winkel auf dem Display anzeigen, kein Dezimalpunkt */ SetUserDisplay (wink_disp, 0); servo_init (); wink_soll = 0; wink_disp = wink_soll; /* Servo auf Winkel 0° fahren */ servo_fahr2wink (); Wait (200); wink_soll = -10; wink_disp = wink_soll; /* Servo auf Winkel -10° fahren */ servo_fahr2wink (); Wait (200); wink_soll = 10; wink_disp = wink_soll; /* Servo auf Winkel +10° fahren */ servo_fahr2wink (); Wait (200); wink_soll = -20; wink_disp = wink_soll; /* Servo auf Winkel -20° fahren */ servo_fahr2wink (); Wait (200); wink_soll = 20; wink_disp = wink_soll; /* Servo auf Winkel +20° fahren */ servo_fahr2wink (); Wait (200); wink_soll = -30; wink_disp = wink_soll; /* Servo auf Winkel -30° fahren */ servo_fahr2wink (); Wait (200); wink_soll = 30; wink_disp = wink_soll; /* Servo auf Winkel +30° fahren */ servo_fahr2wink (); Wait (200); wink_soll = -40; wink_disp = wink_soll; /* Servo auf Winkel -40° fahren */ servo_fahr2wink (); Wait (200); wink_soll = 40; wink_disp = wink_soll; /* Servo auf Winkel +40° fahren */ servo_fahr2wink (); Wait (200); wink_soll = -50; wink_disp = wink_soll; /* Servo auf Winkel -50° fahren */ servo_fahr2wink (); Wait (200); wink_soll = 50; wink_disp = wink_soll; /* Servo auf Winkel +50° fahren */ servo_fahr2wink (); Wait (200); wink_soll = -60; wink_disp = wink_soll; /* Servo auf Winkel -60° fahren */ servo_fahr2wink (); Wait (200); wink_soll = 60; wink_disp = wink_soll; /* Servo auf Winkel +60° fahren */ servo_fahr2wink (); Wait (200); wink_soll = -70; wink_disp = wink_soll; /* Servo auf Winkel -70° fahren */ servo_fahr2wink (); Wait (200); wink_soll = 70; wink_disp = wink_soll; /* Servo auf Winkel +70° fahren */ servo_fahr2wink (); Wait (200); wink_soll = -80; wink_disp = wink_soll; /* Servo auf Winkel -80° fahren */ servo_fahr2wink (); Wait (200); wink_soll = 80; wink_disp = wink_soll; /* Servo auf Winkel +80° fahren */ servo_fahr2wink (); Wait (200); wink_soll = -90; wink_disp = wink_soll; /* Servo auf Winkel -90° fahren */ servo_fahr2wink (); Wait (200); wink_soll = 90; wink_disp = wink_soll; /* Servo auf Winkel +90° fahren */ servo_fahr2wink (); Wait (200); wink_soll = 0; wink_disp = wink_soll; /* Servo auf Winkel 0° fahren */ servo_fahr2wink (); Wait (200); } /**************************************************************************** ** ** ** Inline-Funktion: servo_messtest ** ** ** ** Parameter: keine ** ** ** ** Beschreibung: Diese Inline-Funktion zeigt den aktuell eingestellten ** ** Winkel auf dem Display an. Es gibt keine Rückkehr! ** ** Gut geeignet zum Testen, ob die Funktion ** ** servo_mess2wink zuverlässig funktioniert. ** ** ** ****************************************************************************/ void servo_messtest (void) { int wink_ist; /* Aktuellen Winkel auf dem Display anzeigen, kein Dezimalpunkt */ SetUserDisplay (wink_disp, 0); servo_init (); while (1) { mess_ist = SERVO_SENSOR; /* Messwert holen */ servo_mess2wink (mess_ist, wink_ist); /* Winkel berechnen */ wink_disp = wink_ist; /* Winkel anzeigen */ Wait (5); } }