엔코딩모터 control

// motor control pin

const int motorDirPin1 = 8; // L298N Input 

const int motorPWMPin = 9; // PWM

// encoder pin

const int encoderPinA = 2;

const int encoderPinB = 3;

long encoderPos = 0;

const double ratio = 360./104./167.; // 모터 엔코더 분해능 지정(334Pulse 를 입력하면 멈추지 않고 구동됨..)

// PID control

double kp = 2.5;        

double ki = 1.2;

double kd = 0.9;

double targetDeg = 360; // 목표 각도

double error_previous;

double PID_control;

//엔코더 신호

void doEncoderA(){  encoderPos += (digitalRead(encoderPinA)==digitalRead(encoderPinB))?1:-1;}

void doEncoderB(){  encoderPos += (digitalRead(encoderPinA)==digitalRead(encoderPinB))?-1:1;}

double P_control, I_control, D_control;

// 모터 제어

void doMotor(bool dir, int vel){

  digitalWrite(motorDirPin1, dir);

  analogWrite(motorPWMPin, dir?(255 - vel):vel);

}

void setup() {

  pinMode(encoderPinA, INPUT_PULLUP);

  attachInterrupt(0, doEncoderA, CHANGE);

  

  pinMode(encoderPinB, INPUT_PULLUP);

  attachInterrupt(1, doEncoderB, CHANGE);

  

  pinMode(motorDirPin1, OUTPUT);

 // pinMode(motorDirpin2, OUTPUT);

  Serial.begin(115200);

}

void loop() {

  double motorDeg = double(encoderPos)*ratio; 

  

  double error = targetDeg - motorDeg;

  double P_control = kp * error;

  double I_control = I_control + ki * error;

  double D_control = kd * (error - error_previous);

  PID_control = P_control + I_control + D_control;

  PID_control = constrain(PID_control, 0, 255);

  double control = PID_control; 

  doMotor( (control>=0)?HIGH:LOW, min(abs(control), 55));

  Serial.print("encoderPos : ");

  Serial.print(encoderPos);

  Serial.print("   motorDeg : ");

  Serial.print(float(encoderPos)*ratio);

  Serial.print("   error : ");

  Serial.print(error);

  Serial.print("    control : ");

  Serial.print(control);

  Serial.print("    motorVel : ");

  Serial.println(min(abs(control), 255)); 

}

[출처] DC 모터 제어 질문 있습니다! 감이 안잡힙니다.ㅠㅠㅠ (아두이노 스토리) |작성자 미밍리