Tweet
giúp em cái code C này với !
Code:
#include <16f874a.h>
#use delay(clock = 20,000,000)
#Fuses HS,NOWDT,NOPUT,NOBROWNOUT,NOLVP
#Fuses LVP,CPD,PROTECT
#include "lcd_driver.c"
#include <ctype.h>
//==============================
int1 sensor1=0,sensor2=0,sensor3=0,sensor4=0;
int timer0=0, duty_left = 0, duty_right = 0 ;
int status_old = 0, status_new = 0;
int16 a = 0,time=0;
//------------------------------
void int_sys();
void detected_line(); // phat hien line
int measure_distance(); // do khoan cach
void control_motor_left(int duty);
void control_motor_right(int duty);
/*
*
*/
#define BUT PIN_B0 // xac dinh
#define SENSOR_1 PIN_C0
#define SENSOR_2 PIN_C1
#define SENSOR_3 PIN_C2
#define SENSOR_4 PIN_C3
#define PWM11 PIN_D4
#define PWM12 PIN_D5
#define PWM21 PIN_D6
#define PWM22 PIN_D7
#define TRIG PIN_B1
#define ECHO PIN_B2
//----------------------------------------------
#INT_TIMER0
void timer0_isr()
{
if(status_old != status_new)
{
timer0 = 0;
status_old = status_new;
}
timer0++;time++;
if(timer0 >= 10) timer0 = 0;
// control motor left
if((status_new == 2))// dao chieu ??ng co left
{
output_bit(PWM11,0);
if(timer0 < duty_left)
{
output_bit(PWM12,1);
}
else output_bit(PWM12,0);
}
else
{
output_bit(PWM12,0);
if(timer0 < duty_left)
{
output_bit(PWM11,1);
}
else output_bit(PWM11,0);
}
// control motor right
if(status_new == 3)// dao chieu ??ng co right
{
output_bit(PWM22,0);
if(timer0 < duty_right)
output_bit(PWM21,1);
else output_bit(PWM21,0);
}
else
{
output_bit(PWM21,0);
if(timer0 < duty_right)
output_bit(PWM22,1);
else output_bit(PWM22,0);
}
if(status_new == 4)// dao chieu ??ng co right and left
{
output_bit(PWM11,0);
if(timer0 < duty_left)
{
output_bit(PWM12,1);
}
else output_bit(PWM12,0);
output_bit(PWM21,0);
if(timer0 < duty_right)
output_bit(PWM22,1);
else output_bit(PWM22,0);
}
// measure distance
}
//==============================================
void int_sys()
{
set_tris_B(0x01);
set_tris_C(0x0f);
set_tris_D(0x00);
output_bit(PWM12,0);
output_bit(PWM21,0);
output_bit(PWM22,0);
output_bit(PWM11,0);
output_bit(PWM22,0);
setup_timer_0 (T0_DIV_32|T0_INTERNAL);
lcd_init();
}
//=============================================
void detected_line()// detect = 0, non detect = 1
{
sensor1 = input(SENSOR_1);
sensor2 = input(SENSOR_2);
sensor3 = input(SENSOR_3);
sensor4 = input(SENSOR_4);
begin:
if((sensor3 == 1)&&(sensor2 == 1))
{
status_new = 1;
duty_left = 3, duty_right = 3;
}
if((sensor3 == 0)&&(sensor2 == 1))// sensor3 deteted
{
status_new = 2;
while((input(SENSOR_3)==0)&&(input(SENSOR_2)==1))
{
duty_left = 4, duty_right = 4;
}
if((sensor3 == 1)&&(sensor2 == 1))
{
goto begin;
}
}
if((sensor2 == 0)&&(sensor3 == 1))//rc1 = detect
{
status_new = 3;
while((input(SENSOR_3)==1)&&(input(SENSOR_2)==0))
{
duty_left = 4, duty_right = 4;
}
if((sensor3 == 1)&&(sensor2 == 1))
{
goto begin;
}
}
if((sensor2 == 0)&&(sensor3 == 0)&&(sensor1 == 1)&&(sensor4 == 1))// stop
{
status_new = 4;
while((input(SENSOR_3)==0)&&(input(SENSOR_2)==0))
{
duty_left = 5, duty_right = 5;
}
if((sensor3 == 1)&&(sensor2 == 1))
{
goto begin;
}
}
if((sensor2 == 0)&&(sensor3 == 0)&&(sensor1 == 0)&&(sensor4 == 0))// stop
{
status_new = 5;
duty_left = 0, duty_right = 0;
}
}
//============================================
int measure_distance()
{
int16 echo_time=0, distance = 0;
SET_TRIS_B(0X01);
output_bit(TRIG,1);
delay_us(15);
output_bit(TRIG,0);
SET_TRIS_B(0x05);
while(input(ECHO)==0);
while(input(ECHO)==1)
{
echo_time++;
delay_us(1);
}
distance = echo_time/58;// conver time to cm
return distance;
}
//===========================================
void main() {
// unsigned int16 a =0;
int_sys();
lcd_gotoxy(1,1);
printf(lcd_putc," X ");
lcd_gotoxy(1,0);
printf(lcd_putc," Y ");
delay_ms(3000);
lcd_gotoxy(1,1);
printf(lcd_putc," BAT DAU ");
lcd_gotoxy(1,0);
printf(lcd_putc,"KHOANG CACH: ");
enable_interrupts(GLOBAL);
enable_interrupts(INT_TIMER0);
while(1)
{
while(input(BUT)==1)
{delay_ms(10);}
while(1)
{
detected_line();
if(time >= 1000)
{
disable_interrupts(INT_TIMER0);
a = measure_distance();
lcd_gotoxy(13,0);
printf(lcd_putc,"%03lu",a);
time = 0;
enable_interrupts(INT_TIMER0);
}
}
}
}
Comment