- Sử dụng thêm PID để tăng độ ổn định cho mô hình - Thêm định vị và camera để quan sát cho mobile
Tài liệu tham khảo
Giáo Trình:
[1] http://arduino.vn/bai-viet/893-cach-dung-module-dieu-khien-dong-co-l298n-cau- h-de-dieu-khien-dong-co-dc
Code lập trình
#INCLUDE <16F877a.H> //
#FUSES HS // SU DUNG THANH ANH NGOAI 20m #USE DELAY(CLOCK=20M) // THU VIEN DELAY
#define trng pin_a4 // trai ngoai #define trtr pin_a3 // trai trong #define giua pin_a2 // giua #define phtr pin_a1 // phai trong #define phng pin_a0 // phai ngoai
signed int32 PWM_DUTY; // chay 0 - 1000 //CAC HAM CHO DK DONG CO DC BANG PWM
//XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
#DEFINE MOTOR_ENA0 PIN_c0 #DEFINE MOTOR_ENA1 PIN_c3
INT1 DCMOTOR_TN; // thuan nghich
//CAC HAM CHO DK DONG CO DC BANG PWM
VOID DC_MOTOR_KTAO_CCP1_THUAN() { SETUP_CCP2(CCP_OFF); SETUP_CCP1(CCP_PWM); OUTPUT_LOW(PIN_C1); } VOID DC_MOTOR_KTAO_CCP2_NGHICH() { SETUP_CCP1(CCP_OFF); SETUP_CCP2(CCP_PWM); OUTPUT_LOW(PIN_C2); } VOID DC_MOTOR_KTAO_CCPX_KTAO() { IF(DCMOTOR_TN==1) DC_MOTOR_KTAO_CCP1_THUAN(); ELSE DC_MOTOR_KTAO_CCP2_NGHICH(); } VOID DC_MOTOR_PWM_SETUP_DUTY() { IF (DCMOTOR_TN==1) SET_PWM1_DUTY(PWM_DUTY); ELSE SET_PWM2_DUTY(PWM_DUTY);
} VOID DC_MOTOR_PWM_CCP1_CCP2_STOP() { SETUP_CCP1(CCP_OFF); SETUP_CCP2(CCP_OFF); DC_MOTOR_KTAO_CCPX_KTAO(); } void chay_line() { if((input(trng)==1)&&(input(trtr)==1)&&(input(giua)==0)&&(input(phtr)==1)&&(input (phng)==1)) // 11011 { PWM_DUTY=500; //
OUTPUT_high(MOTOR_ENA0); // bat 2 dong co OUTPUT_high(MOTOR_ENA1);
DCMOTOR_TN=0; // = 0 di thang
DC_MOTOR_KTAO_CCPX_KTAO(); // khoi tao quay
DC_MOTOR_PWM_SETUP_DUTY(); // khoi tao toc do quay }
else
if((input(trng)==1)&&(input(trtr)==0)&&(input(giua)==0)&&(input(phtr)==1)&&(input (phng)==1)) // 10011
{
PWM_DUTY=400;
OUTPUT_high(MOTOR_ENA0); // bat 2 dong co OUTPUT_high(MOTOR_ENA1);
DCMOTOR_TN=0; // = 0 quay thuan
DC_MOTOR_KTAO_CCPX_KTAO(); // khoi tao quay
DC_MOTOR_PWM_SETUP_DUTY(); // khoi tao toc do quay } else if((input(trng)==1)&&(input(trtr)==1)&&(input(giua)==0)&&(input(phtr)==0)&&(input (phng)==1)) // 11001 { PWM_DUTY=400;
OUTPUT_high(MOTOR_ENA0); // bat 2 dong co OUTPUT_high(MOTOR_ENA1);
DCMOTOR_TN=0; // = 0 quay thuan
DC_MOTOR_KTAO_CCPX_KTAO(); // khoi tao quay
DC_MOTOR_PWM_SETUP_DUTY(); // khoi tao toc do quay }
else
if((input(trng)==1)&&(input(trtr)==0)&&(input(giua)==0)&&(input(phtr)==0)&&(input (phng)==1)) //10001
{
PWM_DUTY=400;
OUTPUT_high(MOTOR_ENA0); // bat 2 dong co OUTPUT_high(MOTOR_ENA1);
DCMOTOR_TN=0; // = 0 quay thuan
DC_MOTOR_KTAO_CCPX_KTAO(); // khoi tao quay
DC_MOTOR_PWM_SETUP_DUTY(); // khoi tao toc do quay } else if((input(trng)==1)&&(input(trtr)==0)&&(input(giua)==1)&&(input(phtr)==1)&&(input (phng)==1)) //10111 { PWM_DUTY=400; OUTPUT_low(MOTOR_ENA0); // phai OUTPUT_high(MOTOR_ENA1); // trai DCMOTOR_TN=0; // = 0 quay thuan
DC_MOTOR_KTAO_CCPX_KTAO(); // khoi tao quay
DC_MOTOR_PWM_SETUP_DUTY(); // khoi tao toc do quay }
else if((input(trng)==1)&&(input(trtr)==1)&&(input(giua)==1)&&(input(phtr)==0)&&(input (phng)==1)) //11101 { PWM_DUTY=400; OUTPUT_high(MOTOR_ENA0); OUTPUT_low(MOTOR_ENA1);
DCMOTOR_TN=0; // = 0 quay thuan
DC_MOTOR_KTAO_CCPX_KTAO(); // khoi tao quay
DC_MOTOR_PWM_SETUP_DUTY(); // khoi tao toc do quay } else if((input(trng)==0)&&(input(trtr)==0)&&(input(giua)==1)&&(input(phtr)==1)&&(input (phng)==1)) //00111 { PWM_DUTY=500;
OUTPUT_low(MOTOR_ENA0); // bat 2 dong co OUTPUT_high(MOTOR_ENA1);
DCMOTOR_TN=0; // = 0 quay thuan
DC_MOTOR_KTAO_CCPX_KTAO(); // khoi tao quay
DC_MOTOR_PWM_SETUP_DUTY(); // khoi tao toc do quay }
else
if((input(trng)==1)&&(input(trtr)==1)&&(input(giua)==1)&&(input(phtr)==0)&&(input (phng)==0)) //11100
{
PWM_DUTY=500;
OUTPUT_high(MOTOR_ENA0); // bat 2 dong co OUTPUT_low(MOTOR_ENA1);
DCMOTOR_TN=0; // = 0 quay thuan
DC_MOTOR_KTAO_CCPX_KTAO(); // khoi tao quay
DC_MOTOR_PWM_SETUP_DUTY(); // khoi tao toc do quay } else if((input(trng)==0)&&(input(trtr)==1)&&(input(giua)==1)&&(input(phtr)==1)&&(input (phng)==1)) //01111 { PWM_DUTY=500;
OUTPUT_low(MOTOR_ENA0); // bat 2 dong co OUTPUT_high(MOTOR_ENA1);
DCMOTOR_TN=0; // = 0 quay thuan
DC_MOTOR_KTAO_CCPX_KTAO(); // khoi tao quay
DC_MOTOR_PWM_SETUP_DUTY(); // khoi tao toc do quay }
else
if((input(trng)==1)&&(input(trtr)==1)&&(input(giua)==1)&&(input(phtr)==1)&&(input (phng)==0)) //11110
{
PWM_DUTY=400;
OUTPUT_high(MOTOR_ENA0); // bat 2 dong co OUTPUT_low(MOTOR_ENA1);
DCMOTOR_TN=0; // = 0 quay thuan
DC_MOTOR_KTAO_CCPX_KTAO(); // khoi tao quay
DC_MOTOR_PWM_SETUP_DUTY(); // khoi tao toc do quay } else if((input(trng)==1)&&(input(trtr)==1)&&(input(giua)==0)&&(input(phtr)==0)&&(input (phng)==0)) //11000 { PWM_DUTY=500;
OUTPUT_high(MOTOR_ENA0); // bat 2 dong co OUTPUT_low(MOTOR_ENA1);
DCMOTOR_TN=0; // = 0 quay thuan
DC_MOTOR_KTAO_CCPX_KTAO(); // khoi tao quay
DC_MOTOR_PWM_SETUP_DUTY(); // khoi tao toc do quay }
else
if((input(trng)==0)&&(input(trtr)==0)&&(input(giua)==0)&&(input(phtr)==1)&&(input (phng)==1)) //00011
{
PWM_DUTY=500;
OUTPUT_low(MOTOR_ENA0); // bat 2 dong co OUTPUT_high(MOTOR_ENA1);
DCMOTOR_TN=0; // = 0 quay thuan
DC_MOTOR_KTAO_CCPX_KTAO(); // khoi tao quay
DC_MOTOR_PWM_SETUP_DUTY(); // khoi tao toc do quay //delay_ms(100); } else if((input(trng)==1)&&(input(trtr)==0)&&(input(giua)==0)&&(input(phtr)==0)&&(input (phng)==0)) //10000 { PWM_DUTY=500;
OUTPUT_high(MOTOR_ENA0); // bat 2 dong co OUTPUT_low(MOTOR_ENA1);
DCMOTOR_TN=0; // = 0 quay thuan
DC_MOTOR_KTAO_CCPX_KTAO(); // khoi tao quay
//delay_ms(100); } else if((input(trng)==0)&&(input(trtr)==0)&&(input(giua)==0)&&(input(phtr)==0)&&(input (phng)==1)) //00001 { PWM_DUTY=400;
OUTPUT_low(MOTOR_ENA0); // bat 2 dong co OUTPUT_high(MOTOR_ENA1);
DCMOTOR_TN=0; // = 0 quay thuan
DC_MOTOR_KTAO_CCPX_KTAO(); // khoi tao quay
DC_MOTOR_PWM_SETUP_DUTY(); // khoi tao toc do quay //delay_ms(100);
}
else //khong nhan line {
delay_ms(50); PWM_DUTY=400;
OUTPUT_high(MOTOR_ENA0); // bat 2 dong co OUTPUT_high(MOTOR_ENA1);
DCMOTOR_TN=1; // = 1 quay nghich
DC_MOTOR_PWM_SETUP_DUTY(); // khoi tao toc do quay }
}
void main() {
SET_TRIS_D(0X00); // khoi tao cac port SET_TRIS_C(0X00);
SET_TRIS_A(0Xff); // vi chan AN0 vao nen cho len 1 con lai ve 0 SET_TRIS_E(0Xff);
SET_TRIS_B(0X00);
SETUP_TIMER_2(T2_DIV_BY_16,249,1); // timer2 dung cho pwm // 0.8ms WHILE(TRUE) { chay_line(); delay_ms(20); } }