Home Internet of Things - IoT Arduino – điều khiển tốc độ Motor với PID

Arduino – điều khiển tốc độ Motor với PID

by Khanh Tran

Với điều khiển PID, tốc độ của động cơ có thể được điều khiển một cách chính xác. Trong bài viết này tôi sẽ nói về cách xây dựng một chương trình cho Arduino Pro Mini để điều khiển tốc độ động cơ với thuật toán PID.

Hình 1: Mô hình dự án

Chuẩn bị

1. Motor with encoder

2. H-bridge

3. Arduino Pro Mini

4. UART PCB

Giới thiệu chung

Arduino Pro Mini được sử dụng để lưu và tinh chỉnh dữ liệu điều khiển motor, thuật toán PID và giao tiếp với PC thông qua cổng COM. Chúng ta đồng thời sẽ xây dựng phần UI trên PC (viết trên Visual Studio) để có thể giao tiếp với Arduino.

Giao diện sẽ hiển thị sự thay đổi tốc độ động cơ và có phần setting tốc độ động cơ.

Fig2. HMI on Computer (made by Visual Studio)
Hình 2: Điều khiển tốc độ động cơ với PID trên Arduino

Step 1. Hardware connection

Hình 3: Mô tả kết nối

(1) Connect encoder to Arduino

Encoder có 2 xung A, B -> kết nối Arduino, đừng quên nối nguồn cho chúng. Chúng ta cần kết nối 4 dây từ motor encoder đến Arduino:

Kết nối Arduino pin 2 <-> Encoder pulse A.
Arduino pin 3 <-> Encoder pulse B.
Kết nối Arduino VCC <-> Encoder power.
Arduino GND <-> Encoder GND.

(2) Connect to H-bridge module

H-bridge thường được dùng để điều khiển tốc độ động cơ. H-bridge trong bài này, chúng ta sẽ sử dụng chip L298N.

Hình 4: H-bridge

Kết nối Arduino pin 4 <-> H-bridge “forward”
Arduino pin 5 <-> H-bridge “backward”
Và Arduino pin 6 <-> H-bridge “Enable”

Tất nhiên rằng đầu ra H-bridge PCB sẽ có kết nối với motor. Và đây cũng là nguồn cấp cho nó. Trong trường hợp này, nguồn cấp cho chip L298N là nguồn 12V và cũng bằng với nguồn cấp cho motor.

Step 2. Code with the Arduino

void loop() {
   if (stringComplete) {
     // clear the string when COM receiving is completed
     mySt = "";  //note: in code below, mySt will not become blank, mySt is blank until '\n' is received
     stringComplete = false;
   }
  //receive command from Visual Studio
   if (mySt.substring(0,8) == "vs_start"){
     digitalWrite(pin_fwd,1);      //run motor run forward
     digitalWrite(pin_bwd,0);
     motor_start = true;
   }
   if (mySt.substring(0,7) == "vs_stop"){
     digitalWrite(pin_fwd,0);
     digitalWrite(pin_bwd,0);      //stop motor
     motor_start = false;
   }
   if (mySt.substring(0,12) == "vs_set_speed"){
     set_speed = mySt.substring(12,mySt.length()).toFloat();  //get string after set_speed
   }
   if (mySt.substring(0,5) == "vs_kp"){
     kp = mySt.substring(5,mySt.length()).toFloat(); //get string after vs_kp
   }
   if (mySt.substring(0,5) == "vs_ki"){
     ki = mySt.substring(5,mySt.length()).toFloat(); //get string after vs_ki
   }
   if (mySt.substring(0,5) == "vs_kd"){
     kd = mySt.substring(5,mySt.length()).toFloat(); //get string after vs_kd
   } 
 }
void detect_a() {
   encoder+=1; //increasing encoder at new pulse
   m_direction = digitalRead(pin_b); //read direction of motor
 }
 ISR(TIMER1_OVF_vect)        // interrupt service routine - tick every 0.1sec
 {
   TCNT1 = timer1_counter;   // set timer
   pv_speed = 60.0*(encoder/200.0)/0.1;  //calculate motor speed, unit is rpm
   encoder=0;
   //print out speed
   if (Serial.available() <= 0) {
     Serial.print("speed");
     Serial.println(pv_speed);         //Print speed (rpm) value to Visual Studio
     }
  //PID program
   if (motor_start){
     e_speed = set_speed - pv_speed;
     pwm_pulse = e_speed*kp + e_speed_sum*ki + (e_speed - e_speed_pre)*kd;
     e_speed_pre = e_speed;  //save last (previous) error
     e_speed_sum += e_speed; //sum of error
     if (e_speed_sum >4000) e_speed_sum = 4000;
     if (e_speed_sum <-4000) e_speed_sum = -4000;
   }
   else{
     e_speed = 0;
     e_speed_pre = 0;
     e_speed_sum = 0;
     pwm_pulse = 0;
   }
  //update new speed
   if (pwm_pulse <255 & pwm_pulse >0){
     analogWrite(pin_pwm,pwm_pulse);  //set motor speed 
   }
   else{
     if (pwm_pulse>255){
       analogWrite(pin_pwm,255);
     }
     else{
       analogWrite(pin_pwm,0);
     }
   }
}

Bắt đầu chương trình sẽ nhận lệnh từ máy tính (start/stop motor; motor speed settings; kP, kI, kD của thuật toán PID). Tiếp theo là hàm void detect_a(): được sử dụng để tính toán tốc độ trong quy trình ngắt Timer.

Timer interrupt ISR(TIMER1_OVF_vect): sẽ được gọi mỗi 0.1s, nội dung hàm ngắt này bao gồm:
(1) Tính toán motor speed
(2) Gửi motor speed đến computer
(3) Tính toán xung PWM (base on PID algorithm)
(4) Gửi kết quả PWM đến H-brigde.

Full code for Arduino Pro mini:

/*
  Motor - PID speed control
  (1) Receive command from Visual Studio (via COM4): set_speed, kP, kI, kD
  (2) Control motor speed through PWM (PWM is base on PID calculation)
  (3) Send pv_speed to Visual Studio -> show in graph
  
 Created 31 Dec. 2016
 This example code is in the public domain.
 */
String mySt = "";
char myChar;
boolean stringComplete = false;  // whether the string is complete
boolean motor_start = false;
const byte pin_a = 2;   //for encoder pulse A
const byte pin_b = 3;   //for encoder pulse B
const byte pin_fwd = 4; //for H-bridge: run motor forward
const byte pin_bwd = 5; //for H-bridge: run motor backward
const byte pin_pwm = 6; //for H-bridge: motor speed
int encoder = 0;
int m_direction = 0;
int sv_speed = 100;     //this value is 0~255
double pv_speed = 0;
double set_speed = 0;
double e_speed = 0; //error of speed = set_speed - pv_speed
double e_speed_pre = 0;  //last error of speed
double e_speed_sum = 0;  //sum error of speed
double pwm_pulse = 0;     //this value is 0~255
double kp = 0;
double ki = 0;
double kd = 0;
int timer1_counter; //for timer
int i=0;


void setup() {
  pinMode(pin_a,INPUT_PULLUP);
  pinMode(pin_b,INPUT_PULLUP);
  pinMode(pin_fwd,OUTPUT);
  pinMode(pin_bwd,OUTPUT);
  pinMode(pin_pwm,OUTPUT);
  attachInterrupt(digitalPinToInterrupt(pin_a), detect_a, RISING);
  // start serial port at 9600 bps:
  Serial.begin(9600);
  //--------------------------timer setup
  noInterrupts();           // disable all interrupts
  TCCR1A = 0;
  TCCR1B = 0;
  timer1_counter = 59286;   // preload timer 65536-16MHz/256/2Hz (34286 for 0.5sec) (59286 for 0.1sec)

  
  TCNT1 = timer1_counter;   // preload timer
  TCCR1B |= (1 << CS12);    // 256 prescaler 
  TIMSK1 |= (1 << TOIE1);   // enable timer overflow interrupt
  interrupts();             // enable all interrupts
  //--------------------------timer setup
  
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }

  analogWrite(pin_pwm,0);   //stop motor
  digitalWrite(pin_fwd,0);  //stop motor
  digitalWrite(pin_bwd,0);  //stop motor
}

void loop() {
  if (stringComplete) {
    // clear the string when COM receiving is completed
    mySt = "";  //note: in code below, mySt will not become blank, mySt is blank until '\n' is received
    stringComplete = false;
  }

  //receive command from Visual Studio
  if (mySt.substring(0,8) == "vs_start"){
    digitalWrite(pin_fwd,1);      //run motor run forward
    digitalWrite(pin_bwd,0);
    motor_start = true;
  }
  if (mySt.substring(0,7) == "vs_stop"){
    digitalWrite(pin_fwd,0);
    digitalWrite(pin_bwd,0);      //stop motor
    motor_start = false;
  }
  if (mySt.substring(0,12) == "vs_set_speed"){
    set_speed = mySt.substring(12,mySt.length()).toFloat();  //get string after set_speed
  }
  if (mySt.substring(0,5) == "vs_kp"){
    kp = mySt.substring(5,mySt.length()).toFloat(); //get string after vs_kp
  }
  if (mySt.substring(0,5) == "vs_ki"){
    ki = mySt.substring(5,mySt.length()).toFloat(); //get string after vs_ki
  }
  if (mySt.substring(0,5) == "vs_kd"){
    kd = mySt.substring(5,mySt.length()).toFloat(); //get string after vs_kd
  }  
}

void detect_a() {
  encoder+=1; //increasing encoder at new pulse
  m_direction = digitalRead(pin_b); //read direction of motor
}
ISR(TIMER1_OVF_vect)        // interrupt service routine - tick every 0.1sec
{
  TCNT1 = timer1_counter;   // set timer
  pv_speed = 60.0*(encoder/200.0)/0.1;  //calculate motor speed, unit is rpm
  encoder=0;
  //print out speed
  if (Serial.available() <= 0) {
    Serial.print("speed");
    Serial.println(pv_speed);         //Print speed (rpm) value to Visual Studio
    }


  //PID program
  if (motor_start){
    e_speed = set_speed - pv_speed;
    pwm_pulse = e_speed*kp + e_speed_sum*ki + (e_speed - e_speed_pre)*kd;
    e_speed_pre = e_speed;  //save last (previous) error
    e_speed_sum += e_speed; //sum of error
    if (e_speed_sum >4000) e_speed_sum = 4000;
    if (e_speed_sum <-4000) e_speed_sum = -4000;
  }
  else{
    e_speed = 0;
    e_speed_pre = 0;
    e_speed_sum = 0;
    pwm_pulse = 0;
  }
  

  //update new speed
  if (pwm_pulse <255 & pwm_pulse >0){
    analogWrite(pin_pwm,pwm_pulse);  //set motor speed  
  }
  else{
    if (pwm_pulse>255){
      analogWrite(pin_pwm,255);
    }
    else{
      analogWrite(pin_pwm,0);
    }
  }
  
}
void serialEvent() {
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read();
    // add it to the inputString:
    if (inChar != '\n') {
      mySt += inChar;
    }
    // if the incoming character is a newline, set a flag
    // so the main loop can do something about it:
    if (inChar == '\n') {
      stringComplete = true;
    }
  }
}

Step 3. Code on the computer

Visual Studio 2012 được sử dụng để tạo HMI programs: (1) Gửi dữ liệu setting đến Arduino    (2) gửi thông số PID (kP, kI, kD) to Arduino (3) Nhận tốc độ motor -> Hiển thị biểu đồ.

Fig3. Visual studio program
Hình 5: Visual Studio cho dự án.

Bạn có thể tải toàn bộ code cho chương trình tại đây.

#pragma endregion
 private: System::Void Form1_Load(System::Object^  sender, System::EventArgs^  e) {
 serialPort1->Open();
 timer1->Start();
 mStr = "0";
 i=300;
 }
 private: System::Void button1_Click(System::Object^  sender, System::EventArgs^  e) {
 serialPort1->WriteLine("vs_set_speed"+textBox1->Text); //send set_speed to Arduino
 serialPort1->WriteLine("vs_kp"+textBox2->Text); //send kP to Arduino
 serialPort1->WriteLine("vs_ki"+textBox3->Text); //send kI to Arduino
 serialPort1->WriteLine("vs_kd"+textBox4->Text); //send kD to Arduino
 }
 private: System::Void timer1_Tick(System::Object^  sender, System::EventArgs^  e) {
 String^ length;
 length=mStr->Length.ToString();
 if(mStr->Substring(0,5)=="speed"){
 speed=mStr->Substring(5,System::Convert::ToInt32(length)-6);
 label1->Text=speed;
 //print motor speed into Chart
 this->chart1->Series["Series1"]->Points->AddXY(i,System::Convert::ToDouble(speed));
 i++;
 this->chart1->ChartAreas["ChartArea1"]->AxisX->Minimum=i-300; //shift x-axis
 }
 }
 private: System::Void serialPort1_DataReceived(System::Object^  sender, System::IO::Ports::SerialDataReceivedEventArgs^  e) {
 mStr=serialPort1->ReadLine();
 }
 private: System::Void button2_Click(System::Object^  sender, System::EventArgs^  e) {
 serialPort1->WriteLine("vs_start"); //start motor
 }
 private: System::Void button3_Click(System::Object^  sender, System::EventArgs^  e) {
 serialPort1->WriteLine("vs_stop");  //stop motor
 }

You may also like

Leave a Comment