Nawigacja dla robota

[Braders94]

Witam.

Chciał bym wam zaprezentować moje próby wykonania nawigacji dla robota.

Pomysł zrodził się już jakiś czas temu ale praca nie pozwala na rozwijanie swojego hobby,
ale udało mi się znaleźć trochę czasu na wykonanie pierwszych prób.

Pomysł jest bardzo prosty dwie wieże nadawany sygnał ultradźwiękowy plus przez kabel "później radio" sygnał o wysłaniu sygnału. Odbieramy jak długo fala leciała i znamy odległość. Później obliczamy punkt przecięcia dwóch okręgów o promieniach takich jak odległość wieżyczek od robota i mamy współrzędne robota.

Obrazek z painta:)

Teraz zaprezentuję krótki filmik co mi się udało zrobić, widać tam zasadę działania.

Sorki za miejsce nagrania ale nie chciało mi się tego przenosić

Jako iż odbiornik ultradźwiękowy zbiera fale z 10° co do tego projektu jest znacznie za mało to postanowiłem zbudować taki mały przyrząd czeka na przetestowanie .

Dodałem obsługę czujników za pomocą timerów i zwiększyłem dokładność do dwóch zer po przecinku.

Jak widać na powyższym filmiku mam kłopot z falowaniem wyniku, będzie trzeba dodać jakieś uśrednianie kilku wyników.

Ok udało mi się ogarnąć program potrzeba mu jeszcze trochę podpicowania bo pisałem i składałem go na szybko, ale działa.

Prosty opis planszy do testów

"I znów ten paint"

Punkt (X0:Y0) na dole po lewej stronie.

Drugi nadajnik jest odsunięty od tego o ~400mm w prawo.

Opis danych na wyświetlaczu.

Pierwsza linijka odległość od stacji numer 1 i 2.

Druga linijka pozycja X i Y odbiornika.

Oto filmik z testów.

Jak widać jakoś "ledwie" to działa.

Teraz pozostał największy problem do ogarnięcia, a mianowicie rozproszenie sygnału,tak aby pokrywał sygnałem jakieś 90°.

A po drugie to żeby zebrać ten sygnał na około robota.

Schemat

Kod

#include "HD44780.h"
#include <avr/io.h>

#include <avr/interrupt.h>

#include <util/delay.h>



volatile uint16_t pomiar,ostatniPomiar;
float odleglosc, odleglosc2;


uint16_t bufor[10];
uint16_t bufor2[10];
uint8_t miejsce=0;

double x3, y3, x3_prime, y3_prime;


void Start(void);
void Start2(void);
uint8_t circle_circle_intersection(double x0, double y0, double r0, double x1, double y1, double r1, double *xi, double *yi, double *xi_prime, double *yi_prime) ;

int main(void) {

//WE/WY

DDRB |= (1<<PB1);//jako wyjscie
DDRB |= (1<<PB2);//jako wyjscie

DDRD |= (1<<PD7);//jako wyjście echo

DDRB &= ~(1<<PB0);//jako wejscie echo

PORTB &= ~((1<<PB2)|(1<<PB1) | (1<<PB0));
PORTC &= ~(1<<PD7);



TCCR1B |= (1<<ICES1);//Zbocze narastające wywoła przerwanie

TCCR1B |= (1<<CS11);// preskaler 8

TIMSK |= (1<<TICIE1);// zezwolenie na przerwanie od ICP



sei();//globalne odblokowanie przerwan

LCD_Initalize(); //inicjacja LCD

//PETLA GLOWNA

       while(1) 
       {
          Start();//wywołanie pomiaru
          bufor[miejsce]=pomiar;//zapisz do bufora
          miejsce++;
          if (miejsce>9)
          miejsce=0;      //zapętlenie zmiennej

          pomiar=bufor[0]; //wpisanie 1 rejestru 

          for(uint8_t i=1;i<10;i++)//dodawanie kolejnych rejestrów
          pomiar=pomiar+bufor[i];

          pomiar=pomiar/10;  //dzielenie przez liczbę rejestrów 


          odleglosc = (float)(pomiar / 29.00);//obliczenie dystansu w cm




          Start2();//wywołanie pomiaru
          bufor2[miejsce]=pomiar;

          pomiar=bufor2[0]; //wpisanie 1 rejestru 

          for(uint8_t i=1;i<10;i++)//dodawanie kolejnych rejestrów
          pomiar=pomiar+bufor2[i];

          pomiar=pomiar/10;  //dzielenie przez liczbę


          odleglosc2 = (float)(pomiar / 29.00);//obliczenie dystansu w cm





         //run_test(0.0, 0.0, odleglosc, 40.0, 0.0, odleglosc2);
       if(miejsce==5)  
         {

          LCD_Home(); //czyść ekran
          LCD_WriteFloat(odleglosc);
          LCD_WriteText("cm ");

          LCD_WriteFloat(odleglosc2);
          LCD_WriteText("cm ");

          LCD_GoTo(0, 1);   

        if(circle_circle_intersection(0.0, 0.0,odleglosc , 28, 0, odleglosc2, &x3, &y3, &x3_prime, &y3_prime)) 
          {


          LCD_WriteText("x");
          LCD_WriteFloat(x3);
          LCD_WriteText("y");
          LCD_WriteFloat(y3);
          }
          else
             LCD_WriteText("blad obliczen");


          }
          _delay_ms(100);


       }

}

//Przerwanie z pinu 

ISR(TIMER1_CAPT_vect) {

         if((TCCR1B & (1<<ICES1))) 
         {
            ostatniPomiar = ICR1;//jesli zbocze narastajace, zapisz ICR1
         } else
         {
            pomiar = ICR1 -ostatniPomiar;//Jeśli zbocze opadajace oblicz pomiar
         }

         TCCR1B ^= (1<<ICES1);//Zmiana zbocza wyw przerwanie

}



void Start(void) {
ostatniPomiar=0;
pomiar=0;

PORTB |= (1<<PB1);//uruchomienie pomiaru w nadajniku
PORTD |= (1<<PD7);//uruchomienie pomiaru w odbiorniku
_delay_ms(10);
PORTB &= ~(1<<PB1);
PORTD &= ~(1<<PD7);
_delay_ms(60);

}


void Start2(void) {
ostatniPomiar=0;
pomiar=0;

PORTB |= (1<<PB2);//uruchomienie pomiaru w nadajniku
PORTD |= (1<<PD7);//uruchomienie pomiaru w odbiorniku
_delay_ms(10);
PORTB &= ~(1<<PB2);
PORTD &= ~(1<<PD7);
_delay_ms(60);

}


uint8_t circle_circle_intersection(double x0, double y0, double r0, double x1, double y1, double r1, double *xi, double *yi, double *xi_prime, double *yi_prime)
{ double a, dx, dy, d, h, rx, ry; double x2, y2; 
dx = x1 - x0;
dy = y1 - y0; 

d = hypot(dx,dy); 

if (d > (r0 + r1)) 
{ 
return 0; 
} 
if (d < fabs(r0 - r1)) 
{ 
return 0;
} 
a = ((r0*r0) - (r1*r1) + (d*d)) / (2.0 * d) ; 
x2 = x0 + (dx * a/d);
y2 = y0 + (dy * a/d); 
h = sqrt((r0*r0) - (a*a)); 
rx = -dy * (h/d); 
ry = dx * (h/d); 
*xi = x2 + rx; 
*xi_prime = x2 - rx; 
*yi = y2 + ry; 
*yi_prime = y2 - ry; 
return 1; 
} 

W kodzie wykorzystałem kilka bibliotek z internetu tutaj są linki do stron gdzie je znalazłem.

Obliczanie punktu styku dwóch kół.

http://paulbourke.net/geometry/circlesphere/

Biblioteka do wyświetlacza.

http://radzio.dxp.pl/hd44780/hd44780_avr_4-bit_norw_c.htm

[MaroRobotyk]

Powiem Ci że mega to wygląda. A powiedz mi sam to robiłeś czy gdzieś jakiś poradnik czytales?

[mactro]

O! Taki ciekawy projekt, a przeoczyłem go wcześniej... Kilka pytań:

- impulsy z nadajników wysyłasz naprzemiennie?

- z jaką częstotliwością jest odświeżana pozycja?

- nie masz problemów z odróżnieniem odbić fali od właściwego impulsu?

- jaki maksymalny zasięg uzyskałeś?

- jaka jest dokładność systemu? Na filmiku widać, że współrzędne się zmieniają, ale ile ma to wspólnego z prawdą nie wiadomo...