Zmiana kodu sterowanie pozycja silnika na sterowanie prędkością silnika

[faustin]

Witam 

 Mam kod który ustawia pozycję silnika  w zależności od wartości zadanej 

 

#define ENCA 2

#define ENCB 3

#define PWM 5

#define IN2 6

#define IN1 7

 

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 = 1200;

  //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--;

  }

}

 

I próbuje zrobić tak aby zamiast pozycji była prędkość bo jak jest ten wykres to po ustaleniu zadanej pozycji silnik przestaje się już kręcić 

Edytowano Maj 14, 2022 przez faustin

[faustin]

Nie. Chodziło mi o to że wkleiłam kod z telefonu. Wgl nie wiem po co tyle wokół tego zamętu że kod mi się źle wkeil.

 Co mam zrobić z tym kodem a by był pomiar prędkości a nie pozycji dodam że korzystam z enkodera a nie czujnika prędkości próbowałam to zmieniać przeliczając połączenie na prędkość ale efekt nadal był ten sam silnik zatrzymuje się przy określonej wartości 

 

[Treker (Damian Szymański)]

Kody programów należy umieszczać przez narzędzie Wstaw kod (znajdziesz je w edytorze obok ikonki "<>"). Dzięki niemu programy są odpowiednio formatowane i kolorowane, a wtedy wszystkim znacznie łatwiej analizować wklejony kod. Proszę to poprawić - z góry dziękuję za zrozumienie i pomoc przy utrzymaniu porządku na forum 🚀

[faustin]

wiem jak wkleić kod , nie jestem tutaj nowa na forum tyle tylko nie wiem czego jak próbowałam go dodac w taki sposób jak nalezy to zrobić nie wklejiło sie w takiej formie jak trzeba tylko poza wyznaczonym miejscem  - nie wiem czy to wina mojego telefonu czy po prostu tak ustawione jest to w Waszych ustawieniach na tym forum, także teraz już pisze z laptopa i oto ten kod:

#define ENCA 2
#define ENCB 3 
#define PWM 5
#define IN2 6
#define IN1 7
 
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 = 1200;
  //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--;
  }
}

 

Edytowano Maj 14, 2022 przez faustin

[Polecacz 101]

Produkcja i montaż PCB - wybierz sprawdzone PCBWay!
   • Darmowe płytki dla studentów i projektów non-profit
   • Tylko 5$ za 10 prototypów PCB w 24 godziny
   • Usługa projektowania PCB na zlecenie
   • Montaż PCB od 30$ + bezpłatna dostawa i szablony
   • Darmowe narzędzie do podglądu plików Gerber
Zobacz również » Film z fabryki PCBWay