nowe + funkcje
Diff: main.cpp
- Revision:
- 0:2e73d45d090e
diff -r 000000000000 -r 2e73d45d090e main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Jun 14 12:32:39 2016 +0000 @@ -0,0 +1,193 @@ +#include "mbed.h" +#include "Engine.h" +#include "Servo.h" + +//*************** Deklaracja wszystkich portów potrzebnych do obsługi************** +Serial stm(PA_2, PA_3); +Servo cam_poziom(PB_6); +Servo cam_pion(PC_7); +Engine eng_left = Engine(PB_13, PB_4, PB_10); +Engine eng_right = Engine(PB_14, PB_5, PB_3); + +//*************** Deklaracja zmiennych globalnych, tablic, bufora na ramke znaków ************** +const int BufferSize=10; +char bufor[BufferSize]; +char prawa[3]; +char lewa[3]; +char poziom[2]; +char pion[2]; +int l=0; +int p=0; +int po=0; +int pi=0; +Timer t; + +//*************** Funkcja czyszczaca bufor ***************** +void cleanBuffer(char *buffor) + { + for(int i=0; i<BufferSize; i++) + buffor[i]=NULL; + buffor[BufferSize]=NULL; +} + +//***************OPIS FUNKCJI isCorrectPacket******************************************************// +// funkcja sprawdza czy ramka spelnia wymagania protokolu: @ZCCCZCCC$ lub &ZCCCZCCC$ gdzie Z={+/-} C={0,1,...9} // +//*******************KONIEC OPISU******************************************************************// + +bool isCorrectPacket(char *buffor){ + if ((bufor[0]!='@') && (bufor[0]!='&')) + { + // stm.printf("ZLY ZNAK POCZATKU\n"); + stm.printf("blad\n"); + return false; + } + if((buffor[1] != '+') && (buffor[1] != '-')) + { + // stm.printf("ZLY ZNAK +/- LEWY\n"); + stm.printf("blad\n"); + return false; + } + if((buffor[5] != '+') && (buffor[5] != '-')) + { + // stm.printf("ZLY ZNAK +/- PRAWY\n"); + stm.printf("blad\n"); + return false; + } + if(bufor[9]!='$') + { + // stm.printf("ZLY ZNAK KONCA\n"); + stm.printf("blad\n"); + return false; + } + for(int i=2; i< 5; i++){ + if(((int)buffor[i]) < 48 || ((int)buffor[i]) > 57) + { + // stm.printf("NA LEWE KOLO: NIE LICZBA\n"); + stm.printf("blad\n"); + return false; + } + if(((int)buffor[i+4]) < 48 || ((int)buffor[i+4]) > 57) + { + // stm.printf("NA PRAWE KOLO: NIE LICZBA\n"); + stm.printf("blad\n"); + return false; + } + } + //stm.printf("pakiet poprawny\n");] + return true; +} + +void move_wheels() +{ + //*******PRZYPISANIE DO TABLICY LEWEJ LICZBY*********// + for(int j=0; j<=2; j++){ + lewa[j]=bufor[j+2]; + } + //*******PRZYPISANIE DO TABLICY PRAWEJ LICZBY*********// + for(int k=0; k<=2; k++){ + prawa[k]=bufor[k+6]; + } + //*******KONWERSJA CHAROW NA INTY*********// + sscanf(lewa, "%3d", &l); + sscanf(prawa, "%3d", &p); + //********KOREKCJA***************// + if(abs(l)>100){ + l=100; + } + if(abs(p)>100){ + p=100; + } + //************WYPISYWANIE*******************// + for(int j=0; j<=BufferSize; j++){ + stm.putc(bufor[j]); + } + //******KOREKCJA ZNAKU***********// + if(bufor[1]=='-') l=-l; + if(bufor[5]=='-') p=-p; + //**********ODPALANIE SILNIKOW******// + eng_left.move(l); + eng_right.move(p); + + stm.printf(bufor); +} +void move_camera() +{ + //*******PRZYPISANIE DO KAMERY POZIOM*********// + for(int m=0; m<2; m++){ + poziom[m]=bufor[m+3]; + } + //*******PRZYPISANIE DO KAMERY PION*********// + for(int n=0; n<2; n++){ + pion[n]=bufor[n+3]; + } + //*******KONWERSJA CHAROW NA INTY*********// + sscanf(poziom, "%2d", &po); + sscanf(pion, "%2d", &pi); + //********KOREKCJA***************// + if(abs(po)>90){ + po=90; + } + if(abs(pi)>90){ + pi=90; + } + //************WYPISYWANIE*******************// + for(int j=0; j<=BufferSize; j++){ + stm.putc(bufor[j]); + } + if(bufor[1]=='-') po=-po; + if(bufor[5]=='-') pi=-pi; + //**********RUCH KAMERĄ******// + cam_poziom.position(po); + cam_pion.position(pi); +} +void fill_buffer() +{ + bufor[i] = stm.getc(); + if(i==0){ + if((bufor[i]=='@') || (bufor[i]=='&')) //zaczynamy zapelniac bufor jak dostaniemy @ + ++i; + } + else if(bufor[i]=='@'){ // i != 0 //interpretujemy jakby potencjalny poczatek ramki + cleanBuffer(bufor); + //stm.printf("blad\n"); + bufor[0]='@'; + i=1; //bo zerowy znak '@' juz zczytal + } + else if (bufor[i]=='&'){ // i != 0 //interpretujemy jakby potencjalny poczatek ramki + cleanBuffer(bufor); + //stm.printf("blad\n"); + bufor[0]='&'; + i=1; //bo zerowy znak '@' juz zczytal + } + else + i++; +} + +int main(){ + int i = 0; + stm.baud(115200); + while(true){ + if(t.read_ms()>200){ //jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota + eng_left.move(0); + eng_right.move(0); + } + //if((bufor[9] == NULL)&&(stm.readable())) + if(bufor[9] == NULL){ + fill_buffer(); + continue; + } + if(isCorrectPacket(bufor)){ + t.stop(); + t.reset(); + t.start(); + } + if(bufor[0]=='@'){ + move_wheels(); + } + else if(bufor[0]=='&'){ + move_camera(); + } + i=0; + cleanBuffer(bufor); + } +} \ No newline at end of file