Hallo Danke
schon einmal für die Antwort.
Ich versuche das Problem nun genauer zu beschreiben. Ich versuche einen Wagen über zwei Elektromotoren anzutreiben. Die Geschwindigkeit der Motoren sollen mit Hilfe eines Drehpotentiometer geregelt werden. Damit man nicht zu Nah oder zu weit weg vom Wagen ist will ich die Geschwindigkeit über einen Ultraschallsensor anpassen. D.h. ist die Person zu weit weg wird der Wagen langsamer, ist die zu nah dran wird der Wagen schneller. Ich hoffe das kann man soweit verstehen.
Die gewünschte Geschwindigkeit soll über ein PWM an die Motoren weitergegeben werden. Da das ganze ohne Regelung immer sehr unsanft war, wollte ich es mit einer PID Reglung optimieren.
#include <LiquidCrystal.h>
#include <PID_v1.h>
const int MotorAr = 43;
const int DIAGAr = 47;
const int PWMr=45;
const int DIAGBr =51;
const int MotorBr = 53;
const int CSr =49;
const int MotorAl = 42;
const int DIAGAl = 46;
const int PWMl=44;
const int DIAGBl =50;
const int MotorBl = 52;
const int CSl =48;
const int Poti=A12;
LiquidCrystal lcd(16, 17, 18, 19, 20, 21);
const int pingPin = 2;
int Abstand, optAbstand=40;
int WertPoti;
long duration;
double vsoll, vist, Kp=2, Ki=5, Kd=1, v, SampleTime=100;
PID vSchiebPID(&vist, &v,&vsoll, Kp, Ki, Kd, DIRECT);
void setup(){
Serial.begin(9600);
lcd.begin(16, 2);
pinMode(MotorAr, OUTPUT);
pinMode(MotorBr, OUTPUT);
pinMode(CSr, OUTPUT);
pinMode(DIAGAr, INPUT);
pinMode(DIAGBr, INPUT);
pinMode (PWMr, OUTPUT);
pinMode(MotorAl, OUTPUT);
pinMode(MotorBl, OUTPUT);
pinMode(CSl, OUTPUT);
pinMode(DIAGAl, INPUT);
pinMode(DIAGBl, INPUT);
pinMode (PWMl, OUTPUT);
digitalWrite(CSr, HIGH);
digitalWrite(CSl, HIGH);
lcd.print("Start!");
vSchiebPID.SetOutputLimits(0 , 255); //PWM ansteuern
vSchiebPID.SetMode(AUTOMATIC);
vSchiebPID.SetSampleTime(SampleTime);
}
void loop(){
Abstand = Abstandsmessung(); //Abstand über den Ultraschallsensor einlesen
Serial.print("Abstand");
Serial.print(Abstand);
Serial.println();
digitalWrite(CSr, LOW);
digitalWrite(CSl, LOW);
digitalWrite(MotorAr, HIGH);
digitalWrite(MotorBr, LOW);
digitalWrite(MotorAl, HIGH);
digitalWrite(MotorBl, LOW);
WertPoti=analogRead(Poti);
lcd.print("Poti:");
lcd.print(WertPoti);
lcd.clear();
Serial.print("Poti:");
Serial.print(WertPoti);
Serial.println();
vsoll=analogRead(Poti)/4.02; //die gewünschte Geschwingikeit vom Poti
lcd.print("v:");
lcd.print(vsoll);
Serial.print("VSoll:");
Serial.print(vsoll);
Serial.println();
digitalWrite (CSr, LOW);
digitalWrite (CSl, LOW);
//Geschwindigkeitsänderung über Ultraschallsensor
if (Abstand <= optAbstand+5 || Abstand >= optAbstand-5)
{
vist=vsoll;
}
if (Abstand < optAbstand-5);
{
vist=vsoll+(optAbstand-Abstand)*2;
}
if (Abstand > optAbstand+5)
{
vist=vsoll-(Abstand-optAbstand)*2;
}
analogWrite(PWMl,v);
analogWrite(PWMr, v);
Serial.print("vist:");
Serial.print(vist);
Serial.println();
vSchiebPID.Compute();
analogWrite(PWMl,v);
analogWrite(PWMr, v);
Serial.print("v:");
Serial.print(v);
Serial.println();
}
int Abstandsmessung()
{
lcd.display();
pinMode(pingPin, OUTPUT);
digitalWrite(pingPin, LOW);
delayMicroseconds(2);
digitalWrite(pingPin, HIGH);
delayMicroseconds(5);
digitalWrite(pingPin, LOW);
pinMode(pingPin, INPUT);
duration = pulseIn(pingPin, HIGH);
Abstand = microsecondsToCentimeters(duration);
lcd.print("Abstand");
lcd.setCursor(0, 1);
lcd.print(Abstand);
lcd.clear();
return Abstand;
}
long microsecondsToCentimeters(long microseconds)
{
return microseconds / 29 / 2;
}
Vielen Dank
Gruß Karo
) mit der Karre hinterher.