}
int main(void) {
SysCtlClockSet(SYSCTL_SYSDIV_1 | SYSCTL_USE_OSC | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN); ADC_INT(); RIT128x96x4Init(1000000); RIT128x96x4StringDraw("ADCX", 0, 0, 15); while(1){ Read_ADC(); RIT128x96x4UDecOut3(Data_ADC_1, 30, 0, 15); } }
3.4 Sơ đồ khối điều khiển động cơ
Hình 3.5 Sơ đồ khối điều khiển động cơ.
-Biến flag suy ra từ biến Vantoc ở chương trình điều khiển bằng thuật toán PID.
• Nếu biến Vantoc < 0 thì flag=1 thì động cơ sẽ quay tới.
• Ngược lai, biến Vantoc > 0 thì flag =0 thì động cơ sẽ quay lui.
-Biến period được gọi là độ rộng xung(-10.000 < Period < 10.000) đặc trưng cho tốc độ nhanh chậm của động cơ được tính bằng công thức:
Period=flag*10.000 + Vantoc. Vi điều khiển nhận tín hiệu điều khiển động cơ
Cấu hình chân PF3 và chân PD2 là chân điều khiển quay tới, lui cho động cơ thông qua biến flag(0, 1)
Begin
Flag=0
Xuất tín hiệu điều khiển động cơ quay nhanh hay chậm đến chân PF2 và chân PD1 thông qua biến Period
Cho phép bộ PWM hoạt động đưa tín hiệu độ rộng xung đến mạch cầu H
L6203 động cơ quay tới
Flag=1
Chương trình điều khiển động cơ #include "inc/hw_ints.h" #include "inc/hw_memmap.h" #include "inc/hw_types.h" #include "driverlib/debug.h" #include "driverlib/gpio.h" #include "driverlib/pwm.h" #include "driverlib/sysctl.h" #include "driverlib/interrupt.h" #include "drivers/rit128x96x4.h" #include "driverlib/timer.h" #include "driverlib/adc.h" #include "driverlib/flash.h" #include "stdlib.h" #include "stdio.h" #include "float.h" #include <math.h> //dinh nghia dc 1
#define MOTOR_PORT GPIO_PORTF_BASE #define MOTOR_PORT_INT INT_GPIOF
#define MOTOR_PORT3 GPIO_PORTH_BASE #define MOTOR_PORT_ENABLE SYSCTL_PERIPH_GPIOF #define PIN_PWM GPIO_PIN_2
#define TURN_LR GPIO_PIN_3 #define LEFT 0
#define RIGHT 8
#define LED GPIO_PIN_2 //dinh nghia dc 2
#define MOTOR_PORT1 GPIO_PORTD_BASE #define MOTOR_PORT2 GPIO_PORTG_BASE #define MOTOR_PORT_ENABLE1 SYSCTL_PERIPH_GPIOD #define MOTOR_PORT_ENABLE2 SYSCTL_PERIPH_GPIOG #define PIN_PWM1 GPIO_PIN_1
#define TURN_LR1 GPIO_PIN_3 #define ulPeriod SysCtlClockGet() #ifdef DEBUG
Void
__error__(char *pcFilename, unsigned long ulLine) {
} #endif
void update_motor_param(void) {
if(van_toc<0) dc1_flag = 0; else dc1_flag = 1; if (van_toc == 0) dc2_flag = 0; else { dc2_flag = 1 - dc1_flag; } KK2=(1-dc1_flag)*ulPeriod + van_toc; // KK1=ulPeriod - KK2; } /*****************************/ // chuong trinh cai dat MOTOR void Motor_Init()
{ //DC1
SysCtlPeripheralEnable(MOTOR_PORT_ENABLE); GPIOPinTypeGPIOOutput(MOTOR_PORT, TURN_LR); GPIOPinWrite(MOTOR_PORT, TURN_LR, dc2_flag*8); SysCtlPWMClockSet(SYSCTL_PWMDIV_1);
SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM); GPIOPinTypePWM(MOTOR_PORT, PIN_PWM); PWMGenConfigure(PWM_BASE, PWM_GEN_2,
PWM_GEN_MODE_UP_DOWN | PWM_GEN_MODE_NO_SYNC); PWMGenPeriodSet(PWM_BASE, PWM_GEN_2, ulPeriod);
PWMPulseWidthSet(PWM_BASE, PWM_OUT_4, KK2); PWMOutputState(PWM_BASE, PWM_OUT_4_BIT, true); PWMGenEnable(PWM_BASE, PWM_GEN_2);
//DC2
SysCtlPeripheralEnable(MOTOR_PORT_ENABLE1); SysCtlPeripheralDisable(MOTOR_PORT_ENABLE2); GPIOPinTypeGPIOOutput(MOTOR_PORT1, TURN_LR); GPIOPinWrite(MOTOR_PORT1, TURN_LR, dc2_flag*8); SysCtlPWMClockSet(SYSCTL_PWMDIV_1);
SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM); GPIOPinTypePWM(MOTOR_PORT1, PIN_PWM1); PWMGenConfigure(PWM_BASE, PWM_GEN_0,
PWM_GEN_MODE_UP_DOWN | PWM_GEN_MODE_NO_SYNC); PWMGenPeriodSet(PWM_BASE, PWM_GEN_0, ulPeriod );
PWMPulseWidthSet(PWM_BASE, PWM_OUT_1, KK2); PWMOutputState(PWM_BASE, PWM_OUT_1_BIT, true); PWMGenEnable(PWM_BASE, PWM_GEN_0);
}
int main(void) {
SysCtlClockSet(SYSCTL_SYSDIV_1 | SYSCTL_USE_OSC | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN);
van_toc = 10000; // do rong xung PWM từ -1000010000 MHZ
Sau khi thiết kế thi công các khối mạch và xây dựng phần mềm ta tiến hành thiết kế và thi công phần cơ khí cho robot. Sau đó sẽ lắp ráp hệ thống và kiểm tra hoạt động thực nghiệm của xe 2 bánh tự cân bằng.
3.5 Quá trình thực nghiệm
Ở đây, ta sử dụng mô hình robot 2 bánh. Như trong phần áp dụng thuật toán PID cho xe hai bánh tự cân bằng, bài toán đặt ra là làm sao điều khiển 2 bánh xe để nó tự di chuyển trên mặt phẳng và luôn giữ cho góc lệch giữ thân xe và mặt sàn là 90o (góc lệch so với vị trí cân bằng là 0o).
Thực tế với thông số từ cảm biến gia tốc thì ta biết được vị trí cân bằng hiện tại của xe là 553 tương đương với góc 0o của thân xe so với vị trí cân bằng. Căn cứ vào thông số đó ta suy ra được độ lệch của thân xe so với vị trí cân bằng:
Error = ulTem_ValueC - 553. + ulTem_ValueC: là giá trị đọc về từ cảm biến. + Error < 0 : xe đang ngã về phía sau.
+ Error > 0 : xe đang ngã về phía trước. + Error = 0 : xe ở vị trí cân bằng.
Từ thông số độ lệch đó ta điều khiển động cơ của 2 bánh tuyến tính theo góc nghiêng của xe, khi xe có góc nghiêng lớn so với vị trí cân bằng, hay nói cách khác độ lệch (error) lớn, chúng ta cần điều khiển động cơ quay nhanh hơn (theo hướng ngã) để nhanh chóng đưa xe về vị trí cân bằng và khi độ lệch (error) nhỏ thì ta phải điều khiển động cơ quay chậm trở lại. Một cách đơn giản để công thức hóa việc này là dùng quan hệ tuyến tính P:
Vantoc = Kp* Error.
Sau nhiều lần thử nghiệm tôi chọn Kp = 280. Vì giá trị vận tốc có giới hạn là từ -10.000 < Vantoc < 10.000.
+ Vantoc < 0 : xe chạy về phía sau. + Vantoc > 0 : xe chạy về phía trước.
+ Vantoc = 0 : xe đứng yên.
Rõ ràng nếu Kp lớn thì giá trị Vantoc cũng sẽ lớn và xe rất nhanh chóng tiến về vị trí cân bằng. Tuy nhiên, Vantoc quá lớn sẽ làm cho gia tốc cho xe rất nhanh. Khi xe đã đến vị trí cân bằng (tức Error = 0), thì tuy Vantoc = 0 (vì Vantoc = Kp* Error) nhưng do quán tính xe vẫn tiếp tục tiến về hướng đang chạy và lệch vị trí cân bằng, sai số e lại trở nên khác 0, giá trị sai số lúc này được gọi là overshot (độ vọt lố). Quá trình này cứ tiếp diễn cho đến khi xe ngã do góc nghiêng quá lớn.
Tiếp theo, dựa vào thuật toán PID tôi đã thêm thành phần D ( vi phân) để loại bỏ độ vọt lố.
Vantoc = Kp*Error + Kd*D_value