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? Generalnie to dorzucilem do kodu pomiar usredniajacy na podstawie 5 pomiarow, co troche pomoglo.
Niestety czujnik czasem wywala zamiast 0,5m wartosc odpowiadajaca 20 metrom i wtedy nawet te 5 pomiarow niewiele daje.
Glowna petla programu (w ktorej na 99,99% znajduje sie blad prezentuje sie tak:
[syntax=c]#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
uint16_t odleglosc=300;//domyslna wartosc odleglosci
volatile int flaga_przerwania=0;//flaga przerwania wywolania calej procedury pomiaru
uint16_t najdalej=300;//wartosc najwiekszej odleglosci pomiaru
unsigned int kierunek=0;//zmienna do przechowywania wartosci kierunku jazdy
unsigned int mediana;
int indeks_mediany=0;
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
{
najdalej=301;
odleglosc=0;//zerowanie odleglosci
}
while(flaga==1)//podczas pomiaru
{
odleglosc++;//zwiekszaj licznik odleglosci
}
++indeks_mediany;
mediana=mediana+odleglosc;
if(indeks_mediany>4)
{
najdalej=mediana/5;
indeks_mediany=0;
mediana=0;
}
if(najdalej<200)//jesli podczas jazdy odleglosc od przeszkody mniejsza niz 300
{
mediana=10;
indeks_mediany=0;
najdalej=201;//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
{
indeks_mediany++;
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
}
mediana+=odleglosc;
if((mediana>najdalej)&&(indeks_mediany>=4))//jezeli wykryta odleglosc jest najwyzsza z dotychczas zmierzonych
{
//mediana=mediana/3;
najdalej=mediana/5;//najdalsza zmierzona odlegloscia zostaje ta spelniajaca powyzszy warunek
//kierunek=i;//zostaje przypisany kierunek w ktorym robot ma podazac
indeks_mediany=0;
kierunek=najdalej;
mediana=0;
}
if(indeks_mediany>=4)
{
mediana=0;
indeks_mediany=0;
}
}
najdalej=301;//ustawiamy na wszelki wypadek odleglosc na wieksza niz prog zeby sie nie zapetlil przypadkiem
PWM=11600;//ustawiamy glowe do pozycji startowej
//_delay_ms(400);//odczekujemy na ustabilizowanie sie
TIMSK |=(1<<TOIE0);//po wykonaniu petli przywracamy timer0
}
LCD_GoTo(3, 1); //Ustawienie kursora w pozycji (1,1)
LCD_WriteText(" "); //czyszczenie poprzednij wartości
LCD_GoTo(3, 1);
itoa(kierunek,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++;
}[/syntax]
Po kodzie mozna smialo jezdzic, jest to wersja niechlujna, przed liftingiem ktory zrobie jak wszystko bedzie dzialac w 100%.
Ma ktos jakies pomysly skad sie biora takie bledy?
PS gdzie trzeba definiowac F_CPU? i dlaczego nie tutaj?
Statystyki: Napisane przez klimek476 — 21 paź 2015, o 16:28
]]>