Automatische Schildsteuerung mit Arduino Nano und MPU6050

markusstn · 18.07.2024

Hallo zusammen ich hab mal was versucht zu basteln, leider komm ich da nicht weiter. Bräuchte mal Eure Hilfe.

Hier der Code

Code:

//Automatische Schildsteuerung mit Adruino Nano und MPU6050

#include <Servo.h>
#include <Wire.h>
#include <MPU6050.h>

int ch4=9; // Switch turn on Automatic
int ch1=6; // Schild up or down on the RC controller
int ch3=10;//Schild tilt on the RC controller
const int servoBasePin = 3;   //Servo Schild up or down
const int servoTeaArmPin = 5; //Servo Schild tilt
Servo servoBase;
Servo servoTeaArm;
int servoValRoll, servoValPitch;
MPU6050 sensor ;

int16_t ax, ay, az ;
int16_t gx, gy, gz ;



void setup() {
   Serial.begin(9600);
   pinMode(ch1, INPUT);
   pinMode(ch3, INPUT);
   servoBase.attach(servoBasePin);
   servoTeaArm.attach(servoTeaArmPin);
Serial.println  ( "Initializing the sensor" );
sensor.initialize ( );
Serial.println (sensor.testConnection ( ) ? "Successfully Connected" : "Connection failed");
delay (1000);
Serial.println ( "Taking Values from the sensor" );
delay (500);
}


void loop() {
  int ch3Pitch = pulseIn(ch3,HIGH,25000);
  int ch1Roll = pulseIn(ch1,HIGH,25000);
  int Auto = pulseIn(ch4,HIGH,25000);
  Serial.print("   Schild heben_senken : ");
  Serial.print( ch3Pitch );
  Serial.print("   Schild tillt: ");
  Serial.print( ch1Roll );
  Serial.print("   Winkel Schild_auto: ");
  Serial.print ( ay );
  Serial.print (" Auto On_OFF : ");
  Serial.println (Auto);
 
//Mapping the postions to the servos
  servoValPitch = map(ch3Pitch, 1000, 2000, 0, 180);
  servoBase.write(servoValPitch);  
  servoValRoll = map(ch1Roll, 1000, 2000, 0, 180);

// Mapping the position to the MPU6050 Sensor
  sensor.getMotion6 (&ax, &ay, &az, &gx, &gy, &gz);
  ay = map (ay, -17000, 17000, 0, 180) ;

  //servoTeaArm.write(servoValRoll);
  servoTeaArm.write(ay);

   delay(100);// increase/decrease the delay to match the movement on the gimbal of the RC controller.
}

Also wenn ich den ch4 ( einen Schalter auf der Funke nach oben schalte, also 2000ms) dann soll die Schild Automatik angeschaltet werden, sonst kann man nur mit dem Knüppel das Schild in der Neigung verstellen.
Das geht mit einer if else Anweisung, nur bekomm ich das nicht hin. Steh da etwas auf dem Schlauch.
Grüsse Markus

markusstn · 19.07.2024

Oder lang da nur ein if Befehl?

Der Schalter nicht geschalten, dann den Befehl
servoTeaArm.write(servoValRoll);

sonst den Befehl
servoTeaArm.write(ay);

ausführen

Hier nochmal der Code mit Beschreibung auf Deutsch

Code:

//Automatische Schildsteuerung mit Adruino Nano und MPU6050
// von Markus Steinbach
// 19.07.2024

#include <Servo.h>
#include <Wire.h>
#include <MPU6050.h>

int ch4=9; // Schalter auf Funke Automatik ein_aus
int ch1=6; // Servo Schild auf_ab
int ch3=10;// Servo Schild Neigung einstellen
const int servoBasePin = 3;   //Servo Schild up or down
const int servoTeaArmPin = 5; //Servo Schild tilt

Servo servoBase; 
Servo servoTeaArm; 
int servoValRoll, servoValPitch;

MPU6050 sensor ;
int16_t ax, ay, az ;
int16_t gx, gy, gz ;



void setup()
{
  //Baudrate Serialmonitor
   Serial.begin(9600);

  // Eingänge
   pinMode(ch1, INPUT);
   pinMode(ch3, INPUT);
   pinMode(ch4, INPUT);

  // Servobin bezeichnen
   servoBase.attach(servoBasePin); 
   servoTeaArm.attach(servoTeaArmPin);

// MPU6050 überprüfen
Serial.println  ( "Initializing the sensor" );
sensor.initialize ( );
Serial.println (sensor.testConnection ( ) ? "Successfully Connected" : "Connection failed");
delay (1000);
Serial.println ( "Taking Values from the sensor" );
delay (500);

}


void loop()
{
  int ch3Pitch = pulseIn(ch3,HIGH,25000);
  int ch1Roll = pulseIn(ch1,HIGH,25000);
  int Auto = pulseIn(ch4,HIGH,25000);

  // Ausgabe Daten an den Serialmonitor
  Serial.print("   Schild heben_senken : ");
  Serial.print( ch3Pitch );
  Serial.print("   Schild tillt: ");
  Serial.print( ch1Roll );
  Serial.print("   Winkel Schild_auto: ");
  Serial.print ( ay );
  Serial.print (" Auto On_OFF : ");
  Serial.println (Auto);
 
// Umrechnung der Werte für die Servos
  servoValPitch = map(ch3Pitch, 1000, 2000, 0, 180);   
  servoValRoll = map(ch1Roll, 1000, 2000, 0, 180);

// Umrechnung der Werte für den MPU6050
  sensor.getMotion6 (&ax, &ay, &az, &gx, &gy, &gz);
  ay = map (ay, -17000, 17000, 0, 180) ;

// Befehle an die Servos schicken

  servoBase.write(servoValPitch);

  //servoTeaArm.write(servoValRoll);
  servoTeaArm.write(ay);

   delay(100);
}

Würde das so funktionieren?

INI:

if (Auto >1700) {servoTeaArm.write(ay);}
if (Auto <1700) {servoTeaArm.write(servoValRoll);}

markusstn · 20.07.2024

Es langt nur der if Befehl

Hier mal der Code

Code:

//Automatische Schildsteuerung mit Arduino Nano und MPU6050
// von Markus Steinbach
// 19.07.2024

// RC Reciver Pinbelegung:
// Schild auf_ab = D6
// Schild tilt   = D10
// Schalter Auto = D9

// Servo Pin Belegung:
// Servo Schild auf_ab = D3
// Servo Schild tilt   = D5

// MPU6050 Pinbelegung:
// VCC = 5V
// GND = GND
// SCL = A5
// SDA = A4

// Spannungsversorung vom Reciver:
// + = VIN
// - = GND

// Bei Verbindung mit dem PC über USB kabel (Serial Monitor):
// - = GND
// !!!! Achtung nur den minus miteinander verbinden, da 2 unterschiedliche Spannungen !!!!

#include <Servo.h>
#include <Wire.h>
#include <MPU6050.h>

int ch4=9; // Schalter auf Funke Automatik ein_aus
int ch1=6; // Servo Schild auf_ab
int ch3=10;// Servo Schild Neigung einstellen

const int servoBasePin = 3;   //Servo Schild auf_ab
const int servoTeaArmPin = 5; //Servo Schild Neigung einstellen

Servo servoBase;
Servo servoTeaArm;
int servoValRoll;
int servoValPitch;

MPU6050 sensor ;
int16_t ax, ay, az ;
int16_t gx, gy, gz ;



void setup()
{
  Wire.begin ( );

  //Baudrate Serialmonitor
   Serial.begin(9600);

  // Eingänge
   pinMode(ch1, INPUT);
   pinMode(ch3, INPUT);
   pinMode(ch4, INPUT);

  // Servobin bezeichnen
   servoBase.attach(servoBasePin);
   servoTeaArm.attach(servoTeaArmPin);

  // MPU6050 überprüfen
  Serial.println  ( "Initializing the sensor" );
  sensor.initialize ( );
  Serial.println (sensor.testConnection ( ) ? "Successfully Connected" : "Connection failed");
  delay (1000);
  Serial.println ( "Taking Values from the sensor" );
  delay (1000);

}


void loop()
{
  int ch3Pitch = pulseIn(ch3,HIGH,25000);
  int ch1Roll = pulseIn(ch1,HIGH,25000);
  int Auto = pulseIn(ch4,HIGH,25000);

  // Ausgabe Daten an den Serialmonitor
  Serial.print("   Schild heben_senken : ");
  Serial.print( ch3Pitch );
  Serial.print("   Schild tillt: ");
  Serial.print( ch1Roll );
  Serial.print("   Winkel Schild_auto: ");
  Serial.print ( ay );
  Serial.print (" Auto On_OFF : ");
  Serial.println (Auto);
 
// Umrechnung der Werte für die Servos
  servoValPitch = map(ch3Pitch, 1000, 2000, 0, 180); 
  servoValRoll = map(ch1Roll, 1000, 2000, 180, 0);

// Umrechnung der Werte für den MPU6050
  sensor.getMotion6 (&ax, &ay, &az, &gx, &gy, &gz);
  ay = map (ay, -17000, 17000, 180, 0) ;

// Befehle an die Servos schicken

  //servoBase.write(servoValPitch);
  if (Auto >=1700) {servoTeaArm.write(ay);}
  if (Auto <=1700) {servoTeaArm.write(servoValRoll);}


  //servoTeaArm.write(servoValRoll);
  //servoTeaArm.write(ay);

   delay(100);
}

Hab nur ein Problem jetzt, das Signal vom RC Reciver springt dauert von 0 auf 1498 und zurück ( auf_ab Eingang).
Beim anderen hab ich ein leichtes zucken am Servo, hier ändert sich der Wert immer von 1497-1501.
Ich find den Fehler nicht an was das liegen kann.
Grüsse Markus

gismow · 20.07.2024

Moin, moin….

Im Code sehe ich drei mal hintereinander den pulseIn Befehl, der ist blockierend und wartet immer auf ein komplettes Signal.

Hier solltest du auf Interrupts umstellen, dann läuft dass smooth.

Ein pegeln zwischen 1497 und 1501 ist bei PPM Signalen normal, wenn du z.B. SBUS verwenden würdest, dann hättest du es nicht.

Um dieses Pendeln auszugleichen, merke ich mir immer die letzten 4 Signale und bilde einen Mittelwert daraus.

Wenn ich nachher mal am Rechner sitze versuche ich deinen Code mal auf Interrupts umzustellen.

gismow · 20.07.2024

Moin, moin...

Ging doch etwas schneller als ich dachte. Der hier gepostete Code ist blind entstanden, ich habe die MPU6050 Library nicht.

Ich hoffe es hilft. Beim Schalter habe ich mal einen "Schutzbereich" von 50 eingebaut, dass ein Servowert um die 1700 nicht zu einem ständigen Wechsel führt.

Den Wert des Sensors habe ich direkt in Microsecunden umrechnen lassen, das mal die Library auch mit deinen Gradzahlen und so ist es etwas feiner.

Bei Fragen einfach melden.

C++:

//Automatische Schildsteuerung mit Arduino Nano und MPU6050
// von Markus Steinbach
// 19.07.2024

// RC Reciver Pinbelegung:
// Schild auf_ab = D6
// Schild tilt   = D10
// Schalter Auto = D9

// Servo Pin Belegung:
// Servo Schild auf_ab = D3
// Servo Schild tilt   = D5

// MPU6050 Pinbelegung:
// VCC = 5V
// GND = GND
// SCL = A5
// SDA = A4

// Spannungsversorung vom Reciver:
// + = VIN
// - = GND

// Bei Verbindung mit dem PC über USB kabel (Serial Monitor):
// - = GND
// !!!! Achtung nur den minus miteinander verbinden, da 2 unterschiedliche Spannungen !!!!

#include <Servo.h>
#include <Wire.h>
#include <MPU6050.h>

int ch4 = 9; // Schalter auf Funke Automatik ein_aus
int ch1 = 6; // Servo Schild auf_ab
int ch3 = 10;// Servo Schild Neigung einstellen

const int servoBasePin = 3;   //Servo Schild auf_ab
const int servoTeaArmPin = 5; //Servo Schild Neigung einstellen

Servo servoBase;
Servo servoTeaArm;
int servoValRoll;
int servoValPitch;

MPU6050 sensor;
int16_t ax, ay, az;
int16_t gx, gy, gz;

//    Hier speichern die Interrupts die erfassten Werte
volatile    uint16_t        InterruptSwitch = 1500;
volatile    uint16_t        InterruptUpDown = 1500;
volatile    uint16_t        InterruptInclination = 1500;

//    Dieser Block wird benoetigt um das Servosignal auszuwerten und etwas zu glaetten
uint32_t    LastSwitchChannelChanged = 0;
uint16_t    ReceivedValuesSwitch[4] = { 1500, 1500, 1500, 1500 };
uint16_t    ReceivedValuesSumSwitch = 6000;
uint8_t        ReceivedValuesIndexSwitch = 0;
uint16_t    ReceivedValueSwitch = 1500;

uint32_t    LastUpDownChannelChanged = 0;
uint16_t    ReceivedValuesUpDown[4] = { 1500, 1500, 1500, 1500 };
uint16_t    ReceivedValuesSumUpDown = 6000;
uint8_t        ReceivedValuesIndexUpDown = 0;
uint16_t    ReceivedValueUpDown = 1500;

uint32_t    LastInclinationChannelChanged = 0;
uint16_t    ReceivedValuesInclination[4] = { 1500, 1500, 1500, 1500 };
uint16_t    ReceivedValuesSumInclination = 6000;
uint8_t        ReceivedValuesIndexInclination = 0;
uint16_t    ReceivedValueInclination = 1500;

//    Diese Methode nutzt die aus den Interrupts ermittelten Werte und glaettet diese etwas (Mittelwert aus den letzten 4 Werten)
void NormalizeServoValues()
{
    ReceivedValuesSumSwitch -= ReceivedValuesSwitch[ReceivedValuesIndexSwitch];
    ReceivedValuesSwitch[ReceivedValuesIndexSwitch] = InterruptSwitch;
    ReceivedValuesSumSwitch += InterruptSwitch;
    ReceivedValuesIndexSwitch = ((ReceivedValuesIndexSwitch + 1) & 0x03);    //    Index erhoehen und ggf. von 4 auf 0 springen

    ReceivedValueSwitch = (ReceivedValuesSumUpDown >> 2);    //    durch 4 teilen

    ReceivedValuesSumUpDown -= ReceivedValuesUpDown[ReceivedValuesIndexUpDown];
    ReceivedValuesUpDown[ReceivedValuesIndexUpDown] = InterruptUpDown;
    ReceivedValuesSumUpDown += InterruptUpDown;
    ReceivedValuesIndexUpDown = ((ReceivedValuesIndexUpDown + 1) & 0x03);    //    Index erhoehen und ggf. von 4 auf 0 springen

    ReceivedValueUpDown = (ReceivedValuesSumUpDown >> 2);    //    durch 4 teilen

    ReceivedValuesSumInclination -= ReceivedValuesInclination[ReceivedValuesIndexInclination];
    ReceivedValuesInclination[ReceivedValuesIndexInclination] = InterruptInclination;
    ReceivedValuesSumInclination += InterruptInclination;
    ReceivedValuesIndexInclination = ((ReceivedValuesIndexInclination + 1) & 0x03);    //    Index erhoehen und ggf. von 4 auf 0 springen

    ReceivedValueInclination = (ReceivedValuesSumInclination >> 2);    //    durch 4 teilen
}

volatile    uint8_t            LastInterruptState0 = PINB;
volatile    uint8_t            LastInterruptState2 = PIND;

void InterruptSwitchPosition(uint32_t micSec)
{
    uint16_t    nDifference = (uint16_t)(micSec - LastSwitchChannelChanged);

    LastSwitchChannelChanged = micSec;

    if ((nDifference > 750) && (nDifference < 2250))
        InterruptSwitch = nDifference;
}

void InterruptInclinationPosition(uint32_t micSec)
{
    uint16_t    nDifference = (uint16_t)(micSec - LastInclinationChannelChanged);

    LastInclinationChannelChanged = micSec;

    if ((nDifference > 750) && (nDifference < 2250))
        InterruptInclination = nDifference;
}

ISR(PCINT0_vect)
{

    uint8_t    PinState = PINB;
    uint8_t    PinChanges = PinState ^ LastInterruptState0;
    LastInterruptState0 = PinState;

    uint32_t micSec = micros();

    if (PinChanges & (1 << PCINT1))
        InterruptSwitchPosition(micSec);

    if (PinChanges & (1 << PCINT2))
        InterruptInclinationPosition(micSec);
}

void InterruptUpDownPosition(uint32_t micSec)
{
    uint16_t    nDifference = (uint16_t)(micSec - LastUpDownChannelChanged);

    LastUpDownChannelChanged = micSec;

    if ((nDifference > 750) && (nDifference < 2250))
        InterruptUpDown = nDifference;
}

ISR(PCINT2_vect)
{
    uint8_t    PinState = PIND;
    uint8_t    PinChanges = PinState ^ LastInterruptState2;
    LastInterruptState2 = PinState;

    uint32_t micSec = micros();

    if (PinChanges & (1 << PCINT22))
        InterruptUpDownPosition(micSec);
}


void setup()
{
    Wire.begin();

    //Baudrate Serialmonitor
    Serial.begin(9600);

    // Eingänge
    pinMode(ch1, INPUT);
    pinMode(ch3, INPUT);
    pinMode(ch4, INPUT);

    // Servobin bezeichnen
    servoBase.attach(servoBasePin);
    servoTeaArm.attach(servoTeaArmPin);

    // MPU6050 überprüfen
    Serial.println("Initializing the sensor");
    sensor.initialize();
    Serial.println(sensor.testConnection() ? "Successfully Connected" : "Connection failed");
    delay(1000);
    Serial.println("Taking Values from the sensor");
    delay(1000);

    //    Die Interrupts fuer die benoetigten Kanaele aktivieren
    //    Hier findest du die benoetigten Konstanten
    //    https://wolles-elektronikkiste.de/en/interrupts-part-2-pin-change-interrupts

    uint8_t msk0 = (1 << PCINT1)    //    Kanal 9
        | (1 << PCINT2);            //    Kanal 10
    uint8_t msk2 = (1 << PCINT22);    //    Kanal 6


    PCICR |= ((1 << PCIE0) | (1 << PCIE2));
    PCMSK0 |= msk0;        //    Steuert die Methode ISR(PCINT0_vect) an
    PCMSK2 |= msk2;        //    Steuert die Methode ISR(PCINT2_vect) an

    sei();
}


void loop()
{
    //    Die per interrupt gelesenen Werte auswerten
    NormalizeServoValues();

    // Ausgabe Daten an den Serialmonitor
    Serial.print("   Schild heben_senken : ");
    Serial.print(InterruptInclination);
    Serial.print("   Schild tillt: ");
    Serial.print(InterruptUpDown);
    Serial.print("   Winkel Schild_auto: ");
    Serial.print(ay);
    Serial.print(" Auto On_OFF : ");
    Serial.println(InterruptSwitch);


    // Umrechnung der Werte für die Servos
    // Umrechnung der Werte für den MPU6050
    sensor.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    ay = map(ay, -17000, 17000, 2000, 1000);

    // Befehle an die Servos schicken
    if (ReceivedValueSwitch > 1600)
        servoTeaArm.writeMilliseconds(ay);

    if (ReceivedValueSwitch < 1400)
        servoTeaArm.writeMilliseconds(ReceivedValueUpDown);

    servoBase.write(ReceivedValueInclination);

    delay(15);
}

markusstn · 20.07.2024

Danke Peter für deine Hilfe

Bei dem Schreiben der Servos müsste es da nicht ________.writeMicroseconds heißen?
Der Code meldet mir immer einen Fehler beim hochladen auf den arduino

markusstn · 20.07.2024

Hi Peter,
der Schalter auf der Funke ist ein 3Stufen Schalter.
also oben=2000, Mitte =1500, unten 1000
Schalter mitte steuern über Knüppel
Schalter oben Automatik über MPU 6050 Sensor
Frage?
Wieso hast Du bei ay map den Wert von 0/180 auf 2000/1000 geändert das versteh ich nicht so ganz.

gismow · 20.07.2024

Moin Markus,

die Servo Library wandelt deine Gradangaben eh in den Bereich von 1000 bis 2000 um, dann kannst du das auch direkt tun und sparst ein paar CPU Ticks

markusstn · 20.07.2024

Hi Peter, Danke.
Hab den Code noch etwas ändern müssen, jetzt Funktioniert es schon mal

Code:

// Befehle an die Servos schicken
    if (InterruptSwitch > 1800)
        servoTeaArm.writeMicroseconds(ay);

    if (InterruptSwitch < 1600)
        servoTeaArm.writeMicroseconds(ReceivedValueUpDown);

    servoBase.write(ReceivedValueInclination);

wenn ich jetzt auf Automatik schalte und die Steuerung über den MPU 6050 geht hab ich wieder das zucken am Servo,
wie kann ich deinen Filter auf das MPU Signal anwenden ( Gyro Signal ay)
dein Filteraufbau versteh ich nicht das ist zu hoch für mich
wo genau wird der Wert vom Empfänger gespeichert ?
soviel ich jetzt verstanden hab nimmst Du den Wert der letzten 4 Messungen und ermittelst daraus den Mittelwert, danach hab ich den Faden verloren

gismow · 20.07.2024

Moin Markus,

der Wert des Sensors kann man natürlich auch über einen solchen Puffer laufen lassen. Aktuell nutze ich ja die letzten 4 Werte. Man kann aber auch mehr nehmen, dann wird der Wert noch mehr geglättet, aber mit mehr Werten hat man aber auch eine größere Verzögerung.

Ich versuche mal den Puffer zu erklären.
Am Anfang steht der Index auf 0, sprich auf dem ersten Wert. Die 4 Werte sind mit 1500 vorbelegt, die Summe mit 6000. Kommt nun ein Wert rein, dann tausche ich den Wert des aktuellen Index aus. Ich ziehe von der Summe erst den aktuell gespeicherten Wert ab, ersetze ihn dann durch den neuen und addiere diesen dann auch wieder auf die Summe drauf. Danach schiebe ich den Index eine Position weiter. Am Ende macht er dann wieder einen Sprung auf die 0. Ich könnte auch nur den Wert austauschen und dann alle 4 Werte addieren, aber mit dieser Methode habe ich statt 3 Additionen nur 2 Operationen.
Anschließend teile ich die Summe durch 4, es geschieht durch ein Bit-Shifting ( >> 2 ), 2 Stellen nach rechts, das entspricht einem Teilen durch 4.

Das gleiche kannst du natürlich auch mit dem Wert des Sensors machen.

markusstn · 21.07.2024

Hi Peter,
danke für die Erklärung
Also brauche ich

Code:

//    Hier speichern die Interrupts die erfassten Werte
volatile    uint16_t        InterruptSwitch = 1500;

Code:

//    Dieser Block wird benoetigt um das Servosignal auszuwerten und etwas zu glaetten
uint32_t    LastSwitchChannelChanged = 0;
uint16_t    ReceivedValuesSwitch[4] = { 1500, 1500, 1500, 1500 };
uint16_t    ReceivedValuesSumSwitch = 6000;
uint8_t        ReceivedValuesIndexSwitch = 0;
uint16_t    ReceivedValueSwitch = 1500;

Code:

//    Diese Methode nutzt die aus den Interrupts ermittelten Werte und glaettet diese etwas (Mittelwert aus den letzten 4 Werten)
void NormalizeServoValues()
{
    ReceivedValuesSumSwitch -= ReceivedValuesSwitch[ReceivedValuesIndexSwitch];
    ReceivedValuesSwitch[ReceivedValuesIndexSwitch] = InterruptSwitch;
    ReceivedValuesSumSwitch += InterruptSwitch;
    ReceivedValuesIndexSwitch = ((ReceivedValuesIndexSwitch + 1) & 0x03);    //    Index erhoehen und ggf. von 4 auf 0 springen

    ReceivedValueSwitch = (ReceivedValuesSumUpDown >> 2);    //    durch 4 teilen

Code:

void InterruptSwitchPosition(uint32_t micSec)
{
    uint16_t    nDifference = (uint16_t)(micSec - LastSwitchChannelChanged);

    LastSwitchChannelChanged = micSec;

    if ((nDifference > 750) && (nDifference < 2250))
        InterruptSwitch = nDifference;
}

Hier ist mir aufgefallen das etwas anders ist als bei den anderen beiden, müsste ( ReceivedValuesSumSwitch ) sein oder?

Code:

//    Diese Methode nutzt die aus den Interrupts ermittelten Werte und glaettet diese etwas (Mittelwert aus den letzten 4 Werten)
void NormalizeServoValues()
{
    ReceivedValuesSumSwitch -= ReceivedValuesSwitch[ReceivedValuesIndexSwitch];
    ReceivedValuesSwitch[ReceivedValuesIndexSwitch] = InterruptSwitch;
    ReceivedValuesSumSwitch += InterruptSwitch;
    ReceivedValuesIndexSwitch = ((ReceivedValuesIndexSwitch + 1) & 0x03);    //    Index erhoehen und ggf. von 4 auf 0 springen

    ReceivedValueSwitch = (ReceivedValuesSumUpDown >> 2);    //    durch 4 teilen

Was ich noch nicht so ganz verstanden hab ist der ganze Ablauf

Wo wird der eingelesene Wert eingetragen, woher weiß der Arduino welches Signal er lesen muss

Bei dem MPU 6050 ist alles was ich brauche das hier

Code:

MPU6050 sensor;
int16_t ax, ay, az;
int16_t gx, gy, gz;

Code:

 // Umrechnung der Werte für den MPU6050
    sensor.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    ay = map(ay, -17000, 17000, 2000, 1000);

hier benutze ich dann das ( ay ) das einen Wert von -17000 bis 17000 haben kann und wandle es entsprechend um und schicke es zum Servo

den Rest macht die MPU6050 library und die Servo library

gismow · 21.07.2024

Moin Markus,

um den Sensorwert zu glätten, braucht du folgende Erweiterungen, wie von dir beschrieben:

Die notwendigen Variablen:

C++:

uint16_t    ReadSensorValue = 1500;    //    in diese Variable wird der vom Sensor ermittelte und in Microsekunden umgerechnete Wert eingetragen.
                                    //    Er wird von NormalizeServoValues() ausgelesen und verrechnet.
//    Wenn du über 8 Werte glaetten moechtest, dann musst du die ersten zwei Zeilen durch diese ersetzen
//    uint16_t    ReceivedValuesSensor[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 };
//    uint16_t    ReceivedValuesSumSensor = 12000;
uint16_t    ReceivedValuesSensor[4] = { 1500, 1500, 1500, 1500 };
uint16_t    ReceivedValuesSumSensor = 6000;
uint8_t        ReceivedValuesIndexSensor = 0;
uint16_t    ReceivedValueSensor = 1500;    //    Hier steht dann der geglaettete Wert drin, damit kann dann ein Srevo angefahren werden.

NormalizeServoValues() muss erweitert werden, um den Wert zu glätten

Code:

    //    Den Wert des Sensors glaetten
    ReceivedValuesSumSensor -= ReceivedValuesSensor[ReceivedValuesIndexSensor];    //    Alten Wert aus der Summe entfernen
    ReceivedValuesSensor[ReceivedValuesIndexSensor] = ReadSensorValue;            //    Neuen Wert im Array setzen (damit er spaeter wieder aus der Summe entfernt werden kann)
    ReceivedValuesSumSensor += ReadSensorValue;                                    //    Neuen Wert zur Summe addieren
    ReceivedValuesIndexSensor = ((ReceivedValuesIndexSensor + 1) & 0x03);        //    Index erhoehen und ggf. von 4 auf 0 springen
    ReceivedValueSensor = (ReceivedValuesSumSensor >> 2);    //    durch 4 teilen
    //    Bei einer Glaettung ueber 8 Werte, kommentiere die oberen beiden aus und nutze die folgenden beiden Zeilen
    //    ReceivedValuesIndexSensor = ((ReceivedValuesIndexSensor + 1) & 0x07);        //    Index erhoehen und ggf. von 4 auf 0 springen
    //    ReceivedValueSensor = (ReceivedValuesSumSensor >> 3);    //    durch 8 teilen

Dann muss das Auslesen des Sensors vor den Aufruf von NormalizeServoValues() verlagert und statt dem Wert ay der Wert ReceivedValueSensor verwendet werden.

Code:

void loop()
{
    // Umrechnung der Werte für den MPU6050
    sensor.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    ReadSensorValue = map(ay, -17000, 17000, 2000, 1000);

    //    Die per interrupt gelesenen Werte auswerten
    NormalizeServoValues();

    // Ausgabe Daten an den Serialmonitor
    Serial.print("   Schild heben_senken : ");
    Serial.print(InterruptInclination);
    Serial.print("   Schild tillt: ");
    Serial.print(InterruptUpDown);
    Serial.print("   Winkel Schild_auto: ");
    Serial.print(ReceivedValueSensor);
    Serial.print(" Auto On_OFF : ");
    Serial.println(InterruptSwitch);



    // Befehle an die Servos schicken
    if (ReceivedValueSwitch > 1700)
        servoTeaArm.writeMicroseconds(ReceivedValueSensor);

    if (ReceivedValueSwitch < 1650)
        servoTeaArm.writeMicroseconds(ReceivedValueUpDown);

    servoBase.write(ReceivedValueInclination);

    delay(15);
}

Du hast meinen Copy&Paste&Rename Fehler entdeckt...

Zum Ablauf....
Bisher war dein Ablauf linear, du hast nacheinander die einzelnen kanäle ausgewertet und dann verarbeitet.

Nun hast du eine Parallelverarbeitung.

Im setup() findest du unten etwas Lowlevel Kram, damit sage ich dem Arduino, dass bei einer Signaländerung an einem der Kanäle ein Interrupt ausgelöst wird. In einem solchen Fall wird das Hauptprogramm angehalten und eine der ISR() Methoden aufgerufen. Aus den PINB bzw. PIND Variablen kann ich erkennen, welcher der Kanäle eine Änderung erfahren hat und rufe dann die entsprechende Methode auf.
In diesen Methode schaue ich, wann die letzte Änderung erfolgte. Wenn sie mehr als 2300 Microsekunden her war, dann befinden wir uns in den LOW Bereich des Signals. Ist sie < 2300, dann ist es der HIGH Bereich und den wollen wir ja haben. Der Wert wird dann in die Variable beginnend mit 'Interrupt' geschrieben. Wenn die Methode fertig ist, dann läuft das Hauptprogramm (loop() ) weiter. Deswegen sind die 'Interrupt...' Variablen auch als 'volatile' gekennzeichnet, damit der Interrupt und das Hauptzprogramm darauf zugreifen kann.

Die Funktion NormalizeServoValues() nimmt dann die 'Interrupt...' Variablen, rechnet sie in die Glättung ein und liefert das Ergebnis dann in den 'ReceivedValues...' aus. Die kann dann zur Steuerung der Servos verwendet werden.

gismow · 21.07.2024

Hier nochmal der komplette Code:

C++:

//Automatische Schildsteuerung mit Arduino Nano und MPU6050
// von Markus Steinbach
// 19.07.2024

// RC Reciver Pinbelegung:
// Schild auf_ab = D6
// Schild tilt   = D10
// Schalter Auto = D9

// Servo Pin Belegung:
// Servo Schild auf_ab = D3
// Servo Schild tilt   = D5

// MPU6050 Pinbelegung:
// VCC = 5V
// GND = GND
// SCL = A5
// SDA = A4

// Spannungsversorung vom Reciver:
// + = VIN
// - = GND

// Bei Verbindung mit dem PC über USB kabel (Serial Monitor):
// - = GND
// !!!! Achtung nur den minus miteinander verbinden, da 2 unterschiedliche Spannungen !!!!

#include <Servo.h>
#include <Wire.h>
#include <MPU6050.h>

int ch4 = 9; // Schalter auf Funke Automatik ein_aus
int ch1 = 6; // Servo Schild auf_ab
int ch3 = 10;// Servo Schild Neigung einstellen

const int servoBasePin = 3;   //Servo Schild auf_ab
const int servoTeaArmPin = 5; //Servo Schild Neigung einstellen

Servo servoBase;
Servo servoTeaArm;
int servoValRoll;
int servoValPitch;

MPU6050 sensor;
int16_t ax, ay, az;
int16_t gx, gy, gz;

//    Hier speichern die Interrupts die erfassten Werte
volatile    uint16_t        InterruptSwitch = 1500;
volatile    uint16_t        InterruptUpDown = 1500;
volatile    uint16_t        InterruptInclination = 1500;

//    Dieser Block wird benoetigt um das Servosignal auszuwerten und etwas zu glaetten
uint32_t    LastSwitchChannelChanged = 0;
uint16_t    ReceivedValuesSwitch[4] = { 1500, 1500, 1500, 1500 };
uint16_t    ReceivedValuesSumSwitch = 6000;
uint8_t        ReceivedValuesIndexSwitch = 0;
uint16_t    ReceivedValueSwitch = 1500;

uint32_t    LastUpDownChannelChanged = 0;
uint16_t    ReceivedValuesUpDown[4] = { 1500, 1500, 1500, 1500 };
uint16_t    ReceivedValuesSumUpDown = 6000;
uint8_t        ReceivedValuesIndexUpDown = 0;
uint16_t    ReceivedValueUpDown = 1500;

uint32_t    LastInclinationChannelChanged = 0;
uint16_t    ReceivedValuesInclination[4] = { 1500, 1500, 1500, 1500 };
uint16_t    ReceivedValuesSumInclination = 6000;
uint8_t        ReceivedValuesIndexInclination = 0;
uint16_t    ReceivedValueInclination = 1500;

uint16_t    ReadSensorValue = 1500;    //    in diese Variable wird der vom Sensor ermittelte und in Microsekunden umgerechnete Wert eingetragen.
                                    //    Er wird von NormalizeServoValues() ausgelesen und verrechnet.
//    Wenn du über 8 Werte glaetten moechtest, dann musst du die ersten zwei Zeilen durch diese ersetzen
//    uint16_t    ReceivedValuesSensor[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 };
//    uint16_t    ReceivedValuesSumSensor = 12000;
uint16_t    ReceivedValuesSensor[4] = { 1500, 1500, 1500, 1500 };
uint16_t    ReceivedValuesSumSensor = 6000;
uint8_t        ReceivedValuesIndexSensor = 0;
uint16_t    ReceivedValueSensor = 1500;    //    Hier steht dann der geglaettete Wert drin, damit kann dann ein Srevo angefahren werden.


//    Diese Methode nutzt die aus den Interrupts ermittelten Werte und glaettet diese etwas (Mittelwert aus den letzten 4 Werten)
void NormalizeServoValues()
{
    ReceivedValuesSumSwitch -= ReceivedValuesSwitch[ReceivedValuesIndexSwitch];
    ReceivedValuesSwitch[ReceivedValuesIndexSwitch] = InterruptSwitch;
    ReceivedValuesSumSwitch += InterruptSwitch;
    ReceivedValuesIndexSwitch = ((ReceivedValuesIndexSwitch + 1) & 0x03);    //    Index erhoehen und ggf. von 4 auf 0 springen

    ReceivedValueSwitch = (ReceivedValuesSumSwitch >> 2);    //    durch 4 teilen

    ReceivedValuesSumUpDown -= ReceivedValuesUpDown[ReceivedValuesIndexUpDown];
    ReceivedValuesUpDown[ReceivedValuesIndexUpDown] = InterruptUpDown;
    ReceivedValuesSumUpDown += InterruptUpDown;
    ReceivedValuesIndexUpDown = ((ReceivedValuesIndexUpDown + 1) & 0x03);    //    Index erhoehen und ggf. von 4 auf 0 springen

    ReceivedValueUpDown = (ReceivedValuesSumUpDown >> 2);    //    durch 4 teilen

    ReceivedValuesSumInclination -= ReceivedValuesInclination[ReceivedValuesIndexInclination];
    ReceivedValuesInclination[ReceivedValuesIndexInclination] = InterruptInclination;
    ReceivedValuesSumInclination += InterruptInclination;
    ReceivedValuesIndexInclination = ((ReceivedValuesIndexInclination + 1) & 0x03);    //    Index erhoehen und ggf. von 4 auf 0 springen

    ReceivedValueInclination = (ReceivedValuesSumInclination >> 2);    //    durch 4 teilen

    //    Den Wert des Sensors glaetten
    ReceivedValuesSumSensor -= ReceivedValuesSensor[ReceivedValuesIndexSensor];    //    Alten Wert aus der Summe entfernen
    ReceivedValuesSensor[ReceivedValuesIndexSensor] = ReadSensorValue;            //    Neuen Wert im Array setzen (damit er spaeter wieder aus der Summe entfernt werden kann)
    ReceivedValuesSumSensor += ReadSensorValue;                                    //    Neuen Wert zur Summe addieren
    ReceivedValuesIndexSensor = ((ReceivedValuesIndexSensor + 1) & 0x03);        //    Index erhoehen und ggf. von 4 auf 0 springen
    ReceivedValueSensor = (ReceivedValuesSumSensor >> 2);    //    durch 4 teilen
    //    Bei einer Glaettung ueber 8 Werte, kommentiere die oberen beiden aus und nutze die folgenden beiden Zeilen
    //    ReceivedValuesIndexSensor = ((ReceivedValuesIndexSensor + 1) & 0x07);        //    Index erhoehen und ggf. von 4 auf 0 springen
    //    ReceivedValueSensor = (ReceivedValuesSumSensor >> 3);    //    durch 8 teilen
}

volatile    uint8_t            LastInterruptState0 = PINB;
volatile    uint8_t            LastInterruptState2 = PIND;

void InterruptSwitchPosition(uint32_t micSec)
{
    uint16_t    nDifference = (uint16_t)(micSec - LastSwitchChannelChanged);

    LastSwitchChannelChanged = micSec;

    if ((nDifference > 750) && (nDifference < 2250))
        InterruptSwitch = nDifference;
}

void InterruptInclinationPosition(uint32_t micSec)
{
    uint16_t    nDifference = (uint16_t)(micSec - LastInclinationChannelChanged);

    LastInclinationChannelChanged = micSec;

    if ((nDifference > 750) && (nDifference < 2250))
        InterruptInclination = nDifference;
}

ISR(PCINT0_vect)
{

    uint8_t    PinState = PINB;
    uint8_t    PinChanges = PinState ^ LastInterruptState0;
    LastInterruptState0 = PinState;

    uint32_t micSec = micros();

    if (PinChanges & (1 << PCINT1))
        InterruptSwitchPosition(micSec);

    if (PinChanges & (1 << PCINT2))
        InterruptInclinationPosition(micSec);
}

void InterruptUpDownPosition(uint32_t micSec)
{
    uint16_t    nDifference = (uint16_t)(micSec - LastUpDownChannelChanged);

    LastUpDownChannelChanged = micSec;

    if ((nDifference > 750) && (nDifference < 2250))
        InterruptUpDown = nDifference;
}

ISR(PCINT2_vect)
{
    uint8_t    PinState = PIND;
    uint8_t    PinChanges = PinState ^ LastInterruptState2;
    LastInterruptState2 = PinState;

    uint32_t micSec = micros();

    if (PinChanges & (1 << PCINT22))
        InterruptUpDownPosition(micSec);
}


void setup()
{
    Wire.begin();

    //Baudrate Serialmonitor
    Serial.begin(9600);

    // Eingänge
    pinMode(ch1, INPUT);
    pinMode(ch3, INPUT);
    pinMode(ch4, INPUT);

    // Servobin bezeichnen
    servoBase.attach(servoBasePin);
    servoTeaArm.attach(servoTeaArmPin);

    // MPU6050 überprüfen
    Serial.println("Initializing the sensor");
    sensor.initialize();
    Serial.println(sensor.testConnection() ? "Successfully Connected" : "Connection failed");
    delay(1000);
    Serial.println("Taking Values from the sensor");
    delay(1000);

    //    Die Interrupts fuer die benoetigten Kanaele aktivieren
    //    Hier findest du die benoetigten Konstanten
    //    https://wolles-elektronikkiste.de/en/interrupts-part-2-pin-change-interrupts

    uint8_t msk0 = (1 << PCINT1)    //    Kanal 9
        | (1 << PCINT2);            //    Kanal 10
    uint8_t msk2 = (1 << PCINT22);    //    Kanal 6


    PCICR |= ((1 << PCIE0) | (1 << PCIE2));
    PCMSK0 |= msk0;        //    Steuert die Methode ISR(PCINT0_vect) an
    PCMSK2 |= msk2;        //    Steuert die Methode ISR(PCINT2_vect) an

    sei();
}


void loop()
{
    // Umrechnung der Werte für den MPU6050
    sensor.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    ReadSensorValue = map(ay, -17000, 17000, 2000, 1000);

    //    Die per interrupt gelesenen Werte auswerten
    NormalizeServoValues();

    // Ausgabe Daten an den Serialmonitor
    Serial.print("   Schild heben_senken : ");
    Serial.print(InterruptInclination);
    Serial.print("   Schild tillt: ");
    Serial.print(InterruptUpDown);
    Serial.print("   Winkel Schild_auto: ");
    Serial.print(ReceivedValueSensor);
    Serial.print(" Auto On_OFF : ");
    Serial.println(InterruptSwitch);



    // Befehle an die Servos schicken
    if (ReceivedValueSwitch > 1700)
        servoTeaArm.writeMicroseconds(ReceivedValueSensor);

    if (ReceivedValueSwitch < 1650)
        servoTeaArm.writeMicroseconds(ReceivedValueUpDown);

    servoBase.write(ReceivedValueInclination);

    delay(15);
}

markusstn · 21.07.2024

Peter,

hab´s noch etwas angepasst.

Code:

//Automatische Schildsteuerung mit Arduino Nano und MPU6050
// von Markus Steinbach
// 19.07.2024

// RC Reciver Pinbelegung:
// Schild auf_ab = D6
// Schild tilt   = D10
// Schalter Auto = D9

// Servo Pin Belegung:
// Servo Schild auf_ab = D3
// Servo Schild tilt   = D5

// MPU6050 Pinbelegung:
// VCC = 5V
// GND = GND
// SCL = A5
// SDA = A4

// Spannungsversorung vom Reciver:
// + = VIN
// - = GND

// Bei Verbindung mit dem PC über USB kabel (Serial Monitor):
// - = GND
// !!!! Achtung nur den minus miteinander verbinden, da 2 unterschiedliche Spannungen !!!!

#include <Servo.h>
#include <Wire.h>
#include <MPU6050.h>

int ch4 = 9; // Schalter auf Funke Automatik ein_aus
int ch1 = 6; // Servo Schild auf_ab
int ch3 = 10;// Servo Schild Neigung einstellen

const int servoBasePin = 3;   //Servo Schild auf_ab
const int servoTeaArmPin = 5; //Servo Schild Neigung einstellen

Servo servoBase;
Servo servoTeaArm;
int servoValRoll;
int servoValPitch;

MPU6050 sensor;
int16_t ax, ay, az;
int16_t gx, gy, gz;

//    Hier speichern die Interrupts die erfassten Werte
volatile    uint16_t        InterruptSwitch = 1500;
volatile    uint16_t        InterruptUpDown = 1500;
volatile    uint16_t        InterruptInclination = 1500;

//    Dieser Block wird benoetigt um das Servosignal auszuwerten und etwas zu glaetten
uint32_t    LastSwitchChannelChanged = 0;
uint16_t    ReceivedValuesSwitch[4] = { 1500, 1500, 1500, 1500 };
uint16_t    ReceivedValuesSumSwitch = 6000;
uint8_t        ReceivedValuesIndexSwitch = 0;
uint16_t    ReceivedValueSwitch = 1500;

uint32_t    LastUpDownChannelChanged = 0;
uint16_t    ReceivedValuesUpDown[4] = { 1500, 1500, 1500, 1500 };
uint16_t    ReceivedValuesSumUpDown = 6000;
uint8_t        ReceivedValuesIndexUpDown = 0;
uint16_t    ReceivedValueUpDown = 1500;

uint32_t    LastInclinationChannelChanged = 0;
uint16_t    ReceivedValuesInclination[4] = { 1500, 1500, 1500, 1500 };
uint16_t    ReceivedValuesSumInclination = 6000;
uint8_t        ReceivedValuesIndexInclination = 0;
uint16_t    ReceivedValueInclination = 1500;

uint16_t    ReadSensorValue = 1500;    //    in diese Variable wird der vom Sensor ermittelte und in Microsekunden umgerechnete Wert eingetragen.
                                    //    Er wird von NormalizeServoValues() ausgelesen und verrechnet.
//    Wenn du über 8 Werte glaetten moechtest, dann musst du die ersten zwei Zeilen durch diese ersetzen
//    uint16_t    ReceivedValuesSensor[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 };
//    uint16_t    ReceivedValuesSumSensor = 12000;
uint16_t    ReceivedValuesSensor[4] = { 1500, 1500, 1500, 1500 };
uint16_t    ReceivedValuesSumSensor = 6000;
uint8_t        ReceivedValuesIndexSensor = 0;
uint16_t    ReceivedValueSensor = 1500;    //    Hier steht dann der geglaettete Wert drin, damit kann dann ein Srevo angefahren werden.


//    Diese Methode nutzt die aus den Interrupts ermittelten Werte und glaettet diese etwas (Mittelwert aus den letzten 4 Werten)
void NormalizeServoValues()
{
    ReceivedValuesSumSwitch -= ReceivedValuesSwitch[ReceivedValuesIndexSwitch];
    ReceivedValuesSwitch[ReceivedValuesIndexSwitch] = InterruptSwitch;
    ReceivedValuesSumSwitch += InterruptSwitch;
    ReceivedValuesIndexSwitch = ((ReceivedValuesIndexSwitch + 1) & 0x03);    //    Index erhoehen und ggf. von 4 auf 0 springen

    ReceivedValueSwitch = (ReceivedValuesSumSwitch >> 2);    //    durch 4 teilen

    ReceivedValuesSumUpDown -= ReceivedValuesUpDown[ReceivedValuesIndexUpDown];
    ReceivedValuesUpDown[ReceivedValuesIndexUpDown] = InterruptUpDown;
    ReceivedValuesSumUpDown += InterruptUpDown;
    ReceivedValuesIndexUpDown = ((ReceivedValuesIndexUpDown + 1) & 0x03);    //    Index erhoehen und ggf. von 4 auf 0 springen

    ReceivedValueUpDown = (ReceivedValuesSumUpDown >> 2);    //    durch 4 teilen

    ReceivedValuesSumInclination -= ReceivedValuesInclination[ReceivedValuesIndexInclination];
    ReceivedValuesInclination[ReceivedValuesIndexInclination] = InterruptInclination;
    ReceivedValuesSumInclination += InterruptInclination;
    ReceivedValuesIndexInclination = ((ReceivedValuesIndexInclination + 1) & 0x03);    //    Index erhoehen und ggf. von 4 auf 0 springen

    ReceivedValueInclination = (ReceivedValuesSumInclination >> 2);    //    durch 4 teilen

    //    Den Wert des Sensors glaetten
    ReceivedValuesSumSensor -= ReceivedValuesSensor[ReceivedValuesIndexSensor];    //    Alten Wert aus der Summe entfernen
    ReceivedValuesSensor[ReceivedValuesIndexSensor] = ReadSensorValue;            //    Neuen Wert im Array setzen (damit er spaeter wieder aus der Summe entfernt werden kann)
    ReceivedValuesSumSensor += ReadSensorValue;                                    //    Neuen Wert zur Summe addieren
    ReceivedValuesIndexSensor = ((ReceivedValuesIndexSensor + 1) & 0x03);        //    Index erhoehen und ggf. von 4 auf 0 springen
    ReceivedValueSensor = (ReceivedValuesSumSensor >> 2);    //    durch 4 teilen
    //    Bei einer Glaettung ueber 8 Werte, kommentiere die oberen beiden aus und nutze die folgenden beiden Zeilen
    //    ReceivedValuesIndexSensor = ((ReceivedValuesIndexSensor + 1) & 0x07);        //    Index erhoehen und ggf. von 4 auf 0 springen
    //    ReceivedValueSensor = (ReceivedValuesSumSensor >> 3);    //    durch 8 teilen
}

volatile    uint8_t            LastInterruptState0 = PINB;
volatile    uint8_t            LastInterruptState2 = PIND;

void InterruptSwitchPosition(uint32_t micSec)
{
    uint16_t    nDifference = (uint16_t)(micSec - LastSwitchChannelChanged);

    LastSwitchChannelChanged = micSec;

    if ((nDifference > 750) && (nDifference < 2250))
        InterruptSwitch = nDifference;
}

void InterruptInclinationPosition(uint32_t micSec)
{
    uint16_t    nDifference = (uint16_t)(micSec - LastInclinationChannelChanged);

    LastInclinationChannelChanged = micSec;

    if ((nDifference > 750) && (nDifference < 2250))
        InterruptInclination = nDifference;
}

ISR(PCINT0_vect)
{

    uint8_t    PinState = PINB;
    uint8_t    PinChanges = PinState ^ LastInterruptState0;
    LastInterruptState0 = PinState;

    uint32_t micSec = micros();

    if (PinChanges & (1 << PCINT1))
        InterruptSwitchPosition(micSec);

    if (PinChanges & (1 << PCINT2))
        InterruptInclinationPosition(micSec);
}

void InterruptUpDownPosition(uint32_t micSec)
{
    uint16_t    nDifference = (uint16_t)(micSec - LastUpDownChannelChanged);

    LastUpDownChannelChanged = micSec;

    if ((nDifference > 750) && (nDifference < 2250))
        InterruptUpDown = nDifference;
}

ISR(PCINT2_vect)
{
    uint8_t    PinState = PIND;
    uint8_t    PinChanges = PinState ^ LastInterruptState2;
    LastInterruptState2 = PinState;

    uint32_t micSec = micros();

    if (PinChanges & (1 << PCINT22))
        InterruptUpDownPosition(micSec);
}


void setup()
{
    Wire.begin();

    //Baudrate Serialmonitor
    Serial.begin(9600);

    // Eingänge
    pinMode(ch1, INPUT);
    pinMode(ch3, INPUT);
    pinMode(ch4, INPUT);

    // Servobin bezeichnen
    servoBase.attach(servoBasePin);
    servoTeaArm.attach(servoTeaArmPin);

    // MPU6050 überprüfen
    Serial.println("Initializing the sensor");
    sensor.initialize();
    Serial.println(sensor.testConnection() ? "Successfully Connected" : "Connection failed");
    delay(1000);
    Serial.println("Taking Values from the sensor");
    delay(1000);

    //    Die Interrupts fuer die benoetigten Kanaele aktivieren
    //    Hier findest du die benoetigten Konstanten
    //    https://wolles-elektronikkiste.de/en/interrupts-part-2-pin-change-interrupts

    uint8_t msk0 = (1 << PCINT1)    //    Kanal 9
        | (1 << PCINT2);            //    Kanal 10
    uint8_t msk2 = (1 << PCINT22);    //    Kanal 6


    PCICR |= ((1 << PCIE0) | (1 << PCIE2));
    PCMSK0 |= msk0;        //    Steuert die Methode ISR(PCINT0_vect) an
    PCMSK2 |= msk2;        //    Steuert die Methode ISR(PCINT2_vect) an

    sei();
}


void loop()
{
    // Umrechnung der Werte für den MPU6050
    sensor.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    ReadSensorValue = map(ay, -17000, 17000, 2000, 1000);

    //    Die per interrupt gelesenen Werte auswerten
    NormalizeServoValues();

    // Ausgabe Daten an den Serialmonitor
    Serial.print("   Schild heben_senken : ");
    Serial.print(ReceivedValueInclination);
    Serial.print("   Schild tillt: ");
    Serial.print(ReceivedValueUpDown);
    Serial.print("   Winkel Schild_auto: ");
    Serial.print(ReceivedValueSensor);
    Serial.print("   Auto On_OFF : ");
    Serial.println(InterruptSwitch);



    // Befehle an die Servos schicken
    if (ReceivedValueSwitch > 1800)
        servoTeaArm.writeMicroseconds(ReceivedValueSensor);

    if (ReceivedValueSwitch < 1600)
        servoTeaArm.writeMicroseconds(ReceivedValueUpDown);

    servoBase.write(ReceivedValueInclination);

    delay(10);
}

Funktioniert alles soweit, nur wenn ich von Automatik zurück schalte und der Wert so zwischen 1300 - 1600 ist passt alles, ist der Wert aber außerhalb des Bereichs dann springt der Servo auf eine Seite auf Anschlag und fährt dann auf den momentanen Wert des Knüppels. An was kann das liegen?

Was mir noch aufgefallen ist der Servoweg ist nicht 180° von beiden Servos, der Knüppelweg ist aber richtig von 2015 - 990. Passt da was bei der Umrechnung nicht?

gismow · 21.07.2024

Moin Markus,

Die Interrupts rechnen ja die tatsächlichen Mikrosekunden aus. Etwas feilen kannst du am map() Befehl, indem du die 2000/1000 durch deine Grenzwerte ersetzt.

Für dieses „Fahren auf Anschlag“ beim Umschalten habe ich aktuell keine Erklärung. Die Servowerte werden ja kontinuierlich errechnet, es ist ja nicht abhängig von der Schalterstellung.

gismow · 21.07.2024

Moin Markus,

Die Interrupts rechnen ja die tatsächlichen Mikrosekunden aus. Etwas feilen kannst du am map() Befehl, indem du die 2000/1000 durch deine Grenzwerte ersetzt.

Für dieses „Fahren auf Anschlag“ beim Umschalten habe ich aktuell keine Erklärung.

markusstn · 21.07.2024

Hi Peter,
danke für Deine Hilfe.
So wie ich erkennen kann ist der Servoweg nur so um die 90° bis 100°, wieso auch immer das jetzt so ist versteh ich nicht.
Kann da was in der Servo Libary nicht stimmen oder müssen die Werte doch von 0/180 sein.
Ich probiere das später mal aus ob sich was ändert.

gismow · 21.07.2024

Moin Markus,

eigentlich müsstest du mit einer Annäherung arbeiten. Du hast zwei Quellen, den Knüppel und den Sensor, beide können völlig unterschiedliche Werte haben. Wenn du also umschaltest, dann hast du einen unnatürlichen Sprung.
Vorschlag: Du definierst eine Variable TargetServoValue und eine CurrentServoValue. Je nach Schalterstellung packst du den Sensor- oder den Kanalwert in TargetServoValue. Dann schaust du, ob CurrentServoValue kleiner oder größer ist. Je nachdem addierst oder subtrahierst du eine definierte Schrittweite, um den Wert TargetServoValue anzunähern. Ist die Differenz kleiner als die Schrittweite, setzt du CurrentServoValue gleich den TargetServoValue. So bekommst du eine smoothe Bewegung hin. Mit der Schrittweite kannst du ja spielen, sie wird ja ca. 90 mal pro Sekunde addiert. Starte mal mit einer Schrittweite von 3.

gismow · 21.07.2024

markusstn schrieb:
Hi Peter,
danke für Deine Hilfe.
So wie ich erkennen kann ist der Servoweg nur so um die 90° bis 100°, wieso auch immer das jetzt so ist versteh ich nicht.
Kann da was in der Servo Libary nicht stimmen oder müssen die Werte doch von 0/180 sein.
Ich probiere das später mal aus ob sich was ändert.

Moin Markus,

das hängt auch von Servo ab, wie er gebaut ist und welchen Weg die Ingenieure ihm zugestanden haben.

markusstn · 21.07.2024

gismow schrieb:
Moin Markus,

eigentlich müsstest du mit einer Annäherung arbeiten. Du hast zwei Quellen, den Knüppel und den Sensor, beide können völlig unterschiedliche Werte haben. Wenn du also umschaltest, dann hast du einen unnatürlichen Sprung.
Vorschlag: Du definierst eine Variable TargetServoValue und eine CurrentServoValue. Je nach Schalterstellung packst du den Sensor- oder den Kanalwert in TargetServoValue. Dann schaust du, ob CurrentServoValue kleiner oder größer ist. Je nachdem addierst oder subtrahierst du eine definierte Schrittweite, um den Wert TargetServoValue anzunähern. Ist die Differenz kleiner als die Schrittweite, setzt du CurrentServoValue gleich den TargetServoValue. So bekommst du eine smoothe Bewegung hin. Mit der Schrittweite kannst du ja spielen, sie wird ja ca. 90 mal pro Sekunde addiert. Starte mal mit einer Schrittweite von 3.

Hi Peter,
eigentlich ist der Wert vom MPU6050 meistens so um die 1500, da er ja so wie es geplant ist am Schild befestigt wird um die Neigung in Waage zu halten.
Im Moment teste ich noch auserhalb des Modells. Jetzt läuft es ja schon mal stabil um es dies Woche mal ins Modell zu bauen. Dann kann ich testen ob alles so passt wie ich mir das vorgestellt hab. Das einzigen was vieleicht noch sein kann das ich einen Todbereich vom Hydraulikventil hab, da muss ich erst mal schauen wie sich alles verhält wenn es eingebaut ist.

gismow · 21.07.2024

Moin Markus,

ich drücke dir die Daumen und bin gespannt auf das Ergebnis.

markusstn · 23.07.2024

Kleines Update

Hab mir eine kleine Platine gebastelt

Gerade noch einmal alles getestet, keinen Fehler gemacht

Demnächst werde ich die Platine in die Raupe bauen

markusstn · 24.07.2024

Hallo Peter,
hab heute mal alles eingebaut. Wie vermutet hab ich ein Tod Bereich vom Hydraulik Ventil.
So wie ich das sehe ist des so um die 90°, also 45° pro Seite. Kann mann sowas auch programmieren?

Geht das wenn ich nur den Bereich von den 1000-2000 erweitere so auf 600-2400.
Der MPU Sensor müsste halt eine steile Kurve haben, oder muss ich da was in der Funke einstellen?

markusstn · 25.07.2024

Also ich habe einige Einstellungen probiert aber leider funktioniert es nicht. Es Schaukel sich dann noch hoch und das Servo springt dauernd hin und her. Irgendwo ist der Wurm drin

Schade dass es nicht so wie erhofft funktioniert. Hab jetzt alles wieder ausgebaut

wenn ich dem MPU 6050 mit der Hand bewege steuert er auch das Ventil. Ich sehe schon das wird nicht so einfach wie gedacht

gismow · 26.07.2024

markusstn schrieb:
Hallo Peter,
hab heute mal alles eingebaut. Wie vermutet hab ich ein Tod Bereich vom Hydraulik Ventil.
So wie ich das sehe ist des so um die 90°, also 45° pro Seite. Kann mann sowas auch programmieren?
Geht das wenn ich nur den Bereich von den 1000-2000 erweitere so auf 600-2400.
Der MPU Sensor müsste halt eine steile Kurve haben, oder muss ich da was in der Funke einstellen?

Moin Markus,

du kannst natürlich den Bereich erweitern, hierzu änderst du einfach die beiden letzten Werte im map() Befehl.

Die Auswertmethoden haben ja schon einen größeren Bereich, den kannst du versuchsweise auch erweitern.

Aber bedenke, es kommt auch auf die Servos an, ob sie diesen Bereich auch abdecken können.

Automatische Schildsteuerung mit Arduino Nano und MPU6050

Well-known member

Well-known member

Well-known member

Active member

Active member

Well-known member

Well-known member

Active member

Well-known member

Active member

Well-known member

Active member

Active member

Well-known member

Active member

Active member

Well-known member

Active member

Active member

Well-known member

Active member

Well-known member

Well-known member

Well-known member

Active member

Wir schützen deine Privatsphäre