Sterowanie prędkością silnika (enkoder, PID)

[faustin]

witam wszystkich , chciałabym zrobic tak, aby predkosc mojego silnika z enkoderem była sterowana tak aby była trzymana stała predkosc zadana przezemnie niezaleznie od zakłócen z zewnatrz za pomoca sygnału PWM, który odpowiednio zwiekszałsieby jesli wystapiły by zakłócenia 

oto kod , który mam:

#define ENCA 2

#define ENCB 3 
#define PWM 6
#define IN2 7
#define IN1 8
 
int pos = 0;
long prevT = 0;
float eprev = 0;
float eintegral = 0;
 
void setup() {
  Serial.begin(9600);
  pinMode(ENCA,INPUT);
  pinMode(ENCB,INPUT);
  attachInterrupt(digitalPinToInterrupt(ENCA),readEncoder,RISING);
  Serial.println("target pos");
}
 
void loop() {
 
  // set target position
  int target = 100;
  //target = 250*sin(prevT/1e6);
 
  // PID constants
  float kp = 1;
  float kd = 0.025;
  float ki = 0.0;
 
  // time difference
  long currT = micros();
  float deltaT = ((float) (currT - prevT))/( 1.0e6 );
  prevT = currT;
 
  // error
  int e = pos-target;
 
  // derivative
  float dedt = (e-eprev)/(deltaT);
 
  // integral
  eintegral = eintegral + e*deltaT;
 
  // control signal
  float u = kp*e + kd*dedt + ki*eintegral;
 
  // motor power
  float pwr = fabs(u);
  if( pwr > 255 ){
    pwr = 255;
  }
 
  // motor direction
  int dir = 1;
  if(u<0){
    dir = -1;
  }
 
  // signal the motor
  setMotor(dir,pwr,PWM,IN1,IN2);
 
  // store previous error
  eprev = e;
 
  Serial.print(target);
  Serial.print(" ");
  Serial.print(pos);
  Serial.println();
}
 
void setMotor(int dir, int pwmVal, int pwm, int in1, int in2){
  analogWrite(pwm,pwmVal);
  if(dir == 1){
    digitalWrite(in1,HIGH);
    digitalWrite(in2,LOW);
  }
  else if(dir == -1){
    digitalWrite(in1,LOW);
    digitalWrite(in2,HIGH);
  }
  else{
    digitalWrite(in1,LOW);
    digitalWrite(in2,LOW);
  }  
}
 
void readEncoder(){
  int b = digitalRead(ENCB);
  if(b > 0){
    pos++;
  }
  else{
    pos--;
  }
}

tyle ,ze silnik mój kreci sie przez pewien czas , a pózniej przestaje , po chwili znów sie kreci , a jak sa zakłócenia nie widac aby pwm wzrastało

[farmaceuta]

Zadna madra rada, ale moze spróbuj z biblioteka? Widzialem kilka przykladow z jej uzyciem i bylo niby spoko..

[_LM_]

@faustin w prawdzie dla stm ale artykuły zawierają sporo informacji https://www.stm32wrobotyce.pl/kompendium-sterowania-napedami/

[faustin]

chodzi o to ze sam kod w sobie jest ok ale ja nie chce miec zadanego sygnału w postaci sygnału prostokątnego tylko chce mieć  na wyjściu predkośc zadana i chce aby była ona utrzymywana , a tam w kodzie jest jakby ustawiona zadana pozycja  a nie prędkość stąd pytanie jak ten kod zmodyfikować, przesyłam link do filmu DC motor PID speed control - Bing video

[piotr96]

Nie jestem pewien, czy dobrze zrozumiałem problem.
Jeżeli chcesz sterować prędkością zamiast pozycją, to musisz:
1. Obliczyć aktualną prędkość – skoro masz pozycję, to prędkość możesz spróbować w najprostszym przypadku oszacować jako różnicę aktualnej pozycji oraz poprzedniej podzielonej przez przyrost czasu (trzeba zapamiętać pozycję z poprzedniego obiegu pętli).
2. Liczyć uchyb układu regulacji jako różnicę prędkości zadanej oraz aktualnej (obliczonej w punkcie 1) zamiast różnicy pozycji.

 

[faustin]

To jak by wyglądały takie zmiany w tym kodzie którym mam?

[piotr96]

Generalnie coś takiego w miejscu liczenia uchybu:

float currentSpeed = (pos - prevPos)/deltaT;
// error
float e = targetSpeed - currentSpeed;
prevPos = pos;

Tę zmienną prevPos trzeba gdzieś też wcześniej zadeklarować, najlepiej tam, gdzie pos. Jako targetSpeed można pewnie użyć tej zmiennej, która już tam była w kodzie (zwróć jedynie uwagę na typ, czy potrzeba floatów tak, jak to zrobiłem, czy wystarczą Ci liczby całkowite).
Nie chce mi się analizować całego kodu dokładnie, dziwi mnie, że uchyb liczysz jako różnicę wartości aktualnej i zadanej (zamiast odwrotnie). Ogólnie to dobrze też byłoby, gdyby PID działał ze stałym okresem próbkowania.
 

[faustin]

Spróbuję i dam znać dziękuję