![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
nowe + funkcje
Fork of CZOSN by
main.cpp
- Committer:
- yruiewyrui3
- Date:
- 2016-06-14
- Revision:
- 1:da4c204737bf
- Parent:
- 0:2e73d45d090e
File content as of revision 1:da4c204737bf:
#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(int i) { 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; cleanBuffer(bufor); 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())) while (bufor[9] == NULL){ fill_buffer(i); } if(isCorrectPacket(bufor)){ t.stop(); t.reset(); t.start(); } if(bufor[0]=='@'){ move_wheels(); } else if(bufor[0]=='&'){ move_camera(); } i=0; cleanBuffer(bufor); } }