Kết nối phần cứng

Điều khiển quay thuận/nghịch đơn giản

#include "reg52.h"
#include "intrins.h"

sbit MOTOR_PWM = P1^2;
sbit DIR_A = P1^1;
sbit DIR_B = P1^0;

#define QUAY_THUAN   { DIR_A = 0; DIR_B = 1; }
#define QUAY_NGHICH  { DIR_A = 1; DIR_B = 0; }

void tre_ms(unsigned int ms) {
    unsigned char i, j;
    while(ms--) {
        _nop_();
        i = 2; j = 199;
        do { while (--j); } while (--i);
    }
}

void main() {
    MOTOR_PWM = 1;
    while(1) {
        QUAY_THUAN;
        tre_ms(500);
        QUAY_NGHICH;
        tre_ms(500);
    }
}

Điều khiển chiều quay và tốc độ bằng PWM

#include "reg52.h"
#include "intrins.h"

sbit PWM_OUT = P1^2;
sbit CHIEU_A = P1^1;
sbit CHIEU_B = P1^0;

#define XOA_CHIEU   { CHIEU_A = 0; CHIEU_B = 1; }
#define DAO_CHIEU   { CHIEU_A = 1; CHIEU_B = 0; }

unsigned char ty_le_pwm = 0;

void khoi_tao_timer0(void) {
    TMOD = (TMOD & 0xF0) | 0x01;
    TL0 = 0x66; TH0 = 0xFC;
    TR0 = 1; ET0 = 1;
}

void delay_ms(unsigned int ms) {
    unsigned char i, j;
    while(ms--) {
        _nop_();
        i = 2; j = 199;
        do { while (--j); } while (--i);
    }
}

void main() {
    khoi_tao_timer0();
    ty_le_pwm = 30;
    EA = 1;
    while(1) {
        XOA_CHIEU; delay_ms(500);
        DAO_CHIEU; delay_ms(500);
    }
}

void ngat_timer0(void) interrupt 1 {
    static unsigned int dem = 0;
    TL0 = 0x66; TH0 = 0xFC;
    PWM_OUT = (dem < ty_le_pwm) ? 1 : 0;
    if(++dem >= 100) dem = 0;
}

Điều khiển vòng kín PID với cảm biến encoder

#include "reg52.h"
#include "intrins.h"

typedef struct {
    unsigned char han_che_tich_phan, han_che_ngoa_ra;
    float Kp, Ki, Kd, gioi_han_tich_phan, max_out, min_out;
    float tong_loi, loi_truoc, hien_tai;
} Cau_truc_PID;

sbit TIN_HIEU_PWM = P1^2;
sbit CHAN_A = P1^1;
sbit CHAN_B = P1^0;
sbit ENCODER_A = P3^2;
sbit ENCODER_B = P3^3;

#define QUAY_PHAI   { CHAN_A = 0; CHAN_B = 1; }
#define QUAY_TRAI   { CHAN_A = 1; CHAN_B = 0; }

unsigned char gia_tri_pwm = 0;
int xung_A = 0, xung_B = 0;
bit cap_nhat_toc_do = 0;
Cau_truc_PID bo_dieu_khien;

float tri_tuyet_doi(float x) { return (x < 0) ? -x : x; }

int tinh_toan_pid(Cau_truc_PID* pid, float muc_tieu, float thuc_te) {
    float loi = muc_tieu - thuc_te;
    pid->tong_loi += loi;

    if(pid->han_che_tich_phan && tri_tuyet_doi(pid->tong_loi) > pid->gioi_han_tich_phan) {
        pid->tong_loi = (pid->tong_loi > 0) ? pid->gioi_han_tich_phan : -pid->gioi_han_tich_phan;
    }

    float ket_qua = pid->Kp * loi + pid->Ki * pid->tong_loi + pid->Kd * (loi - pid->loi_truoc);

    if(pid->han_che_ngoa_ra) {
        if(ket_qua > pid->max_out) ket_qua = pid->max_out;
        if(ket_qua < pid->min_out) ket_qua = pid->min_out;
    }

    pid->loi_truoc = loi;
    return (int)ket_qua;
}

float doc_toc_do(void) {
    static float toc_do = 0;
    if(cap_nhat_toc_do) {
        cap_nhat_toc_do = 0;
        toc_do = xung_A + xung_B;
        xung_A = xung_B = 0;
    }
    return toc_do;
}

void khoi_tao_ngat_ngoai_0(void) {
    IT0 = 1; EX0 = 1;
}

void khoi_tao_ngat_ngoai_1(void) {
    IT1 = 1; EX1 = 1;
}

void khoi_tao_timer_chinh(void) {
    TMOD = (TMOD & 0xF0) | 0x01;
    TL0 = 0x18; TH0 = 0xFC;
    TR0 = 1; ET0 = 1;
}

void main() {
    khoi_tao_timer_chinh();
    khoi_tao_ngat_ngoai_0();
    khoi_tao_ngat_ngoai_1();
    QUAY_PHAI;

    bo_dieu_khien.han_che_tich_phan = 1;
    bo_dieu_khien.han_che_ngoa_ra = 1;
    bo_dieu_khien.gioi_han_tich_phan = 2000.0f;
    bo_dieu_khien.max_out = 70.0f;
    bo_dieu_khien.min_out = 0.0f;
    bo_dieu_khien.Kp = 3.0f;
    bo_dieu_khien.Ki = 0.01f;
    bo_dieu_khien.Kd = 0.0f;

    EA = 1;
    while(1) {
        gia_tri_pwm = tinh_toan_pid(&bo_dieu_khien, 10.0f, doc_toc_do());
    }
}

void ngat_timer_chinh(void) interrupt 1 {
    static unsigned int chu_ky = 0;
    TL0 = 0x18; TH0 = 0xFC;
    TIN_HIEU_PWM = (chu_ky < gia_tri_pwm) ? 1 : 0;
    if(++chu_ky >= 100) {
        chu_ky = 0;
        cap_nhat_toc_do = 1;
    }
}

void ngat_encoder_A(void) interrupt 0 {
    if(ENCODER_A == 0 && ENCODER_B == 1) xung_A++;
    if(ENCODER_A == 1 && ENCODER_B == 0) xung_A--;
}

void ngat_encoder_B(void) interrupt 2 {
    if(ENCODER_A == 0 && ENCODER_B == 1) xung_B++;
    if(ENCODER_A == 1 && ENCODER_B == 0) xung_B--;
}

Điều khiển từ xa qua UART

#include "reg52.h"
#include "intrins.h"

typedef struct {
    unsigned char han_che_tich_phan, han_che_ngoa_ra;
    float Kp, Ki, Kd, gioi_han_tich_phan, max_out, min_out;
    float tong_loi, loi_truoc, hien_tai;
} PID_Struct;

sbit PWM_CHAN = P1^2;
sbit CHAN_DAU = P1^1;
sbit CHAN_CUOI = P1^0;
sbit CAM_BIEN_A = P3^2;
sbit CAM_BIEN_B = P3^3;

#define BAT_QUAY   { CHAN_DAU = 0; CHAN_CUOI = 1; }
#define NGUNG_QUAY { CHAN_DAU = 1; CHAN_CUOI = 1; }

unsigned char pwm_output = 0;
int dem_xung_A = 0, dem_xung_B = 0;
bit cho_phep_pid = 0, can_lam_moi = 0;
PID_Struct bo_pid;
unsigned char du_lieu_vao[5];
unsigned char du_lieu_ra[4] = {0xA5, 0x00, 0x00, 0x5A};

float abs_float(float val) { return val < 0 ? -val : val; }

int xu_ly_pid(PID_Struct* ctrl, float dat, float thuc) {
    float error = dat - thuc;
    ctrl->tong_loi += error;

    if(ctrl->han_che_tich_phan && abs_float(ctrl->tong_loi) > ctrl->gioi_han_tich_phan) {
        ctrl->tong_loi = (ctrl->tong_loi > 0) ? ctrl->gioi_han_tich_phan : -ctrl->gioi_han_tich_phan;
    }

    float out = ctrl->Kp * error + ctrl->Ki * ctrl->tong_loi + ctrl->Kd * (error - ctrl->loi_truoc);

    if(ctrl->han_che_ngoa_ra) {
        if(out > ctrl->max_out) out = ctrl->max_out;
        if(out < ctrl->min_out) out = ctrl->min_out;
    }

    ctrl->loi_truoc = error;
    return (int)out;
}

int lay_toc_do(void) {
    int speed = dem_xung_A + dem_xung_B;
    dem_xung_A = dem_xung_B = 0;
    return speed;
}

void gui_byte(unsigned char data) {
    SBUF = data;
    while(!TI);
    TI = 0;
}

void khoi_tao_uart(void) {
    PCON &= 0x7F;
    SCON = 0x50;
    TMOD = (TMOD & 0x0F) | 0x20;
    TL1 = TH1 = 0xFD;
    TR1 = 1; ES = 1;
}

void main() {
    int toc_do_hien_tai, toc_do_dat;
    khoi_tao_timer_chinh();
    khoi_tao_ngat_ngoai_0();
    khoi_tao_ngat_ngoai_1();
    khoi_tao_uart();

    bo_pid.han_che_tich_phan = 1;
    bo_pid.han_che_ngoa_ra = 1;
    bo_pid.gioi_han_tich_phan = 2000.0f;
    bo_pid.max_out = 70.0f;
    bo_pid.min_out = 0.0f;
    bo_pid.Kp = 3.0f;
    bo_pid.Ki = 0.01f;
    bo_pid.Kd = 0.0f;

    EA = 1;
    while(1) {
        if(du_lieu_vao[1]) {
            if(du_lieu_vao[2]) BAT_QUAY;
            else NGUNG_QUAY;
            toc_do_dat = du_lieu_vao[3];
        } else {
            NGUNG_QUAY;
            toc_do_dat = 0;
        }

        if(can_lam_moi) {
            toc_do_hien_tai = lay_toc_do();
            du_lieu_ra[1] = (unsigned char)abs_float(toc_do_hien_tai);
            du_lieu_ra[2] = du_lieu_ra[1];
            for(int i=0; i<4; i++) gui_byte(du_lieu_ra[i]);
        }

        if(cho_phep_pid) {
            pwm_output = xu_ly_pid(&bo_pid, toc_do_dat, toc_do_hien_tai);
        }
    }
}

void timer_handler(void) interrupt 1 {
    static unsigned int counter = 0;
    TL0 = 0x18; TH0 = 0xFC;
    PWM_CHAN = (counter < pwm_output) ? 1 : 0;
    counter = (counter + 1) % 100;

    can_lam_moi = (counter % 100 == 0);
    cho_phep_pid = (counter % 10 == 0);
}

void encoder_A_isr(void) interrupt 0 {
    if(CAM_BIEN_A == 0 && CAM_BIEN_B == 1) dem_xung_A++;
    if(CAM_BIEN_A == 1 && CAM_BIEN_B == 0) dem_xung_A--;
}

void encoder_B_isr(void) interrupt 2 {
    if(CAM_BIEN_A == 0 && CAM_BIEN_B == 1) dem_xung_B++;
    if(CAM_BIEN_A == 1 && CAM_BIEN_B == 0) dem_xung_B--;
}

void uart_handler(void) interrupt 4 {
    static unsigned char byte, index = 0, trang_thai = 0;
    if(RI) {
        byte = SBUF;
        if(byte == 0xA5 && trang_thai == 0) trang_thai = 1;
        else if(trang_thai == 1) {
            du_lieu_vao[index++] = byte;
            if(byte == 0x5A) trang_thai = 2;
        } else if(trang_thai == 2) {
            trang_thai = index = 0;
        } else {
            trang_thai = index = 0;
        }
        RI = 0;
    }
}