pid Positionsteuerung mit dc motor und poti arduino

[Philipp D. (dreyer)]

Moin,
ich bin Neuling und ungelehrter aber höchst interessiert an 
mikrocontrollern.
Habe mir für den anfang ein arduino duemillanove mit motorshield rev 
zugelegt bereu aber auch schon den kauf des motorshields, aber egal zum 
Thema. ich habe hier als set up Duelmillanove sainsmart motorshiel einen 
getriebe dc motor 1 zu 17 übersetzung 12v travo mehrere linera potis 
gewindestange mit einenr mutter an der ein Teleskopierbare Hebel 
angebaut ist. Dieser führt zum poti und betätigt diesen. Die 
gewindestange wird vom dc motor gedreht und schwenk den hebel von links 
nach rechts. Das ist mein Versuchsaufbau, das spätere ziel das der motor 
mit hilfe eines moosgummirades am lenkrad und das an der vorderachse ein 
ein poti den lenkwinkel bestimmt.


ich hab schon geübt und gespielt mit motor und poti aber bei der pid 
positions regelung finde ich momentan meine Grenzen.
Ich hab mir mehrere codes und librarys durchgelesen muss aber sagen aus 
den librarys werde ich nicht klug! darum meine bitte an euch könnt ihr 
mir bitte weiterhelfen
Brett beguards pid example aus dem tutorial wurde meist als sehr rubust 
gelobt aber irgendwie fehlt da doch die würze für meon vorhaben ich 
dachte daran die aggressiv KP,KI,KD und konstant KD,KI,KP von der 
adaptive tuning pid library einzubauen um schnell und sicher zur 
gewünschten position zu kommen.

so ich hab mich mal probiert die beiden zusammen zu bringen weis aber 
nicht mehr weiter wie ich die parameter KP,KI,KD tausche und verdammte 
axt wie krieg da die drehrichtung raus das kann doch nicht so einfach 
wie zum beispiel mit einer if funktion. :D

/*working variables*/
unsigned long lastTime;
double Input, Output, Setpoint;
double ITerm, lastInput;
double kp, ki, kd;
int SampleTime = 1000; //1 sec
double outMin, outMax;
bool inAuto = false;
double aggKp=4, aggKi=0.2, aggKd=1;
double consKp=1, consKi=0.05, consKd=0.25;
 
#define MANUAL 0
#define AUTOMATIC 1
 
#define DIRECT 0
#define REVERSE 1

#include <MSMotorShield.h>

MS_DCMotor motor(4);



int controllerDirection=DIRECT;
 
 void setup() {
   Serial.begin(9600);
   motor.run(RELEASE);
   Input = analogRead(5);
   Setpoint = 500;
   
   
 
 }
 
 void loop()  
 {  
   Input = analogRead(5);
   
   double gap = abs(setoint-input);
   if(grap<15);
   SetTunings (consKp, consKi, consKd);
   else
   SetTunings(aggKp, aggKi, aggKd);
 }
void Compute()
{
   if(!inAuto) return;
   unsigned long now = millis();
   int timeChange = (now - lastTime);
   if(timeChange>=SampleTime)
   Input=analogRead(5);
   {
      /*Compute all the working error variables*/
      double error = Setpoint - Input;
      ITerm+= (ki * error);
      if(ITerm > outMax) ITerm= outMax;
      else if(ITerm < outMin) ITerm= outMin;
      double dInput = (Input - lastInput);
 
      /*Compute PID Output*/
      Output = kp * error + ITerm- kd * dInput;
      if(Output > outMax) Output = outMax;
      else if(Output < outMin) Output = outMin;
 
      /*Remember some variables for next time*/
      lastInput = Input;
      lastTime = now;
   }
}
 
void SetTunings(double Kp, double Ki, double Kd)
{
   if (Kp<0 || Ki<0|| Kd<0) return;
 
  double SampleTimeInSec = ((double)SampleTime)/1000;
   kp = Kp;
   ki = Ki * SampleTimeInSec;
   kd = Kd / SampleTimeInSec;
 
  if(controllerDirection ==REVERSE)
   {
      kp = (0 - kp);
      ki = (0 - ki);
      kd = (0 - kd);
   }
}
 
void SetSampleTime(int NewSampleTime)
{
   if (NewSampleTime > 0)
   {
      double ratio  = (double)NewSampleTime
                      / (double)SampleTime;
      ki *= ratio;
      kd /= ratio;
      SampleTime = (unsigned long)NewSampleTime;
   }
}
 
void SetOutputLimits(double Min, double Max)
{
   if(Min > Max) return;
   outMin = Min;
   outMax = Max;
 
   if(Output > outMax) Output = outMax;
   else if(Output < outMin) Output = outMin;
 
   if(ITerm > outMax) ITerm= outMax;
   else if(ITerm < outMin) ITerm= outMin;
}
 
void SetMode(int Mode)
{
    bool newAuto = (Mode == AUTOMATIC);
    if(newAuto == !inAuto)
    {  /*we just went from manual to auto*/
        Initialize();
    }
    inAuto = newAuto;
}
 
void Initialize()
{
   lastInput = Input;
   ITerm = Output;
   if(ITerm > outMax) ITerm= outMax;
   else if(ITerm < outMin) ITerm= outMin;
}
 
void SetControllerDirection(int Direction)
{
   controllerDirection = Direction;
}

oder sollte man ein autotune regler nehmen auf jeden fall darf er nie 
aufhören zu regeln da es fremdeinflüsse auf die lenkung gibt die den 
schonmal ne bewegung am lenkrad ausmachen, aber das dauerhafte regeln 
dürfte ja ein positiver nebeneffekt sein.

ich hoffe ihr könnt mir helfen

Vielen dank im vorraus

dreyer

[tommy (Gast)]

Philipp Dreyer schrieb:
> if(grap<15);

wo haste denn grap deklariert?

ausserdem: nach if (irgendeine Bedingung) hätte ich jetzt
kein Semikolon (;) erwartet.

[Mike (Gast)]

tommy schrieb:
> ausserdem: nach if (irgendeine Bedingung) hätte ich jetzt
> kein Semikolon (;) erwartet.

Nur mit Semikolon dahinter kann der Compiler die Zeile wegoptimieren und 
über das nicht deklarierte grap hinwegsehen. ;-)

[Philipp D. (dreyer)]

Muss man zum drehrichtungswechsel die pid paramter mit einem - versehen 
?
               if (grap<0) (contollerDirection=REVERSE);



               if (Output<0) motor.run(BACKWARD);
               if (Output>0) motor.run(FORWARD);
               double Min=110, Max=250;
               set.MotorSpeed(Output);

was meint ihr dazu?

danke im vorraus

dreyer

[Matthias S. (Firma: matzetronics) (mschoeldgen)]

Mir scheint, du stocherst da etwas im Dunklen. Lass dir doch mal per 
seriell einige Parameter, wie z.B. outMin und outMax, sowie den Output 
des PID Reglers ausgeben und schau erstmal, wo die Werte überhaupt sind, 
wenn du manuell das Rückmeldepoti drehst (Motor abgeklemmt). Je näher du 
dann dem Setpoint kommst, umso kleiner sollte Output werden. Übrigens 
ist double hier wirklich übertrieben und kostet sehr viel Zeit. Wenn du 
schon mit Gleitkommazahlen agierst, reicht float hier dicke aus. Eine 
flinke PID Berechnung wäre besser mit int realisiert.
Gut, wenn Output das Vorzeichen wechselt, müsstest du, wenn alles o.k. 
ist, den Setpoint überschritten haben und das Servo will in die andere 
Richtung drehen.
Dein Ziel ist es nun, Output in ein passendes Format zu bringen und 
damit die PWM anzusteuern.

Dein nächstes Ziel müsste sein, die Parameter P,I und D per Konsole 
ändern zu können, um dein Servogebilde stabil und reaktionsschnell zu 
bekommen, wenn du den Setpoint per Sollwert-Poti änderst.

Als kleiner Tipp: Wenn du den einen Parameter auf null drehst, ist er 
ausser Gefecht. Also D auf null und es bleibt ein PI Regler übrig.
Fang mit P alleine an und dann füge die I und D Komponente zu.

[Philipp D. (dreyer)]

Warum krieg ich da kein Output raus?

/*working variables*/
unsigned long lastTime;
float Input, Output, Setpoint;
float ITerm, lastInput;
float kp, ki, kd;
int SampleTime = 1000; //1 sec
float outMin, outMax;
bool inAuto = false;
float aggKp=4, aggKi=0.2, aggKd=1;
float consKp=1, consKi=0.05, consKd=0.25;
float Min=110, Max=250;
#define MANUAL 0
#define AUTOMATIC 1
 
#define DIRECT 0
#define REVERSE 1

#include <MSMotorShield.h>

MS_DCMotor motor(4);


int controllerDirection=DIRECT;
 
 void setup() {
   Serial.begin(9600);
   motor.run(RELEASE);
   Input = analogRead(5);
   Setpoint = 500;
   

 
 }
 
 void loop()  
 {  
   Input = analogRead(5);
   
   float grap = (Setpoint-Input);
   if(grap<10);
   SetTunings (consKp, consKi, consKd);
  
   if(grap>10)
   SetTunings(aggKp, aggKi, aggKd);
   
     Serial.print("setpoint: ");Serial.print(Setpoint); Serial.print(" ");
  Serial.print("input: ");Serial.print(Input); Serial.print(" ");
  Serial.print("output: ");Serial.print(Output); Serial.print(" ");
  delay(1000);
  
 }
void Compute()
{
   if(!inAuto) return;
   unsigned long now = millis();
   int timeChange = (now - lastTime);
   if(timeChange>=SampleTime)
   Input=analogRead(5);
   {
      /*Compute all the working error variables*/
      float error = Setpoint - Input;
      ITerm+= (ki * error);
      if(ITerm > outMax) ITerm= outMax;
      else if(ITerm < outMin) ITerm= outMin;
      float dInput = (Input - lastInput);
 
      /*Compute PID Output*/
      Output = kp * error + ITerm- kd * dInput;
      if(Output > outMax) Output = outMax;
      else if(Output < outMin) Output = outMin;
 
      /*Remember some variables for next time*/
      lastInput = Input;
      lastTime = now;
   }
}
 
void SetTunings(double Kp, double Ki, double Kd)
{
   if (Kp<0 || Ki<0|| Kd<0) return;
 
  float SampleTimeInSec = ((float)SampleTime)/1000;
   kp = Kp;
   ki = Ki * SampleTimeInSec;
   kd = Kd / SampleTimeInSec;
 
  if(controllerDirection ==REVERSE)
   {
      kp = (0 - kp);
      ki = (0 - ki);
      kd = (0 - kd);
   }
}
 
void SetSampleTime(int NewSampleTime)
{
   if (NewSampleTime > 0)
   {
      float ratio  = (float)NewSampleTime
                      / (float)SampleTime;
      ki *= ratio;
      kd /= ratio;
      SampleTime = (unsigned long)NewSampleTime;
   }
}
 
void SetOutputLimits(float Min, float Max)
{ 
  
   if(Min > Max) return;
   outMin = Min;
   outMax = Max;
 
   if(Output > outMax) Output = outMax;
   else if(Output < outMin) Output = outMin;
 
   if(ITerm > outMax) ITerm= outMax;
   else if(ITerm < outMin) ITerm= outMin;
}
 
void SetMode(int Mode)
{
    bool newAuto = (Mode == AUTOMATIC);
    if(newAuto == !inAuto)
    {  /*we just went from manual to auto*/
        Initialize();
    }
    inAuto = newAuto;
}
 
void Initialize()
{
   lastInput = Input;
   ITerm = Output;
   if(ITerm > outMax) ITerm= outMax;
   else if(ITerm < outMin) ITerm= outMin;
}
 
void SetControllerDirection(int Direction)
{
   controllerDirection = Direction;
}

[Philipp D. (dreyer)]

Sagen wir mal so wie kriegt man in arduino rein??

[Matthias S. (Firma: matzetronics) (mschoeldgen)]

Philipp Dreyer schrieb:
> Sagen wir mal so wie kriegt man in arduino rein??

Dieser Satz kein Verb :-P

Evtl. fehlt bei deinem Programm die Hauptschleife? Du hast nichts über 
das gesagt, was am seriellen Port rauskommt?
Geh doch mal bitte schrittweise vor und fang nicht gleich mit dem 
Komplettprogramm an. Prüfe jedes Unterprogramm für sich. Und am besten 
liest du dir auch mal die Application Note 221 von Atmel durch, da wird 
ein prima PID Regler beschrieben, den du leicht auf deine Bedürfnisse 
ändern kannst. Ausserdem steht da eine ganze Menge zur Theorie eines 
PID.

[Mike (Gast)]

Matthias Sch. schrieb:
> Evtl. fehlt bei deinem Programm die Hauptschleife?

Da ist sie doch (Arduino OS)

Philipp Dreyer schrieb:
> void loop()

[Philipp D. (dreyer)]

Matthias Sch. schrieb:
> Philipp Dreyer schrieb:
>> Sagen wir mal so wie kriegt man in arduino rein??
>
> Dieser Satz kein Verb :-P
>
> Evtl. fehlt bei deinem Programm die Hauptschleife? Du hast nichts über
> das gesagt, was am seriellen Port rauskommt?
> Geh doch mal bitte schrittweise vor und fang nicht gleich mit dem
> Komplettprogramm an. Prüfe jedes Unterprogramm für sich. Und am besten
> liest du dir auch mal die Application Note 221 von Atmel durch, da wird
> ein prima PID Regler beschrieben, den du leicht auf deine Bedürfnisse
> ändern kannst. Ausserdem steht da eine ganze Menge zur Theorie eines
> PID.

moin,
 ich war frustriert :D
hab mich aber zusammengerissen und ein bisschen gebastellt und bin bis 
jetzte bei folgendem code angekommen. Meon problem ist momentan, dass 
ich die Pid outputs begränzen will für eine pwm funktion. Also im 
positiven bereich von 110bis250 und im negativen bereich von 
-110bis-250. Dann hab ich hoffentlich die möglichkeit über eine if 
abfrage ob output < als 0 ist die möglichkeit die drehrrichtung des 
motors zu bestimmen und kann den output direkt als pwm zahl einsetzen. 
Das ist mein nächstes ziel! ich hab die Output begrenzung mit den beiden 
Kommentar Strichen "//" aus den code raus genommen. So wie der code ist 
lauft er und regelt auch, aber wenn ich die begrenzung einsetzen will 
setzt er sich auf -10 beim ITerm fest
und beim Output auf -110.

Noch mal geschirr habe ein poti auf A5 und lasse mir die Zahlen Serial 
ausgeben

unsigned long lastTime;
float Input, Output, Setpoint;
float fehler, ITerm, lastInput;
float kp, ki, kd;
int SampleTime = 1000; //1 sec
float NewSampleTime=0;
float outMin, outMax;
bool inAuto = true;
float aggKp=4, aggKi=0.2, aggKd=1;
float consKp=1, consKi=0.05, consKd=0.25;
int Min=(110), Max=(250);
int minusMin(-110), minusMax=(-250);
float Kp=2,Ki=0.5,Kd=2;

float ratio;

 float outmax = (255);
   int outmin = (10);
   int minusoutmax = (-255);
   int minusoutmin = (-10);
   int minusoutMax;
   int minusoutMin;
void setup() {

 Serial.begin(9600);

   Input=analogRead(5);
   Setpoint=(500);




    // put your setup code here, to run once:

}

void loop() {

  Input=analogRead(5);

 if(!inAuto) return;
   unsigned long now = millis();
   int timeChange = (now - lastTime);
   if(timeChange>=SampleTime)

  Input=analogRead(5);
  /*Compute all the working error variables*/
      float fehler = Setpoint - Input;
      ITerm = (ki * fehler);



   // {  if (ITerm > outmax) ITerm = (255);
    //  if (ITerm < outmin) ITerm = (10);}



    // { if  (ITerm > minusoutmax) ITerm = (-255);
     //  if   (ITerm < minusoutmin) ITerm = (-10) ;
     // }
     float dInput = (Input - lastInput);

      /*Compute PID Output*/
      Output = kp * fehler + ITerm- kd * dInput;

      /*Remember some variables for next time*/
      lastInput = Input;
      lastTime = now;


      //if (Kp<0 || Ki<0|| Kd<0) return;


  SampleTime=1000;
  float SampleTimeInSec = ((int) SampleTime/1000);
   kp = Kp;
   ki = Ki * SampleTimeInSec;
   kd = Kd / SampleTimeInSec;


    if (NewSampleTime > 0)

     {  float ratio  = (NewSampleTime / SampleTime);
      ki *= ratio;
      kd /= ratio;
      SampleTime = (unsigned long)NewSampleTime;  }


      if(Min > Max) return;
   outMin = Min;
   outMax = Max;
   minusoutMin = minusMin;
   minusoutMax = minusMax;

   //if(Output > outMax) Output = outMax;
   //else if(Output < outMin) Output = outMin;

   //if(Output > minusoutMax) Output = outMax;
   //else if(Output < minusoutMin) Output = outMin;
  //if(ITerm > outMax) ITerm= outMax;
  //else if(ITerm < outMin) ITerm= outMin;





 //Serial.print("setpoint: ");Serial.print(Setpoint); Serial.print(" ");
  Serial.print("input: ");Serial.print(Input); Serial.print(" ");
  Serial.print("output: ");Serial.print(Output); Serial.print(" ");
  //Serial.print("fehler");Serial.print(fehler);Serial.print(" ");
  Serial.print("iterm");Serial.print(ITerm);Serial.print(" ");
  Serial.print("ki");Serial.print(ki);Serial.print(" ");



  Serial.print("dinput");Serial.print(dInput);Serial.print(" ");
  Serial.print("ratio");Serial.print(ratio);Serial.print(" ");
  Serial.print("sampletimeinsec");Serial.print(SampleTimeInSec);Serial.pri 
nt("  ");
  Serial.print("timechange");Serial.print(timeChange);Serial.print(" ");


  Serial.flush();
  delay(500);


  // put your main code here, to run repeatedly:







für irgendwelche verbesserungen in meiner code grammatik und Aufbau? 
Immer her damit habe ein offenes ohr für alles.
vielen dank im vorraus Dreyer

[Matthias S. (Firma: matzetronics) (mschoeldgen)]

Philipp Dreyer schrieb:
> outMin = Min;
>    outMax = Max;
>    minusoutMin = minusMin;
>    minusoutMax = minusMax;

Das ist mir nicht klar. Warum setzt du den deine eigentlich festen 
Grenzwerte hier auf neue Werte? Lass das mal weg und schau dir dann 
nochmal die Werte an.
Ausserdem solltest du dir wirklich mal die AVR221 ansehen, dein ganzes 
float-zu-int-und-wieder-zurück hin und her ist nämlich noch eine 
Fehlerquelle.
Noch ein Tipp: Bewege dich nur im positiven Zahlenbereich und definiere 
einfach z.B. 128 als Grenze. Ist der Output darunter, dreht sich der 
Motor in eine Richtung, dadrüber in die andere.

[Philipp D. (dreyer)]

Guten
so hab float int aufgeräumt



unsigned long lastTime;
float Input, Output, Setpoint;
float fehler, ITerm, lastInput;
float kp, ki, kd;
int SampleTime = 1000; //1 sec
float NewSampleTime=0;
float outMin, outMax;
bool inAuto = true;
float aggKp=4, aggKi=0.2, aggKd=1;
float consKp=1, consKi=0.05, consKd=0.25;
float Min=(110), Max=(250);
float Kp=2,Ki=0.5,Kd=2;
float minusMin=(-255), minusMax=(-110);
float ratio;



und so krieg ich nu auch die pwm signale eingeschränkt

     //  if(Min > Max) return;
   if (Output > 0)

   outMin = Min,  outMax = Max;

   else if (Output < 0)

   outMin = minusMin, outMax = minusMax;

   if(Output > outMax) Output = outMax;
   else if(Output < outMin) Output = outMin;

   //if (ITerm > outMax) ITerm= outMax;
   //else if(ITerm < outMin) ITerm= outMin;


mal eine frage zum iterm meint ihr ich sollte noch mehrer iterms 
zusammen addieren ?