Ich habe keinen Arduino greifbar, aber müssten das nicht 17*2000 sein?TheManFromMoon hat geschrieben:// lets set a standard rate of 50 Hz by setting a frame space of 10 * 2000 = 3 Servos + 7 times 2000
CRCArduinoFastServos::setFrameSpaceA(SERVO_FRAME_SPACE,7*2000);
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. 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;
}
