Go Down

Topic: [gelöst:] PID library - funktioniert [-nicht-] (Read 14858 times) previous topic - next topic

HaWe

#60
Jul 07, 2014, 08:07 pm Last Edit: Jul 07, 2014, 11:39 pm by HaWe Reason: 1
ok, srry, ich kenne  mich mit Objekten und Konstrukten oder OOP absolut nicht aus. Ich verstehe also nicht, was für defaults er dann genau schreibt. Aber ich habe sicherheitshalber noch mal alle pids =0 gesetzt.

edit:

funktioniert - habe runstates eigefügt, jetzt ist er nach Ziel erreichen stille 8-)

Code: [Select]

//***********************************************************************************
// PID controller
// Reading motor encoder[0] to control PWM motor [0]
// ver 1.010
//************************************************************************************

#include <PID_v1.h>

//Define Variables we'll be connecting to
double PID0setpoint, PID0input, PID0output;

//Specify the links and initial tuning parameters
PID PID0(&PID0input, &PID0output, &PID0setpoint, 20, 70, 1.5, DIRECT);

#define  PID_REGTIME_MS  8
#define  PID_REG_PREC    2


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

#define sensortouch(pinHIGH) !digitalRead(pinHIGH)

#define startpin  12
#define MAXMOTORS  2

#define    OUT_RUNSTATE_IDLE     0
#define    OUT_RUNSTATE_ACTIVE   1

// set motor pins
// motor 0
#define pinmenc0A  2  // encA yellow
#define pinmenc0B  3  // encB blue
#define pinmot0d1  4  // dir1
#define pinmot0d2  5  // dir2
#define pinmot0pwm 6  // enable
// motor 1
#define pinmenc1A  7  // encA yellow
#define pinmenc1B  8  // encB blue
#define pinmot1d1  9  // dir1
#define pinmot1d2  10 // dir2
#define pinmot1pwm 11 // enable


//************************************************************************************
// Encoder functions courtesy of / entnommen aus: http: //www.meinDUINO.de //
//
// Globale Variablen zur Auswertung in der Interrupt-Service-Routine (ISR)
//************************************************************************************
volatile int8_t altAB[MAXMOTORS]  = {0,0};
volatile long   motenc[MAXMOTORS] = {0,0},
               oldenc[MAXMOTORS] = {0,0};
volatile int  runstate[MAXMOTORS];                
               

// Die beiden Schritt-Tabellen für 1/1, 1/2 oder 1/4-Auflösung/resolution
// 1/2 Auflösung/resolution
 int8_t schrittTab[16] = {0, 0,0,0,1,0,0,-1, 0,0,0,1,0,0,-1,0};

//*************************************************************
// Interrupt Service Routine: wenn Interrupt ausgelöst wird
//*************************************************************
ISR(TIMER1_COMPA_vect) {
 altAB[0] <<= 2;
 altAB[0] &= B00001100;
 altAB[0] |= (digitalRead(pinmenc0A) << 1) | digitalRead(pinmenc0B);
 motenc[0] += schrittTab[altAB[0]];           //

 altAB[1] <<= 2;
 altAB[1] &= B00001100;
 altAB[1] |= (digitalRead(pinmenc1A) << 1) | digitalRead(pinmenc1B);
 motenc[1] += schrittTab[altAB[1]];           //
}

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

byte pinmotdir[MAXMOTORS][2]={ {pinmot0d1, pinmot0d2},
                              {pinmot1d1, pinmot1d2} };
                                       
int  pinmotpwm[MAXMOTORS]={pinmot0pwm, pinmot1pwm};



//************************************************************************************
inline void motoron(byte motnr, int power) {
 if(power>0) {
    digitalWrite( pinmotdir[motnr][0],HIGH);
    digitalWrite( pinmotdir[motnr][1],LOW);
 }
 else
 if(power<0) {
    digitalWrite( pinmotdir[motnr][0],LOW);
    digitalWrite( pinmotdir[motnr][1],HIGH);
 }
 else
 if(power==0) {
    digitalWrite( pinmotdir[motnr][0],LOW);
    digitalWrite( pinmotdir[motnr][1],LOW);
 }

 power = abs(power);
 if(power>254) power=254;
 analogWrite(pinmotpwm[motnr],power);
}

//------------------------------------------------------------------------------------

inline void motorbrake(byte motnr) {  // to do list !!!
 motoron(motnr, 0);
}

//------------------------------------------------------------------------------------

inline void motorcoast(int motnr) {
 motoron( motnr, 0);
}

#define  motoroff    motorcoast

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

void setup()
{
 
  Serial.begin(115200);
 
  pinMode(startpin,INPUT_PULLUP);   // btn to start
 
  pinMode(pinmot0d1,OUTPUT);        // dir1
  pinMode(pinmot0d2,OUTPUT);        // dir2
  pinMode(pinmenc0A,INPUT_PULLUP);  // encA
  pinMode(pinmenc0B,INPUT_PULLUP);  // encB
  pinMode(pinmot0pwm,OUTPUT);       // enable
 
  pinMode(pinmot1d1,OUTPUT);        // dir1
  pinMode(pinmot1d2,OUTPUT);        // dir2
  pinMode(pinmenc1A,INPUT_PULLUP);  // encA
  pinMode(pinmenc1B,INPUT_PULLUP);  // encB
  pinMode(pinmot1pwm,OUTPUT);       // enable

  // time interrupt for encoder readings
  noInterrupts(); // Jetzt keine Interrupts / disable
      TIMSK1 |= (1<<OCIE1A);  // Timer 1 PIDOutput Compare A Match Interrupt Enable
      TCCR1A = 0;             // "Normaler" Modus
      // WGM12: CTC-Modus einschalten (Clear Timer on Compare match)
      //        Stimmen OCR1A und Timer überein, wird der Interrupt ausgelöst
      // Bit CS12 und CS10 setzen
      // => Prescaler=8:
      TCCR1B = (1<<WGM12) | (1<<CS11);
      // Frequenz = 16,000,000 / 8 / 512 = rd. 4 kHz
      OCR1A =511;
     
  interrupts(); // Interrupts wieder erlauben / enable
 
  //initialize the variables we're linked to
  PID0input = motenc[0];

  // set PID parameters
  PID0.SetMode(AUTOMATIC);
  PID0.SetOutputLimits(-255, 255);
  PID0.SetSampleTime(PID_REGTIME_MS);
   
  PID0setpoint = 360;                 // set target,  
  runstate[0]=OUT_RUNSTATE_ACTIVE;   // switch the PID on to motor[0]
}

char started =0;
int  cnt=0;


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

void loop()
{
  cnt++;
  if (!started) {                               // pause before start
    Serial.println("enc - output");
    Serial.print(motenc[0]);Serial.print(" - "); Serial.println(PID0output);
    delay(500);
    started=1;
  }
 
 
  PID0input = motenc[0];

  float gap = abs(PID0setpoint-PID0input); //distance away from setpoint
 
  if (runstate[0]==OUT_RUNSTATE_ACTIVE) {
     PID0.Compute();
     PID0.SetTunings(20, 70, 1.5);
  }
  else PID0output=0;
 
  if ((gap>PID_REG_PREC)&&(runstate[0]==OUT_RUNSTATE_ACTIVE)) motoron(0, PID0output);

  if(cnt>=9) {
    if( (gap<=PID_REG_PREC)&& abs(oldenc[0]-motenc[0])<1 ) {
      PID0.SetMode(MANUAL);
      PID0.SetTunings( 0,  0, 0);
      runstate[0]=OUT_RUNSTATE_IDLE;
      motoroff(0);
    }
    Serial.print(motenc[0]);Serial.print(" - "); Serial.println(PID0output);
    cnt=0;
  }
 
  oldenc[0]=motenc[0];
  delay(10);

}



Code: [Select]

enc - output
0 - 0.00
40 - 255.00
124 - 255.00
215 - 255.00
302 - 255.00
342 - 240.00
359 - 87.50
358 - 255.00
358 - 0.00
358 - 0.00
358 - 0.00
358 - 0.00
358 - 0.00
358 - 0.00
358 - 0.00
358 - 0.00
358 - 0.00
358 - 0.00
358 - 0.00
358 - 0.00
358 - 0.00
358 - 0.00
358 - 0.00
358 - 0.00
358 - 0.00



Go Up