#include <MPU6050_tockn.h>
#include <Wire.h>
//MPU6050 mpu6050(Wire);
MPU6050 mpu6050(Wire, 0.5, 0.5);
#include <Arduino.h>
#include <U8g2lib.h>
#ifdef U8X8_HAVE_HW_I2C
#include <Wire.h>
#endif
U8G2_SH1106_128X64_NONAME_2_HW_I2C Display(U8G2_R0,U8X8_PIN_NONE);
float accelerationX = 0;
float accelerationY = 0;
float accelerationZ = 0;
float lastSentAccelerationX = 0;
float lastSentAccelerationY = 0;
float lastSentAccelerationZ = 0;
int xJetztPos = 0;
int yJetztPos = 0;//float gehts mit kommastelle
float angleZ = 0;
float lastSentAngleX = 0;
float lastSentAngleY = 0;
float lastSentAngleZ = 0;
int Taster_rechts_hoch = 6;
int Taster_rechts_runter = 3;
int Taster_links_hoch = 5;
int Taster_links_runter = 4;
int End_rechts_hoch = 8;
int End_rechts_runter = 7;
int End_links_hoch = 14;
int End_links_runter = 15;
int Wert;
int Endschalter1;
int Endschalter2;
int Endschalter3;
int Endschalter4;
const int Progammschalter = 16;
#define PIN_CALIBRATION_MPU6050 22 // when D10 is LOW with a switch on GND. MPU6050 NOT calibrating on start up
// the INPUT_PULLUP is going to activated later
#define MPU6050_GET_DATA_EVERY 10 // request data from sensor every 400ms
unsigned long mpu6050Timer = millis();
int unterschied10 = 0;
int unterschied1 = 5; // links Kurve fängt an zu steuern
int unterschied2 = 5; // rechts Kurve fängt an zu steuern
int unterschied3 = 14; // BUG runter drücken Platten gehen runter
int unterschied9 = 7; //hier gehen die platten aus Gleitfahrt
const int Rechts_runter = 12;
const int Rechts_hoch = 11;
const int Links_runter = 9;
const int Links_hoch = 10;
bool rechts_hoch_aktiv = false;
bool rechts_runter_aktiv = false;
bool links_hoch_aktiv = false;
bool links_runter_aktiv = false;
void setup()
{
Display.setI2CAddress(0x78);
Display.setFont(u8g2_font_profont15_tf);// 7x14_tf verändert die Schriftgröße und Schreibweise
Display.begin();
delay(200);
pinMode(Rechts_runter, OUTPUT);
pinMode(Rechts_hoch, OUTPUT);
pinMode(Links_runter, OUTPUT);
pinMode(Links_hoch, OUTPUT);
pinMode(End_rechts_hoch, INPUT_PULLUP);
pinMode(End_rechts_runter, INPUT_PULLUP);
pinMode(End_links_hoch, INPUT_PULLUP);
pinMode(End_links_runter, INPUT_PULLUP);
pinMode(Taster_rechts_hoch, INPUT_PULLUP);//schaltet - ohne Pullup schaltet +
pinMode(Taster_rechts_runter, INPUT_PULLUP);//schaltet - ohne Pullup schaltet +
pinMode(Taster_links_hoch, INPUT_PULLUP);//schaltet - ohne Pullup schaltet +
pinMode(Taster_links_runter, INPUT_PULLUP);//schaltet - ohne Pullup schaltet +
pinMode(Progammschalter,INPUT_PULLUP);
digitalWrite(Rechts_runter, HIGH);
digitalWrite(Rechts_hoch, HIGH);
digitalWrite(Links_runter, HIGH);
digitalWrite(Links_hoch, HIGH);
Serial.begin(9600);
pinMode(PIN_CALIBRATION_MPU6050, INPUT_PULLUP);
Wire.begin();
mpu6050.begin();
if(digitalRead(PIN_CALIBRATION_MPU6050) == HIGH) {
mpu6050.calcGyroOffsets(true);
}
}
void loop()
{
if (digitalRead(Progammschalter)==0 && digitalRead(Progammschalter)==0) loop01();
if (digitalRead(Progammschalter)==1 && digitalRead(Progammschalter)==1) loop00();
}
void loop00() //mit Automatische Nachstellung
{
if((millis()-mpu6050Timer) > MPU6050_GET_DATA_EVERY){
mpu6050.update();
if((millis()-mpu6050Timer) > (MPU6050_GET_DATA_EVERY + 10)){
accelerationX = mpu6050.getAccX();
accelerationY = mpu6050.getAccY();
accelerationZ = mpu6050.getAccZ();
xJetztPos = mpu6050.getAngleX();
yJetztPos = mpu6050.getAngleY();
angleZ = mpu6050.getAngleZ();
mpu6050Timer = millis();
//mpu6050.update();
Serial.print("seitliche Bewegung: ");
Serial.print(xJetztPos);
Serial.print("\ Bug hoch : ");
Serial.print(yJetztPos);
Serial.print("\ tangleZ : ");
Serial.println();
}
}
if (xJetztPos < -(unterschied1)) { // Links Kurve
digitalWrite(Rechts_runter, LOW);
digitalWrite(Rechts_hoch, HIGH);
digitalWrite(Links_runter,HIGH);
digitalWrite(Links_hoch, LOW);
rechts_hoch_aktiv = true;
links_hoch_aktiv = true;
rechts_runter_aktiv = true;
links_runter_aktiv = true;
} else {
// Relais ausschalten, wenn sich die Position in den Nullbereich bewegt
if (rechts_runter_aktiv && xJetztPos <= -unterschied1) // Relay geht aus
{
digitalWrite(Rechts_runter, HIGH);
rechts_runter_aktiv = true;
}
if (links_hoch_aktiv && xJetztPos >= -unterschied10) // Relay geht aus
{
digitalWrite(Links_hoch,HIGH);
links_hoch_aktiv = true;
}
}
if (xJetztPos > +(unterschied2)) { // Rechts Kurve
digitalWrite(Rechts_runter, HIGH);
digitalWrite(Rechts_hoch, LOW);
digitalWrite(Links_runter, LOW);
digitalWrite(Links_hoch, HIGH);
rechts_hoch_aktiv = true;
links_hoch_aktiv = true;
rechts_runter_aktiv = true;
links_runter_aktiv = true;
} else {
// Relais ausschalten, wenn sich die Position in den Nullbereich bewegt
if (rechts_hoch_aktiv && xJetztPos <= -unterschied10) //Relay geht aus
{
digitalWrite(Rechts_hoch, HIGH);
rechts_hoch_aktiv = true;
}
if (links_runter_aktiv && xJetztPos >= -unterschied2) // Relay geht aus
{
digitalWrite(Links_runter, HIGH);
links_runter_aktiv = true;
}
}
/*
if (yJetztPos < -(unterschied3)) { // Bug hoch Minus Bereich
digitalWrite(Rechts_runter, LOW);
digitalWrite(Rechts_hoch, HIGH);
digitalWrite(Links_runter, LOW);
digitalWrite(Links_hoch, HIGH);
rechts_hoch_aktiv = true;
links_hoch_aktiv = true;
rechts_runter_aktiv = true;
links_runter_aktiv = true;
} else {
// Relais ausschalten, wenn sich die Position in den Nullbereich bewegt
if (rechts_runter_aktiv && yJetztPos <= -unterschied3)
{
digitalWrite(Rechts_runter, HIGH);
rechts_runter_aktiv = true;
}
if (links_runter_aktiv && yJetztPos <= -unterschied3)
{
digitalWrite(Links_runter, HIGH);
links_runter_aktiv = true;
}
//}
if (yJetztPos < -(unterschied9)) { // Bug hoch Minus Bereich
digitalWrite(Rechts_runter, HIGH);
digitalWrite(Rechts_hoch, LOW);
digitalWrite(Links_runter, HIGH);
digitalWrite(Links_hoch, LOW);
rechts_hoch_aktiv = true;
links_hoch_aktiv = true;
rechts_runter_aktiv = true;
links_runter_aktiv = true;
} else {
// Relais ausschalten, wenn sich die Position in den Nullbereich bewegt
if (rechts_hoch_aktiv && yJetztPos >= -unterschied9)
{
digitalWrite(Rechts_hoch, HIGH);
rechts_hoch_aktiv = true;
}
if (links_hoch_aktiv && yJetztPos >= -unterschied9)
{
digitalWrite(Links_hoch, HIGH);
links_hoch_aktiv = true;
}
}
}
*/
if (Wert = digitalRead(End_rechts_runter));
else if (Wert == 1)
{
digitalWrite (Rechts_runter, LOW);
}
else
{
digitalWrite (Rechts_runter, HIGH);
}
if (Wert = digitalRead(End_links_runter));
else if (Wert == 1)
{
digitalWrite (Links_runter, LOW);
}
else
{
digitalWrite (Links_runter, HIGH);
}
if (Wert = digitalRead(End_links_hoch));
else if (Wert == 1)
{
// der Mikroschalter wurde gedrückt
digitalWrite (Links_hoch, LOW);
}
else
{
// der Mikroschalter wurde losgelassen
digitalWrite (Links_hoch, HIGH);
}
if (Wert = digitalRead(End_rechts_hoch));
else if (Wert == 1)
{
digitalWrite (Rechts_hoch, LOW);
}
else
{
digitalWrite (Rechts_hoch, HIGH);
}
{
if (Wert = digitalRead(Taster_rechts_hoch));
else if (Wert == 0)
{
// der Mikroschalter wurde gedrückt
digitalWrite (Rechts_hoch, LOW);
digitalWrite (Rechts_runter, HIGH);
}
else
{
// der Mikroschalter wurde losgelassen
digitalWrite (Rechts_hoch, HIGH);
digitalWrite (Rechts_runter, LOW);
}
if (Wert = digitalRead(Taster_rechts_runter));
else if (Wert == 0)
{
// der Mikroschalter wurde gedrückt
digitalWrite (Rechts_runter, LOW);
digitalWrite (Rechts_hoch, HIGH);
}
else
{
// der Mikroschalter wurde losgelassen
digitalWrite (Rechts_runter, HIGH);
digitalWrite (Rechts_hoch, LOW);
}
}
{
if (Wert = digitalRead(Taster_links_hoch));
else if (Wert == 0)
{
// der Mikroschalter wurde gedrückt
digitalWrite (Links_hoch, LOW);
digitalWrite (Links_runter, HIGH);
}
else
{
// der Mikroschalter wurde losgelassen
digitalWrite (Links_hoch, HIGH);
digitalWrite (Links_runter, LOW);
}
{
if (Wert = digitalRead(Taster_links_runter));
else if (Wert == 0)
{
// der Mikroschalter wurde gedrückt
digitalWrite (Links_runter, LOW);
digitalWrite (Links_hoch, HIGH);
}
else
{
// der Mikroschalter wurde losgelassen
digitalWrite (Links_runter, HIGH);
digitalWrite (Links_hoch, LOW);
}
}
}
Serial.println();
Serial.print(" ");
Serial.print("Seitliche Bewegung ");
Serial.print(xJetztPos);
Serial.print(" ");
Serial.print("Bug hoch/runter ");
Serial.print(yJetztPos);
Serial.print(" ");
Serial.print("Rechts/ Links Bewegung ");
// Serial.println(zJetztPos);
Serial.println();
Serial.print("Taster links hoch ");
Serial.print(Wert = digitalRead(Taster_links_hoch)) ;
Serial.println(" ");
Serial.print("Taster links runter ");
Serial.print(Wert = digitalRead(Taster_links_runter)) ;
Serial.println(" ");
Serial.print("Taster rechts hoch ");
Serial.print(Wert = digitalRead(Taster_rechts_hoch)) ;
Serial.println(" ");
Serial.print("Taster rechts runter ");
Serial.print(Wert = digitalRead(Taster_rechts_runter)) ;
Serial.println(" ");
Serial.println();
Serial.print("End links hoch ");
Serial.print(Wert = digitalRead(End_links_hoch)) ;
Serial.println(" ");
Serial.print("End links runter ");
Serial.print(Wert = digitalRead(End_links_runter)) ;
Serial.println(" ");
Serial.print("End rechts hoch ");
Serial.print(Wert = digitalRead(End_rechts_hoch)) ;
Serial.println(" ");
Serial.print("End rechts runter ");
Serial.print(Wert = digitalRead(End_rechts_runter)) ;
Serial.println(" ");
Serial.println();
//Variante Ausgabe auf OledDisplay1 Direktanschluss
Display.clearBuffer();
Display.firstPage();
do
{
Display.setCursor(2, 10);
Display.print("Seitliche Bewegung ");
Display.setCursor(60, 27);
Display.print(xJetztPos) ;
Display.setCursor(2, 42);
Display.print("Bug hoch/runter ");
Display.setCursor(58, 60);
Display.print(yJetztPos) ;
Display.setCursor(2, 27);
Display.print("End");
Display.setCursor(30, 27);
Display.print(Wert = digitalRead(End_links_hoch)) ;
Display.setCursor(2, 60);
Display.print("End");
Display.setCursor(30, 60);
Display.print(Wert = digitalRead(End_links_runter)) ;
Display.setCursor(96, 27);
Display.print("End");
Display.setCursor(122, 27);
Display.print(Wert = digitalRead(End_rechts_hoch)) ;
Display.setCursor(96, 60);
Display.print("End");
Display.setCursor(122, 60);
Display.print(Wert = digitalRead(End_rechts_runter)) ;
Display.sendBuffer();
}
while (Display.nextPage());
//delay(150);
}
void loop01() // Automatische Nachstellung deaktiviert
{
Endschalter1 = digitalRead(End_rechts_runter );
if ((digitalRead(Taster_rechts_runter)== LOW)&&(Endschalter1==HIGH))
{
digitalWrite(Rechts_runter, LOW);
} else {
digitalWrite(Rechts_runter, HIGH);
}
Endschalter2 = digitalRead(End_rechts_hoch );
if ((digitalRead(Taster_rechts_hoch)== LOW)&&(Endschalter2==HIGH))
{
digitalWrite(Rechts_hoch, LOW);
} else {
digitalWrite(Rechts_hoch, HIGH);
}
Endschalter3 = digitalRead(End_links_runter );
if ((digitalRead(Taster_links_runter)== LOW)&&(Endschalter3==HIGH))
{
digitalWrite(Links_runter, LOW);
} else {
digitalWrite(Links_runter, HIGH);
}
Endschalter4 = digitalRead(End_links_hoch );
if ((digitalRead(Taster_links_hoch)== LOW)&&(Endschalter4==HIGH))
{
digitalWrite(Links_hoch, LOW);
} else {
digitalWrite(Links_hoch, HIGH);
}
Serial.print("Taster links hoch ");
Serial.print(Wert = digitalRead(Taster_links_hoch)) ;
Serial.println(" ");
Serial.print("Taster links runter ");
Serial.print(Wert = digitalRead(Taster_links_runter)) ;
Serial.println(" ");
Serial.print("Taster rechts hoch ");
Serial.print(Wert = digitalRead(Taster_rechts_hoch)) ;
Serial.println(" ");
Serial.print("Taster rechts runter ");
Serial.print(Wert = digitalRead(Taster_rechts_runter)) ;
Serial.println(" ");
Serial.println();
Serial.print("End links hoch ");
Serial.print(Wert = digitalRead(End_links_hoch)) ;
Serial.println(" ");
Serial.print("End links runter ");
Serial.print(Wert = digitalRead(End_links_runter)) ;
Serial.println(" ");
Serial.print("End rechts hoch ");
Serial.print(Wert = digitalRead(End_rechts_hoch)) ;
Serial.println(" ");
Serial.print("End rechts runter ");
Serial.print(Wert = digitalRead(End_rechts_runter)) ;
Serial.println(" ");
Serial.println();
Display.clearBuffer();
Display.firstPage();
do
{
Display.setCursor(2, 10);
Display.print("Seitliche Bewegung ");
Display.setCursor(58, 27);
Display.print("M.") ;
Display.setCursor(2, 42);
Display.print("Bug hoch/runter ");
Display.setCursor(58, 60);
Display.print("M.") ;
Display.setCursor(2, 27);
Display.print("End");
Display.setCursor(30, 27);
Display.print(Wert = digitalRead(End_links_hoch)) ;
Display.setCursor(2, 60);
Display.print("End");
Display.setCursor(30, 60);
Display.print(Wert = digitalRead(End_links_runter)) ;
Display.setCursor(96, 27);
Display.print("End");
Display.setCursor(122, 27);
Display.print(Wert = digitalRead(End_rechts_hoch)) ;
Display.setCursor(96, 60);
Display.print("End");
Display.setCursor(122, 60);
Display.print(Wert = digitalRead(End_rechts_runter)) ;
Display.sendBuffer();
}
while (Display.nextPage());
//delay(500);
}
Hallo kann mir jemand weiterhelfen?
Wenn ich die x oder die y-Achse einzeln nutze (also eins von beiden ausdeklariere) funktioniert das Programm wie erwünscht. Ich möchte aber das beide Achsen mit einander arbeiten.
Das Problem ist folgendes:
Ab den Wert: unterschied3 = 14
Bei der links Kurve darf nicht links runter, sondern links hoch.
Bei der rechts Kurve darf nicht rechts runter, sondern rechts hoch.
Ich bedanke mich für eure Mühe.



