Quellcode

Quellcode

#include <Wire.h>
#include <ESP32_Servo.h>

#define interrupt_pin GPIO_NUM_15
#define MPU 0x68 // IC2 Adresse des MPU-6050
#define A_R 16384.0 // Umrechnungsfaktor für die Beschleunigung bei 2g Voreinstellung
#define G_R 131.0 // Umrechnungsfaktor für die Drehrate bei 250°/s Voreinstellung

TaskHandle_t Task1; // Neuer Task um das Beschleunigen und Entschleunigen im einem parallelen Prozess abläuft
SemaphoreHandle_t baton=NULL; // Semaphore um gleichzeitiges Schreiben und Lesen zu verhindern

int16_t AcX, AcY, AcZ, GyX, GyY, GyZ; // Beschleunigungs- und Drehratenwerte vor der Umrechnung

volatile unsigned long timeolddebounce=0, timeoldrpm=0, debouncetime=10; // Vergleichszeit und Entprellzeit für die Berechnung der Drehzahl
unsigned int rpm; // Umdrehungen des Antriebrades pro Minute
float pace; // Geschwindigkeit des Antriebrades
float radius=0.1524; //Radius des Rades um die Geschwindigkeit berechnen zu können
volatile byte pulses; // Anzahl der Pulse (Muss noch durch 2 geteilt werden)
unsigned int pulsesperturn=2; // Anzahl der Pulse die für eine Umdrehung benötigt werden. Momentan noch ein Testwert

float Ac[3]; // Beschleunigungswerte umgerechnet
float AcA[2]; // Winkelwerte um die X- und Y-Achse durch die Beschleunigungswerte errechnet
float Gy[3]; // Drehratenwerte umgerechnet
float Angle[3]; // Winkelwerte nach Komplementärfilter
float XaC, YaC, ZaC, XgC, YgC, ZgC; // Werte zum Kalibrieren
long total_time; // Gesamtzeit seit Start des Programms
float dt; // Differenzeit für die Schleifendurchgänge

int val=1620; // Pulsweite mit der der Motor ohne Verzögerung anfährt
float AccX; // Zwischenspeicherung des Beschleunigungswertes durch das Abstoßen
float AccNeeded=-0.5; // Benötigter Beschleunigungswert um die Unterstützung auszulösen (Zum Testen -1)
float AngleY; // Zwischenspeicherung des Winkels um die Y-Achse

Servo ESC; // ESC-Objekt für den Betrieb des Motors

void accelerate() // Funktion für das Beschleunigen der Draisine
{
while(val<1750)
{
ESC.writeMicroseconds(val);
val=val+1;
delay(20);
}
}

void decelerate() // Funktion für das kontrollierte Auslaufen der Draisine
{
while(val>1620) // Noch mit der Höchsgeschwindigkeit zu ersetzten
{
val=val-1;
ESC.writeMicroseconds(val);
delay(20);
xSemaphoreTake(baton,portMAX_DELAY); // Semaphore zur Sicherheit
AngleY=Angle[1];
AccX=(Ac[0]-(-0.45/25)*AngleY)/0.98;
xSemaphoreGive(baton);
//AccNeeded=-1.0+((-0.43/25)*AngleY); // Umrechnung der zur Auslösung der Unterstützung benötigten Beschleunigung,
if(AccX>AccNeeded) // falls gerade eine gewisse Neigung vorhanden ist und dementsprechend bereits ohne Treten eine Beschleunigung vorhanden ist (Teil der Erdbeschleunigung).
{ // Die aufzuwendene Beschleunigung soll erstmal zur Vereinfachung immer die selbe sein
accelerate(); // Die Unterstützung kann auch dann ausgelöst werden wenn gerade das Auslaufen aktiv ist
}
}
}

void speeding(void * parameter) // Überprüfung der Winkel und Beschleunigung um die Unterstützung auszulösen
{ // Lauft auf Kern 0 damit parallel weiterhin Messwerte erfasst werden
for(;;)
{
if((Angle[0]<15.00)&&(Angle[0]>(-15.00))&&(Angle[1]<0.00)&&(Angle[1]>(-25.00)))
{
//Serial.println("Momentan in der Beschleunigungsfunktion"); // Debug-Hilfe
xSemaphoreTake(baton,portMAX_DELAY); // Semaphore zu Sicherheit
AngleY=Angle[1];
AccX=(Ac[0]-(-0.45/25)*AngleY)/0.98;
xSemaphoreGive(baton);
//AccNeeded=-1.0+((-0.45/25)*AngleY); // Siehe decelerate()
//Serial.println(AccNeeded); // Debug-Hilfe
if(AccX>AccNeeded)
{
accelerate(); // Beschleunigen falls der Beschleunigungswert durch das Abstoßen ausreicht
}
else
{
decelerate(); // Ansonsten Auslaufen lassen
}
}
delay(10);
}
}

void counter() // Zählfunktion um alle Pulse pro Umdrehung zu erfassen
{
if((millis()-timeolddebounce)>debouncetime) // Zum Entprellen
{
pulses++;
timeolddebounce=millis();
}
}

void velocity() // Funktion zur Berechnung der Drehzahl und Geschwindigkeit
{
detachInterrupt(digitalPinToInterrupt(interrupt_pin)); // Interrupt für die Berechnung deaktivieren
rpm=(pulses*0.5*60)/pulsesperturn; // Drehzahl
pace=(2*PI*rpm*radius*3.6)/60; // Geschwindigkeit
timeoldrpm=millis();
pulses=0;
attachInterrupt(digitalPinToInterrupt(interrupt_pin), counter, CHANGE);
}

void setup()
{
pinMode(interrupt_pin, INPUT); // Interrupt-Pin initialisieren
attachInterrupt(digitalPinToInterrupt(interrupt_pin), counter, CHANGE); // Interrupt aktivieren
rpm=0;
pace=0;
pulses=0;
AccX=0;
AccNeeded=0;
AngleY=0;
ESC.attach(4); // ESC-Objekt mit Pin 4 initialisieren
ESC.writeMicroseconds(1000); // Pulsweite mit 1000µS initialisieren
Wire.begin();
Wire.beginTransmission(MPU);
Wire.write(0x6B); // PWR_MGMT_1 Register
Wire.write(0); // Auf Null setzen (weckt die MPU-6050 auf)
//Wire.write(0x1C); // ACCEL_CONFIG Register
//Wire.write(0); // Full Scale Range auf 2g
Wire.endTransmission(true);
Serial.begin(115200);
vSemaphoreCreateBinary(baton);
xTaskCreatePinnedToCore(speeding,"Task1",1000,NULL,1,NULL,0);
}

void loop()
{
// Ausgabe der Drehzahl
if (millis()-timeoldrpm>=1000)
{
velocity();
}
xSemaphoreTake(baton,portMAX_DELAY);
// Lesen der Beschleunigungswerte
Wire.beginTransmission(MPU);
Wire.write(0x3B); // Beginnend mit Register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU,6,true); // Fordert sechs Register an
AcX=Wire.read()<<8|Wire.read(); //Alle Werte belegen zwei Register
AcY=Wire.read()<<8|Wire.read();
AcZ=Wire.read()<<8|Wire.read();

// Umrechnung der Beschleunigungswerte
Ac[0]=(AcX+XaC)/A_R;
Ac[1]=(AcY+YaC)/A_R;
Ac[2]=(AcZ+ZaC)/A_R;

//Aus den Beschleunigungswerten werden mit Arkustangens die Winkel um die X- und Y-Achse berechnet (Euler)
AcA[0] = atan(Ac[1]/sqrt(pow(Ac[0],2)+pow(Ac[2],2)))*RAD_TO_DEG;
AcA[1] = atan(-1*Ac[0]/sqrt(pow(Ac[1],2)+pow(Ac[2],2)))*RAD_TO_DEG;

// Lesen der Gyroskopwerte
Wire.beginTransmission(MPU);
Wire.write(0x43); // Beginnend mit Register 0x43 (GYRO_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU,6,true); // Fordert sechs Resgister an
GyX=Wire.read()<<8|Wire.read(); // Alle Werte belegen zwei Register
GyY=Wire.read()<<8|Wire.read();
//GyZ=Wire.read()<<8|Wire.read();

// Umrechnung der Gyroskopwerte
Gy[0]=(GyX+XgC)/G_R;
Gy[1]=(GyY+YgC)/G_R;
//Gy[2]=(GyZ+ZgC)/G_R;

dt=(millis()-total_time)/1000.0; // Berechnung der Differenzzeit
total_time=millis(); // Ermittlung der Gesamtzeit

// Komplementärfilter anwenden
Angle[0]=0.98*(Angle[0]+Gy[0]*dt)+0.02*AcA[0];
Angle[1]=0.98*(Angle[1]+Gy[1]*dt)+0.02*AcA[1];

// Angle[2]=Angle[2]+Gy[2]*dt; // Berechnung des Winkels um die Z-Achse

// Ausgabe der Beschleunigungs- und Winkelwerte als Debughilfe
Serial.print("RPM: "); Serial.print(rpm); Serial.print(" | Ges.: "); Serial.print(pace);
Serial.print(" | X-Bes.: "); Serial.print(Ac[0]); Serial.print(" | X-Winkel: "); Serial.print(Angle[0]);
Serial.print(" | Y-Bes.: "); Serial.print(Ac[1]); Serial.print(" | Y-Winkel: "); Serial.print(Angle[1]);
Serial.print(" | Z-Bes.: "); Serial.print(Ac[2]); //Serial.print(" | Z-Winkel: "); Serial.print(Angle[2]);
Serial.print(" | Kern: "); Serial.println(xPortGetCoreID());
xSemaphoreGive(baton);
delay(10);
}