#61 Re: Servo Position-Halten - Adruino Nano
Verfasst: 09.08.2018 17:14:23
Funzt!
Danke Frankyfly, ich versteh davon leider nur Bahnhof. Steuerung ist ganz cool, macht Spaß damit auf dem Tisch sich an die Steuerung zu gewoehnen. Brauch nur etwas granulat zum echten baggern.frankyfly hat geschrieben:scheint ja soweit zu funktionieren. Sorry das ich aus Zeitmangel nichts Sinnvolles dazu beitragen konnte.
Ich habe aber noch eine Anmerkung zum Thema "in Hardware Umsetzen"
Der pfiffigste Weg währe hier vermutlich der abschied von Arduino IDE und deren doch recht eingeschränkten Bildbibliotheken und die direkte Verwendung der im Controller verbauten Peripherie wie Timer/Counter, Interrupt-System und DMA-Controller, allerdings ist der Atmega 328P damit nicht gerade reich bestückt (DMA: gar nicht, Timer: 2x8Bit und 1x16bit mit je 2x Output Compare Register, IRQ-System vorhanden) und gerade die 8Bit Counter dürften die Sache dann doch von der Funktionalität etwas einschränken .
telicopter hat geschrieben:Ich habe das Ganze jetzt doch mal mit der Standard Servo Library durchgezogen. Mit ein wenig Filterung der Eingangswerte ist auf jeden Fall schon mal Ruhe bei den Servos. Das ganze geht natürlich etwas auf Kosten der Reaktionsfreudigkeit, aber da es sich um einen Bagger und keinen 3D-Heli handelt, sollte das wohl zu verkraften sein. Die Fahrgeschwindigkeit ist abhängig vom Knüppelausschlag ( Alle Einstellungen können in den Defines gemacht werden). Ich habe zwar versucht, die Servoposition möglichst oft zu aktualisieren, aber da die Ablaufgeschwindigkeit des Programms hier der begrenzende Faktor ist und die Positionsaktualisierung mit diskreten Werten erfolgt, lässt sich ein leichtes Ruckeln nicht verhindern. Je langsamer der Ausleger bewegt wird bzw. je langsamer das Servo reagiert, desto geringer ist das Ruckeln natürlich. Um hier noch zu optimieren, müsste der Code an gewissen Stellen noch etwas eingestampft und gesäubert werden, um die Updaterate entsprechend zu erhöhen. Oder einfach langsame Analogservos benutzen.
Ihr könnt ja mal probieren, ob es bei euch auch läuft.
Grüße
Tim
Code: Alles auswählen
#include <Servo.h> #define RC1_PIN 5 //Empfänger Steuer-Kanal D5 #define RC2_PIN 6 //Empfänger Steuer-Kanal D6 #define RC3_PIN 7 //Empfänger Steuer-Kanal D7 #define SERVO1_PIN 8 //Servosteuerleitung D8 #define SERVO2_PIN 9 //Servosteuerleitung D9 #define SERVO3_PIN 10 //Servosteuerleitung D10 #define RC_MIN 1000 //Maximale und minimale Pulslänge #define RC_MAX 2000 #define MAX_SPEED 50 //Maximale Geschwindigkeit bei Knüppelvollausschlag (Microsekunden-Inkrement je Berechnungsschritt) #define RC_TOTZONE 10 //Totzone um die Knüppelmitte #define RC_INPUT_TOLERANCE 35 //Toleranzbereich der RC-EIngangssignale in Microsekunden #define LM_SIZE 2 //Breite des Laufenden Mittelwert-Filters Servo servo_1; Servo servo_2; Servo servo_3; unsigned int prevPulse [3] = {0, 0, 0}; //letzte berechnete Pulsweite unsigned int actPulse [3] ={0, 0, 0}; //aktuelle berechnete Pulsweite unsigned int LM[LM_SIZE][3]; // Letzte Messungen unsigned int sum [3]= {0, 0, 0}; //Summe aller Messungen int channelPin[3] = {RC1_PIN, RC2_PIN, RC3_PIN}; //Array mit den Servobelegungen unsigned int servoPosition[3] = {1500, 1500, 1500}; //aktuelle Servoposition als Pulsweite byte index = 0; //Position im Array der Messwerte void setup() { servo_1.attach(SERVO1_PIN); servo_2.attach(SERVO2_PIN); servo_3.attach(SERVO3_PIN); InitializeLMArray(); //Initialisierung der Filterfunktion Serial.begin(115200); } void loop() { readChannelFiltered(); } /***************************************************************************************************************************** // Funktion zum Simulieren der Hydraulik-Eigenschaften // *****************************************************************************************************************************/ unsigned int Hydraulik(int pulse, int servoNr ) { if (abs( pulse - 1500) <= RC_TOTZONE) //Anpassung der RC-Totzone in der Knüppelmitte { pulse = 1500; } int correction = map(pulse, RC_MIN, RC_MAX, -MAX_SPEED, MAX_SPEED); servoPosition[servoNr] += correction; if ( servoPosition[servoNr] <= RC_MIN ) { servoPosition[servoNr] = RC_MIN; } if ( servoPosition[servoNr] >= RC_MAX) { servoPosition[servoNr] = RC_MAX; } return servoPosition[servoNr]; } /***************************************************************************************************************************** // Funktion zum Auslesen und Filtern der RC-Eingangskanäle mithilfe eines laufenden Mittelwertfilters // *****************************************************************************************************************************/ void readChannelFiltered() { for (int i = 0; i <= 2 ; i++) { unsigned int currPulse = pulseIn(channelPin[i], HIGH, 25000); //Aufnahme der RC-Pulsweite if(currPulse == 0) { currPulse = 1500; //Ausgabe des Neutralwertes, wenn kein gültiger Puls vorhanden } sum [i] -= LM[index][i]; //Berechnung des gleitenden Mittelwertes LM[index][i] = currPulse; sum[i] += LM[index][i]; actPulse[i] = sum [i] / LM_SIZE; //Gleitender Mittelwert wird zur aktuellen Pulsweite if ( abs (prevPulse[i] - actPulse[i]) <= RC_INPUT_TOLERANCE) //Unterdrückung von Servozittern durch die Einführung eines Toleranzbereiches { //Wenn neuer Wert innerhalb der Toleranz liegt, wird der alte Wert weiterverwendet actPulse[i] = prevPulse[i]; } prevPulse[i] = actPulse[i]; servo_1.writeMicroseconds(Hydraulik(actPulse[0], 0)); //Möglichst häufige Ansteuerung der Servos für einen flüssigeren Lauf servo_2.writeMicroseconds(Hydraulik(actPulse[1], 1)); servo_3.writeMicroseconds(Hydraulik(actPulse[2], 2)); } index++; index = index % LM_SIZE; } /***************************************************************************************************************************** // Initialisierungsfunktion zum Auffüllen aller Arraywerte // *****************************************************************************************************************************/ void InitializeLMArray() { while ( index < LM_SIZE) { for (int i = 0; i <= 2 ; i++) { unsigned int currPulse = pulseIn(channelPin[i], HIGH, 25000); //Aufnahme der RC-Pulsweite if(currPulse == 0) { currPulse = 1500; //Ausgabe des Neutralwertes, wenn kein gültiger Puls vorhanden } sum [i] -= LM[index][i]; //Berechnung des gleitenden Mittelwertes LM[index][i] = currPulse; sum[i] += LM[index][i]; actPulse[i] = sum [i] / (index + 1 ); //Gleitender Mittelwert wird zur aktuellen Pulsweite if ( abs (prevPulse[i] - actPulse[i]) <= RC_INPUT_TOLERANCE) //Unterdrückung von Servozittern durch die Einführung eines Toleranzbereiches { //Wenn neuer Wert innerhalb der Toleranz liegt, wird der alte Wert weiterverwendet actPulse[i] = prevPulse[i]; } prevPulse[i] = actPulse[i]; } servo_1.writeMicroseconds(1500); //Servoposition in Neutrallage halten bis Initialisierung beendet servo_2.writeMicroseconds(1500); servo_3.writeMicroseconds(1500); index++; } index = 0; }