/*
Automatische Schildsteuerung mit Arduino Nano und MPU6050
von Markus & Peter
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 !!!!
*/
// Libary (Bibliotheken)
#include <Servo.h>
#include <Wire.h>
#include <MPU6050.h>
// OLED Display
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define OLED_RESET -1 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_WIDTH 128 // OLED display Breite, in pixels
#define SCREEN_HEIGHT 64 // OLED display Höhe, in pixels
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
// Eingänge, Ausgänge, Variablen
int ch4 = 9; // Signal vom RC Empfänger (Schalter auf Funke Automatik ein_aus)
int ch1 = 6; // Signal vom RC Empfänger (Servo Schild auf_ab)
int ch3 = 10; // Signal vom RC Empfänger (Servo Schild Neigung einstellen)
const int servoBasePin = 3; // Servo Schild auf_ab
const int servoTeaArmPin = 5; // Servo Schild Neigung
Servo servoBase;
Servo servoTeaArm;
int servoValRoll;
int servoValPitch;
// MPU6050 Senor
MPU6050 sensor;
int16_t ax, ay, az;
int16_t gx, gy, gz;
// Hier speichern die Interrupts die erfassten Werte
volatile uint16_t InterruptSwitch = 1487;
volatile uint16_t InterruptUpDown = 1487;
volatile uint16_t InterruptInclination = 1487;
// Dieser Block wird benoetigt um das Servosignal auszuwerten und etwas zu glaetten
uint32_t LastSwitchChannelChanged = 0;
uint16_t ReceivedValuesSwitch[4] = { 1487, 1487, 1487, 1487 };
uint16_t ReceivedValuesSumSwitch = 5948;
uint8_t ReceivedValuesIndexSwitch = 0;
uint16_t ReceivedValueSwitch = 1487;
uint32_t LastUpDownChannelChanged = 0;
uint16_t ReceivedValuesUpDown[4] = { 1487, 1487, 1487, 1487 };
uint16_t ReceivedValuesSumUpDown = 5948;
uint8_t ReceivedValuesIndexUpDown = 0;
uint16_t ReceivedValueUpDown = 1487;
uint32_t LastInclinationChannelChanged = 0;
uint16_t ReceivedValuesInclination[4] = { 1487, 1487, 1487, 1487 };
uint16_t ReceivedValuesSumInclination = 5948;
uint8_t ReceivedValuesIndexInclination = 0;
uint16_t ReceivedValueInclination = 1487;
// Sensor ay Wert
uint16_t ReadSensorValue = 1487; // 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] = { 1487, 1487, 1487, 1487 };
uint16_t ReceivedValuesSumSensor = 5948;
uint8_t ReceivedValuesIndexSensor = 0;
uint16_t ReceivedValueSensor = 1487; // Hier steht dann der geglaettete Wert drin, damit kann dann ein Srevo angefahren werden.
// Sensor ax Wert
uint16_t ReadSensorValue1 = 1487; // 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 ReceivedValuesSensor1[4] = { 1487, 1487, 1487, 1487 };
uint16_t ReceivedValuesSumSensor1 = 5948;
uint8_t ReceivedValuesIndexSensor1 = 0;
uint16_t ReceivedValueSensor1 = 1487; // 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()
{
// Den Wert des Schalters glaetten
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
// Den Wert Auf_Ab Schild glaetten
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
// Den Wert Auf_Ab Schild glaetten
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 ay 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
// Den Wert ax des Sensors glaetten
ReceivedValuesSumSensor1 -= ReceivedValuesSensor1[ReceivedValuesIndexSensor1]; // Alten Wert aus der Summe entfernen
ReceivedValuesSensor1[ReceivedValuesIndexSensor1] = ReadSensorValue1; // Neuen Wert im Array setzen (damit er spaeter wieder aus der Summe entfernt werden kann)
ReceivedValuesSumSensor1 += ReadSensorValue1; // Neuen Wert zur Summe addieren
ReceivedValuesIndexSensor1 = ((ReceivedValuesIndexSensor1 + 1) & 0x03); // Index erhoehen und ggf. von 4 auf 0 springen
ReceivedValueSensor1 = (ReceivedValuesSumSensor1 >> 2); // durch 4 teilen
// Bei einer Glaettung ueber 8 Werte, kommentiere die oberen beiden aus und nutze die folgenden beiden Zeilen
// ReceivedValuesIndexSensor1 = ((ReceivedValuesIndexSensor1 + 1) & 0x07); // Index erhoehen und ggf. von 4 auf 0 springen
// ReceivedValueSensor1 = (ReceivedValuesSumSensor1 >> 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(115200);
// OLED Display
/*display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // I2C address = 0x3C
delay(1000);
display.clearDisplay();
display.setTextSize(2);
display.setTextColor(WHITE);
display.setCursor(43,3);
display.print("CAT");
display.fillTriangle(45, 18, 75, 18, 60, 13, 1);
display.drawLine(45, 18, 60, 13, 0);
display.drawLine(60, 13, 75, 18, 0);
display.drawRoundRect(30, 30, 60, 30, 5, 1);
display.drawRect(0, 0, 128, 64, 1);
display.display();
*/
// 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);
ReadSensorValue1 = map(ax, -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 aY Schild_auto: ");
Serial.print(ReceivedValueSensor);
Serial.print(" Winkel aX Schild_auto: ");
Serial.print(ReceivedValueSensor1);
Serial.print(" Auto On_OFF : ");
Serial.println(InterruptSwitch);
// Ausgabe Daten an das OLED Display
// Befehle an die Servos schicken
/*
Anpassung an den Servo der Schild Neigung und Scild auf_ab
>=1510 bis <=1460 Bereich in dem der MPU6050 arbeiten soll
1260 = Servoweg und 1800 = Servoweg muss an das jeweilige Servo angepasst werden!
um die Richtung zu ändern müssen die zwei Werte einfach getauscht werden
*/
if (ReceivedValueSwitch > 1800) // wenn der Schalter ein Signal von mehr als 1800 schickt (Schalter nach oben), dann führe diese Befehle aus (voll automatik)
{
// Signal Servo Neigung
if (ReceivedValueSensor >= 1532) // wenn ein Signal von mehr gleich 1510 anliegt, dann schicke zum Servo 1260 als Signal
servoTeaArm.writeMicroseconds(1362);
if (ReceivedValueSensor <= 1493) // wenn ein Signal von weniger gleich 1460 anliegt, dann schicke zum Servo 1800 als Signal
servoTeaArm.writeMicroseconds(1688);
if (ReceivedValueSensor == 1513) // dieser Wert ist die Mittelstellung, hier kann man den Wert ändern um die Mittelstellung des Schild´s zu ändern
servoTeaArm.writeMicroseconds(1513);
// Signal Servo auf_ab
if (ReceivedValueSensor1 >= 1492) // wenn ein Signal von mehr gleich 1520 anliegt, dann schicke zum Servo 1770 als Signal
servoBase.writeMicroseconds(1698);
if (ReceivedValueSensor1 <= 1452) // wenn ein Signal von weniger gleich 1480 anliegt, dann schicke zum Servo 1210 als Signal
servoBase.writeMicroseconds(1362);
if (ReceivedValueSensor1 == 1472) // dieser Wert ist die Mittelstellung, hier kann man den Wert ändern um die Mittelstellung des Schild´s zu ändern
servoBase.writeMicroseconds(1472);
}
else if (ReceivedValueSwitch < 1100) // wenn der Schalter ein Signal von weniger als 1100 schickt (Schalter nach unten), dann führe diese Befehle aus (automatik tilt)
{
// Signal Servo Neigung
if (ReceivedValueSensor >= 1532) // wenn ein Signal von mehr gleich 1510 anliegt, dann schicke zum Servo 1260 als Signal
servoTeaArm.writeMicroseconds(1362);
if (ReceivedValueSensor <= 1493) // wenn ein Signal von weniger gleich 1460 anliegt, dann schicke zum Servo 1800 als Signal
servoTeaArm.writeMicroseconds(1688);
if (ReceivedValueSensor == 1513) // dieser Wert ist die Mittelstellung, hier kann man den Wert ändern um die Mittelstellung des Schild´s zu ändern
servoTeaArm.writeMicroseconds(1513);
// Signal Servo auf_ab
servoBase.writeMicroseconds(ReceivedValueInclination); // schreibe den geglätteten Wert von der Funke an das Servo(Auf_Ab)
}
else // wenn (Schalter mitte), dann führe diese Befehle aus (manuell)
{
servoTeaArm.writeMicroseconds(ReceivedValueUpDown); // schreibe den geglätteten Wert von der Funke an das Servo (Neigung)
servoBase.writeMicroseconds(ReceivedValueInclination); // schreibe den geglätteten Wert von der Funke an das Servo(Auf_Ab)
}
delay(10);
}
