Robot z dalmierzem - problem z programem [C]

[klimek4765]

Witam,

Chcialem zrobic robota w oparciu o ultradzwiekowy czujnik odleglosci HC SR04 zamontowany na serwie w taki sposob aby robot mogl sie "rozgladac" w razie napotkania przeszkody.

Wszystko nawet dziala, jednak przy okreslaniu kierunku robot uparcie wskazuje na skrajne prawo (chyba sugeruje zeby glosowac na korwina 😅 ) wiec cos tam w srodku nie dziala, tylko pytanie co? Czy ma ktos jakis pomysl?

Glowna petla programu (w ktorej na 99,99% znajduje sie blad prezentuje sie tak:

#define F_CPU 8000000L				//wewnetrzy oscylator
#include <avr/io.h>
#include <util/delay.h>
#include <stdlib.h>
#define PWM OCR1B
#include "addons.h"
#include "HD44780.h"
#include <avr/interrupt.h>
volatile int flaga=0;					//flaga przerwania pomiaru odleglosci 
unsigned int odleglosc=300;				//doyslna wartosc odleglosci (
volatile int flaga_przerwania=0;		//flaga przerwania wywolania calej procedury pomiaru
unsigned int najdalej=0;				//wartosc najwiekszej odleglosci pomiaru
int kierunek=0;							//zmienna do przechowywania wartosci kierunku jazdy




int main(void)
{
char wynik[4];				//do przechowywania wyniku wyswietlacza alfanumerycznego

LCD_Initalize();				 //inicjalizacja LCD
LCD_GoTo(0, 0);				 //Ustawienie kursora w pozycji (0,0)
LCD_WriteText("    ");			//wyczyszczenie wyswietlacza
LCD_WriteText("SRV1");
_delay_ms(500);					//odczekanie
init();								//inicjalizacja wszystkiego

   while(1)					//glowna petla
  {
   if(flaga==1)				//jesli rozpoczal sie pomiar
   {
	   odleglosc=0;			//zerowanie odleglosci
   }
   while(flaga==1)			//podczas pomiaru
   {
	   odleglosc++;			//zwiekszaj licznik odleglosci
   }
   if(odleglosc<300)			//jesli podczas jazdy odleglosc od przeszkody mniejsza niz 300
   {
	   najdalej=0;				//zerujemy najdalsza odleglosc
	   int i;					//zmienna pomocnicza
	   TIMSK &=~(1<<TOIE0);		//wylaczanie glownego timera zeby nie przeszkadzal
	   PWM=7000;				//obrot czujnika w prawo (pozycja startowa pomiaru)
	   _delay_ms(400);			//odczekanie az sie ustabilizuje
	   for(i=0;i<=100;i++)		//rozpoczecie jednoczesnego obracania i serii pomiarow
	   {    
		   PWM=7000+i*100;		//wartosc PWM potrzebna do obrotu
		   _delay_ms(5);		//odczekanie na stabilizacje
		   uruchom_pomiar();	//wyzwolenie pomiaru
		   if(flaga==1)			//jezeli rozpoczal sie pomiar
		   {
			   odleglosc=0;		//zerujemy odleglosc
		   }
		   while(flaga==1)		//podczas pomiaru
		   {
			   odleglosc++;		//zwiekszamy licznik odleglosci
		   }
		   if(odleglosc>najdalej)	//jezeli wykryta odleglosc jest najwyzsza z dotychczas zmierzonych
		   {
			   najdalej=odleglosc;	//najdalsza zmierzona odlegloscia zostaje ta spelniajaca powyzszy warunek 
			   kierunek=i;			//zostaje przypisany kierunek w ktorym robot ma podazac  
		   }
	   }
	   TIMSK |=(1<<TOIE0);			//po wykonaniu petli przywracamy timer0 
	   PWM=11600;					//ustawiamy glowe do pozycji startowej
	   _delay_ms(200);				//odczekujemy na ustabilizowanie sie
	   odleglosc=301;				//ustawiamy na wszelki wypadek odleglosc na wieksza niz prog zeby sie nie zapetlil przypadkiem
   }
		LCD_GoTo(1, 1);         //Ustawienie kursora w pozycji (1,1)
		LCD_WriteText("      ");   //czyszczenie poprzednij wartości
		itoa(odleglosc,wynik,10);      //konwersja wyniku do tablicy char
		LCD_WriteText(wynik);   //Wyświetlenie wyniku

/*if(flaga_przerwania%5==0)				//2 razy na jedno glowne przerwanie aktualizujemy stan toru jazdy(na wszelki wypadek)
{
	if((kierunek>40)&&(kierunek<60)){
	prosto();}
	if(kierunek<=40){
	prawo();}
	if(kierunek>=60){
	lewo();}
}		*/
  if(flaga_przerwania>=10)			//glowne przerwanie
 {
  flaga_przerwania=0;				//zerujemy flage przerwania
  uruchom_pomiar();					//wyzwalamy pomiar
 }

}
}
ISR(INT1_vect)
{
if(flaga==0)					//jesli poza czasem pomiaru
{
flaga=1;						//flaga rozpoczecia pomiaru
 MCUCR &=~(1<<ISC10);			//przerwanie wyzwalane zboczem opadajacym    
}
else if(flaga==1)				//jesli w trakcie pomiaru
{
	MCUCR |= (1<<ISC10);		//przerwanie wzwalane zboczem narastajacym
	flaga=0;					//zerowanie flagi pomiaru
}
}

ISR(TIMER0_OVF_vect)				//glowne przerwanie, zwieksza licznik
{
flaga_przerwania++;
}

Prosze o sugestie, na kod mozna smialo narzekac bo jest to wersja robocza, jeszcze przed liftingiem 😅

PS

odnosnie tego czujnika odleglosci, przy czestych pomiarach podczas obracania serwem czesto zdarza mu sie wykonac zly pomiar, ma ktos jakis pomysl?