- LiFePO4 Speicher Test         
Ergebnis 41 bis 45 von 45

Thema: pthread-Task als Methode einer C++ Klasse?

Baum-Darstellung

Vorheriger Beitrag Vorheriger Beitrag   Nächster Beitrag Nächster Beitrag
  1. 29.09.2016, 18:51 #34
    HaWe
    Gast
    (zur späteren Gesamt-Programmstruktur komme ich später, wenn du einverstanden bist - im Moment sind , genau wie die schon vorhandenen simplen Befehle
    motorOn()
    motorBrake()
    motorCoast()

    die jetzt neuen PID-Funktionen
    RotatePIDtoTarget ()
    RotatePIDdegrees ()
    RotatePIDcontinue ()
    StopPIDcontrol ();
    PIDinit();
    SetPIDparam();
    SetPIDparamEx();

    nur allgemeine "bequeme" Motor-API Funktionen, die universell eingesetzt werden können/sollen zur exakten Steuerung der Encodermotoren;
    hinzu kommen sollen später auch noch weitere Funktionen wie rampup/down und motorSync(port1, port2, syncratio, pwm )



    der aktuelle Code ist dieser hier - es gibt zu PID_calc noch eine Variante 2 wo die #ifdef debugs noch drin sind:
    (das mit dem Stoppen ist noch nicht geändert bzw. berichtigt)

    Code:
                           
                       
//*************************************************************
// PID control
//*************************************************************   

void RotatePIDtoTarget (uint8_t port, long Target, double RotatSpeed); // approach absolute target once
void RotatePIDdegrees  (uint8_t port, long Target, double RotatSpeed); // turn relative degrees
void RotatePIDcontinue (uint8_t port, long Target, double RotatSpeed); // approach target continuously
void StopPIDcontrol    (uint8_t port);               

//*************************************************************

void PIDinit(); // P=0.4, I=0.4, D=10.0
 
// simple customized PID setting:
void SetPIDparam(uint8_t port, double P,double I,double D);
 
// extended customized parameter setting:
void SetPIDparamEx(uint8_t port, double P, double I, double D, double prec, int16_t regtime, double damp);                 // stop PID  

//*************************************************************                    
                       
 

void * PID_calc(void *arg) {
  double  aspeed, damp, PWMpwr, readold, errorold, tprop;
  long    readstart, cmax, cmin;                         // for monitoring
  long    starttime, runtime, clock, dtime;              // timer
  int     regloop;
  
  // arg nach index casten
  unsigned port = (unsigned)arg;

  motor[port].runstate = 0x10;                               // reg state: RAMPUP
  motor[port].read     = motor[port].motenc;                 // get current encoder reading
  motor[port].err      = motor[port].target - motor[port].read;          // error to target

  readstart      = motor[port].read;
  regloop        = 1;

  damp=0;                                 // damp the integral memory

  starttime= millis();


  // appoach target
  _Astart:
  motor[port].runstate = 0x10;                  // run state: RUNNING

  do {
    pthread_testcancel();

    dtime    = millis() - clock;
    clock    = millis();
    runtime  = clock - starttime;
    tprop    = dtime/20.0;

    if ((motor[port].err==errorold)&& (abs(motor[port].err)>motor[port].precis))
      damp=1; // stalling
    else
      damp=motor[port].damp;

    motor[port].integr = (damp * motor[port].integr) + motor[port].err;

    if((motor[port].integr) > 3*motor[port].maxout) 
      motor[port].integr = 3*motor[port].maxout; // cut away
    else if((motor[port].integr) <-3*motor[port].maxout)
      motor[port].integr =-3*motor[port].maxout;

    PWMpwr= (motor[port].P*motor[port].err) + (motor[port].I*motor[port].integr)*tprop + (motor[port].D*(motor[port].err-errorold))/tprop;


    if(PWMpwr >  motor[port].maxout)
      PWMpwr=  motor[port].maxout;   // forward maxout
    else if(PWMpwr < -motor[port].maxout)
      PWMpwr= -motor[port].maxout;   // reverse maxout

    motor[port].speed= (motor[port].read-readold)*100/dtime;  // rotat speed [degrees/100ms]

    aspeed = abs(motor[port].speed) ;

    if (aspeed > motor[port].tarpwm)  {
        PWMpwr = sign(PWMpwr)*motor[port].tarpwm;
    }

    motor[port].outp = round(PWMpwr);

     //************************************************************************
                                                 // PID regulation !
    motorOn(port, (motor[port].outp));           // action !
    delay(motor[port].regtime);                  // wait regulation time

    //**************************************************************************

    readold     = motor[port].read;              // save old sensor
    errorold    = motor[port].err;               // save old error

    motor[port].read  = motor[port].motenc;                   // get new encoder value
    motor[port].err   = motor[port].target-motor[port].read;  // new error to target

    if (motor[port].read>cmax) 
      cmax=motor[port].read;               // monitor overshooting
    else if (motor[port].read<cmin)
      cmin=motor[port].read;               // monitor overshooting

    if ((motor[port].cont)&& (abs(motor[port].err)<=motor[port].precis))
      motor[port].runstate = 0x60;
    else 
      motor[port].runstate = 0x20;

    if (motor[port].cont) 
      continue;
    if (abs(motor[port].err)<=motor[port].precis) {
      regloop +=1 ; 
      motor[port].runstate = 0x40;
    }

  } while ((abs(motor[port].err)>=motor[port].precis) && (regloop<=5));  // target reached

  motorCoast(port);                                      // finished - brake motor
  motor[port].runstate = 0x40;                           // run state: RAMPDOWN
  motor[port].outp=0;

  delay(50);
  motor[port].read = motor[port].motenc;
  regloop=1;

  if (motor[port].read>cmax) cmax=motor[port].read;      // detect overshooting
  if (motor[port].read<cmin) cmin=motor[port].read;      // detect overshooting
  motor[port].err = motor[port].target-motor[port].read;

  if ((abs(motor[port].err)>motor[port].precis)) {
    goto _Astart;
  }

  motor[port].runstate=0;
  delay(1);                                //runstate = IDLE
  
  return NULL;

}

//************************************************************************

void * PID_calc2(void *arg) {
  double  aspeed, damp, PWMpwr, readold, errorold, tprop;
  long    readstart, cmax, cmin;                         // for monitoring
  long    starttime, runtime, clock, dtime;              // timer
  int     regloop;

  // arg nach index casten
  unsigned port = (unsigned)arg;
  
  
  motor[port].runstate = 0x10;                           // reg state: RAMPUP
  motor[port].read     = motor[port].motenc;             // get current encoder reading
  motor[port].err      = motor[port].target - motor[port].read; // error to target

  readstart      = motor[port].read;
  regloop        = 1;



#ifdef debug_motor0
  //............................................................................
  // init variables for graph output
  ClearScreen();
  DisplayMask();


  int timex, oldtx, oldy=15, pwm0=15;           // values for graphic screen

  float scrXratio, scrYratio;

  scrXratio=abs(motor[port].err)/8;
  if(motor[port].target<50) scrXratio=abs(motor[port].err)/4;

  scrYratio=abs(motor[port].err)/40;
  //............................................................................
#endif

  damp=0;                                 // damp the integral memory

  starttime= millis();


  // appoach target
  _Astart:
  motor[port].runstate = 0x10;                  // run state: RUNNING

  do {

    dtime    = millis() - clock;
    clock    = millis();
    runtime  = clock - starttime;
    tprop    = dtime/20.0;

    if ((motor[port].err==errorold)&& (abs(motor[port].err)>motor[port].precis)) damp=1;    // stalling
    else
    damp=motor[port].damp;

    motor[port].integr = (damp * motor[port].integr) + motor[port].err;

    if((motor[port].integr) > 3*motor[port].maxout) motor[port].integr = 3*motor[port].maxout; // cut away
    else
    if((motor[port].integr) <-3*motor[port].maxout) motor[port].integr =-3*motor[port].maxout;

    PWMpwr= (motor[port].P*motor[port].err) + (motor[port].I*motor[port].integr)*tprop + (motor[port].D*(motor[port].err-errorold))/tprop;


    if(PWMpwr >  motor[port].maxout) PWMpwr=  motor[port].maxout;   // forward maxout
    else
    if(PWMpwr < -motor[port].maxout) PWMpwr= -motor[port].maxout;   // reverse maxout


    motor[port].speed= (motor[port].read-readold)*100/dtime;  // rotat speed [degrees/100ms]

    aspeed = abs(motor[port].speed) ;

    if (aspeed > motor[port].tarpwm)  {
        PWMpwr = sign(PWMpwr)*motor[port].tarpwm;
    }

    motor[port].outp = round(PWMpwr);

#ifdef debug_motor0
    //..........................................................................
    //  for graph output
    timex= runtime/scrXratio;
    PointOut(timex,(motor[port].read-readstart)/scrYratio);
    LineOut(oldtx, oldy, timex, pwm0+motor[port].speed*0.3);

    oldtx=timex; oldy=pwm0+motor[port].speed*0.3;
    //..........................................................................
#endif


    //**************************************************************************
                                      // PID regulation !
    motorOn(port, motor[port].outp);                  // action !
    delay(motor[port].regtime);                       // wait regulation time
   //***************************************************************************

    readold     = motor[port].read;                           // save old sensor
    errorold    = motor[port].err;                            // save old error

    motor[port].read  = motor[port].motenc;                   // get new encoder value
    motor[port].err   = motor[port].target-motor[port].read;  // new error to target

    if (motor[port].read>cmax) cmax=motor[port].read;         // monitor overshooting
    else
    if (motor[port].read<cmin) cmin=motor[port].read;         // monitor overshooting

    if ((motor[port].cont)&& (abs(motor[port].err)<=motor[port].precis)) motor[port].runstate = 0x60;
    else motor[port].runstate = 0x20;

#ifdef debug_motor0
    //..........................................................................
    printf1(68, 48,"%-5d"   , cmax);
    printf1(68, 40,"%-5d"   , cmin);
    printf1(68, 32,"%-5d"   , motor[port].read);
    //..........................................................................
#endif

    if (motor[port].cont) continue;
    if (abs(motor[port].err)<=motor[port].precis) { regloop +=1 ; motor[port].runstate = 0x40; }

  } while ((abs(motor[port].err)>=motor[port].precis) && (regloop<=5));  // target reached

  motorCoast(port);                                      // finished - brake motor
  motor[port].runstate = 0x40;                           // run state: RAMPDOWN
  motor[port].outp=0;

  delay(50);
  motor[port].read = motor[port].motenc;
  regloop=1;

  if (motor[port].read>cmax) cmax=motor[port].read;            // detect overshooting
  if (motor[port].read<cmin) cmin=motor[port].read;            // detect overshooting
  motor[port].err = motor[port].target-motor[port].read;


#ifdef debug_motor0
  //............................................................................
  printf1(68, 56,"%-5d"    , motor[port].target);
  printf1(68, 48,"%-5d"    , cmax);
  printf1(68, 40,"%-5d"    , cmin);
  printf1(68, 32,"%-5d"    , motor[port].read);
  printf1(56, 24,"P %5.2f" , motor[port].P);
  printf1(56, 16,"I %5.2f" , motor[port].I);
  printf1(56,  8,"D %5.2f" , motor[port].D);
  //............................................................................
#endif


  if ((abs(motor[port].err)>motor[port].precis))  {goto _Astart;}



#ifdef debug_motor0
  //............................................................................
  PointOut(timex,motor[port].read/scrYratio);
  LineOut(oldtx, oldy, timex, pwm0);
  LineOut(timex+2,motor[port].target/scrYratio, timex+10, motor[port].target/scrYratio);
  LineOut(timex+2, pwm0, timex+10, pwm0);
  //............................................................................
#endif

  motor[port].runstate=0;
  delay(1);                                //runstate = IDLE

  return (NULL);
}



//*************************************************************

void RotatePID(uint8_t port, long Target, float RotatSpeed, int8_t cont) {
    
    motor[port].runstate=1;                // set runstate: PID active
    // custom init PID [port]    
    motor[port].target =Target;                   // assign target
    motor[port].tarpwm =RotatSpeed;               // assign rotation speed
    motor[port].cont=cont;                        // cont vs. hit once
    // Reset PID control defaults
    motor[port].outp    =0;                // PID control output value
    motor[port].maxout  =100;              // absolute max possible output (max pwr)
    motor[port].read    =0;                // current reading
    motor[port].err     =0;                // current error
    motor[port].integr  =0;                // integral of errors
    motor[port].speed   =0;                // current speed

    pthread_create( & motor[port].tid,  // id speichern in dem dazugehörigen Array-Element
                    0,                  // Default Attribute (threads cancelbar)
                    PID_calc,           // Name der PID-Kalkulationsroutinge
                    (void*)port);       // der Index des Array-Elements für eine PID Struktur, 
                                        // die mit dem Motorindex gleich ist.   
                                        
    pthread_join(motor[port].tid, NULL);  //  <<<<<<<<<<<<<<<<<< ????
}



void StopPIDcontrol (uint8_t port, int cancel) {

  if (motor[port].tid) { // stop PID task if already running
    motorCoast(port);

    if(cancel)
       pthread_cancel(motor[port].tid);

    pthread_join(motor[port].tid, 0);
    motor[port].tid = 0; // wichtig!

    motor[port].cont=false;
    motor[port].runstate=0;
  }

}
                      
 //*************************************************************
 //  ^^^^^^^^^^^^^^ PID end ^^^^^^^^^^^^^
 //*************************************************************

    zu deinen Anmerkungen / den offenen Fragen:

    ohne das
    while(_TASKS_ACTIVE_) { delay(1); }
    läuft das Programm mit den Tasks nicht korrekt, es ist einfach "durchgerauscht", ich habe es daher nachträglich noch hinzufügen müssen.
    Das
    pthread_cancel
    wollte ich mit der neuen zusätzlichen Struktur-Variablen einsparen, denn das canceln schien mir vlt etwas zu rabiat.
    Nur so ein Gefühl.

    Es erschien mir nachträglich, alternativ, auch einfacher, fürs Stoppen nur eine Variable zu setzen, damit sich dann der Task selber beendet, als dies von außen über eine recht komplizierte Zusatzfunktion zu machen; stattdessen also nur
    Code:
    stopPID(char port) {
  motor[port].StopPIDcontrol=1;
}
    ( anders war es bei NXC, da hat die VM alles mit start und stop task für mich erledigt (ähnlich wie bei Java). )

    Wenn ich also die Variablen-/Semaphor-Methode auch für die PID-Tasks nutzen wollte: wo im Code wäre dann was aufzuräumen oder zu joinen?




    ps,
    da ich ja jetzt alle PID Threads einfach "durchlaufen lasse", sind sie ja i.P. alle "detached" -

    also käme dann doch einfach hinter alle pthread_create(& motor[port].tid,...)
    einfach ein
    pthread_detach(motor[port].tid);
    ohne weiteres join(..) ?

    - denn nun beenden sich alle immer selber (auch die, die den externen Anschubser zum Beenden gekriegt haben)
    - und können sich daher auch immer automatisch selber aufräumen - oder....?
    Geändert von HaWe (29.09.2016 um 21:18 Uhr)
    ZitierenZitieren
« Vorheriges Thema | Nächstes Thema »

Ähnliche Themen

  1. [ERLEDIGT] Abgeleitete Klasse = konkrete Klasse?
    Von vixo im Forum Software, Algorithmen und KI
    Antworten: 4
    Letzter Beitrag: 15.09.2016, 16:02
  2. doppelter widersprüchlicher Zugriff auf MotorPins durch 2 konkurr. pthread Tasks?
    Von HaWe im Forum Raspberry Pi
    Antworten: 4
    Letzter Beitrag: 02.04.2016, 14:23
  3. Task motionControl() mit der M32
    Von inka im Forum Robby RP6
    Antworten: 8
    Letzter Beitrag: 10.04.2013, 06:40
  4. Gegen-EMK-Methode
    Von MatlStg im Forum Motoren
    Antworten: 7
    Letzter Beitrag: 11.02.2008, 17:07
  5. Was ist die besser Methode? (ADC auswerten)
    Von quantum im Forum Basic-Programmierung (Bascom-Compiler)
    Antworten: 3
    Letzter Beitrag: 28.01.2007, 12:57

Lesezeichen

Berechtigungen

  • Neue Themen erstellen: Nein
  • Themen beantworten: Nein
  • Anhänge hochladen: Nein
  • Beiträge bearbeiten: Nein
  •  

Foren-Regeln


Labornetzteil AliExpress