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.
Mục lục
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ơ.
Step 1. Hardware connection
(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.
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 đồ.
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
}