help me!xe tránh vật cản dùng cảm biến siêu âm sfr05.

[wikj10]

Mạch em dùng con atmega16, động cơ DC 12V
Em dùng con cảm biến siêu âm sfr05( sfr04 cũng giống tương tự) để đo khoảng cách.ai đã từng dùng con này sẽ hiểu cách hoạt động của nó. Nói sơ về nguyên lý như thế này: sfr05 dùng ở chế độ mode 0 nghĩa là chân Echo và chân Trigger độc lập. timer0 để đọc giá trị thời gian của cảm biến. timer1 để điều chế xung PWM dùng chế ở chế độ 14 nghĩa là chu kỳ xung được đặt trên thanh ghi ICR1, giá trị duty được đặt vào 2 thanh ghi OCR1A và OCR1B.
Xe hoạt động như sau: cảm biến siêu âm đo khoảng cách vật cản và hiển thị trên LCD khoảng cách từ cảm biến đến vật cản. đồng thời VDK cho xe chạy(PWM 2 bánh xe chạy cùng duty) khi gặp vật cản nhỏ hơn 40cm thì 1 động cơ đảo chiều tới khi cảm biến đo được khoảng cách lớn hơn 40cm thì 2 động cơ chạy cùng 1 duty(cùng vận tốc).
Tuy nhiên em gặp 1 vấn đề khi khoảng cách < 40 cm rồi mà 2 động cơ dừng luôn mà không đổi chiều. bác nào biết lỗi ở chỗ nào chỉ dùm em với. thanks nhiều lắm
Em lập trình bằng codevision.
Code:
/************************************************** ***
This program was produced by the
CodeWizardAVR V2.05.0 Professional
Automatic Program Generator
© Copyright 1998-2010 Pavel Haiduc, HP InfoTech s.r.l.
HP InfoTech, Development Tools for Microcontrollers, C Compilers, In-System Programmers

Project :
Version :
Date : 5/9/2013
Author : NeVaDa
Company :
Comments:


Chip type : ATmega16
Program type : Application
AVR Core Clock frequency: 1.000000 MHz
Memory model : Small
External RAM size : 0
Data Stack size : 256
************************************************** ***/
#include <mega16.h>
#include <stdio.h>
#include <alcd.h>
#include <math.h>
#include <delay.h>
#include <stdlib.h>
#define TRIGGER PORTD.3
#define ECHO PIND.2
#define EN PORTB.0
#define DIR1 PORTB.1
#define DIR2 PORTB.2
unsigned char range_ok;
unsigned long range;
unsigned long distance;
char str[16];
//unsigned char i=0;

// External Interrupt 0 service routine
interrupt [EXT_INT0] void ext_int0_isr(void)
{
// Place your code here
//distance=0;
//range=0;
distance=TCNT0; // ?o.c giá tri. Timer 1
range=(distance*64)/58;
TCNT0 = 0x00; // Xóa du+~ lie^.u trong Timer 1
TCCR0 = 0x00; // Ngu+`ng Timer1, Timer 1 chi? hoa.t ?o^.ng khi MCU yêu ca^`u ?o khoa?ng cách
range_ok=1; // hoàn thành phép ?o
}
void StartRange()
{
TRIGGER=1;
delay_ms(12); // Ta.o 1 xung ít nha^'t 10ms
TRIGGER=0; // Ba('t ?a^`u gu+?i le^.nh ?o
while(!(ECHO)); // ?o+.i cho chân Echo kéo lên mu+'c cao
TCCR0=0x03; // Cho Phép Timer 1 hoa.t ?o^.ng
}
// Declare your global variables here

void main(void)
{
// Declare your local variables here

// Input/Output Ports initialization
// Port A initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTA=0x00;
DDRA=0x00;

// Port B initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTB=0x00;
DDRB=0x07;

// Port C initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTC=0x00;
DDRC=0x00;

// Port D initialization
// Func7=In Func6=In Func5=Out Func4=Out Func3=Out Func2=In Func1=In Func0=In
// State7=T State6=T State5=0 State4=0 State3=0 State2=T State1=T State0=T
PORTD=0x00;
DDRD=0x38;

// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: Timer 0 Stopped
// Mode: Normal top=0xFF
// OC0 output: Disconnected
TCCR0=0x00;
TCNT0=0x00;
OCR0=0x00;

// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: 1000.000 kHz
// Mode: Ph. & fr. cor. PWM top=ICR1
// OC1A output: Non-Inv.
// OC1B output: Non-Inv.
// Noise Canceler: Off
// Input Capture on Falling Edge
// Timer1 Overflow Interrupt: Off
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off
// Compare B Match Interrupt: Off

TCCR1A=0xA0;
TCCR1B=0x12;
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x27;
ICR1L=0x10;
OCR1A=10000;
OCR1B=10000;

// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: Timer2 Stopped
// Mode: Normal top=0xFF
// OC2 output: Disconnected
ASSR=0x00;
TCCR2=0x00;
TCNT2=0x00;
OCR2=0x00;

// External Interrupt(s) initialization
// INT0: On
// INT0 Mode: Falling Edge
// INT1: Off
// INT2: Off
GICR|=0x40;
MCUCR=0x02;
MCUCSR=0x00;
GIFR=0x40;

// Timer(s)/Counter(s) Interrupt(s) initialization
TIMSK=0x00;

// USART initialization
// Communication Parameters: 8 Data, 1 Stop, No Parity
// USART Receiver: On
// USART Transmitter: On
// USART Mode: Asynchronous
// USART Baud Rate: 4800
UCSRA=0x00;
UCSRB=0x18;
UCSRC=0x86;
UBRRH=0x00;
UBRRL=0x0C;

// Analog Comparator initialization
// Analog Comparator: Off
// Analog Comparator Input Capture by Timer/Counter 1: Off
ACSR=0x80;
SFIOR=0x00;

// ADC initialization
// ADC disabled
ADCSRA=0x00;

// SPI initialization
// SPI disabled
SPCR=0x00;

// TWI initialization
// TWI disabled
TWCR=0x00;

// Alphanumeric LCD initialization
// Connections specified in the
// Project|Configure|C Compiler|Libraries|Alphanumeric LCD menu:
// RS - PORTA Bit 0
// RD - PORTA Bit 1
// EN - PORTA Bit 2
// D4 - PORTA Bit 4
// D5 - PORTA Bit 5
// D6 - PORTA Bit 6
// D7 - PORTA Bit 7
// Characters/line: 16
lcd_init(16);
EN=1;
DIR1=1;
DIR2=1;
// Global enable interrupts
#asm("sei")
lcd_gotoxy(0,0);
lcd_puts("trobe");
printf("trobe\n\r");
delay_ms(200);
while (1)
{
// Place your code here
// printf("turn right\n\r");
EN=1;
DIR1=1;
DIR2=1;
// lcd_clear();
if(!range_ok)
{
StartRange();
}
if(range_ok)
{
lcd_clear();
lcd_gotoxy(8,1);
itoa(range,str);
lcd_puts(str);
lcd_gotoxy(8,0);
lcd_puts("go on");
// delay_ms(50); // d?i 200ms d? ti?n hành phép do ti?p theo
range_ok=0;
delay_ms(100);
}

while(range<=40)
{
EN=1;
DIR1=0;
DIR2=1;
// printf("turn left\n\r");
if(!range_ok)
{
StartRange();
}
if(range_ok)
{
lcd_clear();
lcd_gotoxy(8,1);
itoa(range,str);
lcd_puts(str);
lcd_gotoxy(4,0);
lcd_puts("turn_left");
// delay_ms(50); // d?i 200ms d? ti?n hành phép do ti?p theo
range_ok=0;
delay_ms(100);
}
}

// printf("go on\n\r");
}
}

[qttc.sc]

Xung trên chân Triger cần tuân thủ tần số <=20Hz, có thể dùng timer để đảm bảo điều này! (việc dùng delay là ko hợp lý)
Nễu 40cm là khoảng cách để robot xác nhận có vật cản và dừng lại thì nên để khoảng cách lớn hơn thế để robot đc phép tiếp tục chuyển động tiến (có thể để 50cm chẳng hạn)
Chúc thành công!