안녕하세요 
atmega128을 사용하여 초음파센서에서 읽는 거리를 측정해 서보모터를 동작 중입니다.
현재 초음파센서는 HC-SR04를 이용하고 있으며 서보모터는 SG-90을 사용중입니다.
50CM 이하일 때 서보모터가 좌우로 동작하고 그 외에는 멈추는 코드를 짜고 있는데
동작이 되지않아 문의드립니다.
 
#include <avr/io.h>
#define F_CPU 16000000UL
#include <util/delay.h>
#define  TRIG  6
#define  ECHO  7
#define  SOUND_VELOCCITY 340UL
unsigned char i;
 
 
unsigned int cal_distance() //장애물까지의 거리 계산
{
	unsigned distance;
	TCCR1B = 0x03;
	PORTB &= ~(1<<TRIG);
	_delay_us(10);
	PORTB |= (1<<TRIG);
	_delay_us(10);
	PORTB &= ~(1<<TRIG);
	while(!(PIND& (1<<ECHO)));
	TCNT1=0x0000;
	while (PIND & (1<<ECHO));
	TCCR1B = 0x00;
	distance = (unsigned int)(SOUND_VELOCCITY * (TCNT1*4/2) /1000);
	return(distance);
}
 
void servoStop()
{
	TCCR3A &= ~((1<<COM3A1) | (1<<COM3A0));
}
 
int main(void)
{
	unsigned int distance;
	DDRE|= 0x08;
	PORTE|= 0x08;
	DDRB =DDRB|(1<<TRIG);
	DDRD= DDRD|~(1<<ECHO);
	TCCR3A=0x82;
	TCCR3B=0x1b;
	ICR3 =4999; //TOP
	
	while(1)
	{
		distance=cal_distance();
		if(distance < 500)
		{
		
		OCR3A=125; //0도
		_delay_ms(500);
		OCR3A=375; //90도
		_delay_ms(500);
		OCR3A=625; //180도
		_delay_ms(500);
		}
	else
	{
		servoStop();
	}
	}
}