File content as of revision 3:0de9cb065bee:

#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=17;
char bufor[BufferSize];
char prawa[3];
char lewa[3];
char poziom[2];
char pion[2];
int p=0;
int l=0;
int pi=0;
int po=0;
long counter=0;
Timer t;
 
//*************** Funkcja czyszczaca bufor *****************
void cleanBuffer(char *buffor)
 {
    for(int i=0; i<BufferSize; i++)     
        buffor[i]=NULL;
        buffor[BufferSize]=NULL;
}

//by Michal 8-06-2016
    //***************OPIS FUNKCJI isCorrectPacket******************************************************//
    //  funkcja sprawdza czy ramka spelnia wymagania protokolu: @ZCCCZCCC$ZCCZCC& gdzie Z={+/-} C={0,1,...9}  //
    //*******************KONIEC OPISU******************************************************************//
//by Dominik 8-06-2016  

bool isCorrectPacket(char *buffor){
    if(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 MIEDZY INFORMACJAMI\n");
       stm.printf("blad\n");
        return false;
    }
    if(bufor[16]!='^')
    {
       // 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;
        } 
    }
    if((buffor[10] != '+') && (buffor[10] != '-'))
        {
       // stm.printf("ZLY ZNAK +/- KAMERA PION\n");
       stm.printf("blad\n");
        return false;
        }
    if((buffor[13] != '+') && (buffor[13] != '-'))
        {
       // stm.printf("ZLY ZNAK +/- KAMERA POZIOM\n");
       stm.printf("blad\n");
        return false;
        }
    for(int i=1; i< 3; i++){
        if(((int)buffor[i+10]) < 48 || ((int)buffor[i+10]) > 57) 
        {
       // stm.printf("KAMERA POZIOM: NIE LICZBA\n");
       stm.printf("blad\n");
        return false;
        } 
        if(((int)buffor[i+13]) < 48 || ((int)buffor[i+13]) > 57) 
        {
       // stm.printf("KAMERA PION: NIE LICZBA\n");
       stm.printf("blad\n");
        return false;
        } 
    }
        
    
    //stm.printf("pakiet poprawny\n");]
    
    return true;
}
 
 
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);
            //stm.printf("TIMEOUT-ROBOT STOP\n");
            }
        
        //if((bufor[16] == NULL)&&(stm.readable()))
        if(bufor[16] == NULL)
        {
            bufor[i] = stm.getc();            
            if(i==0){
                if(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
                
//end added part//
 
            }else
                i++;
            
            continue;
        }           
        
        
        
        if(isCorrectPacket(bufor))
        {   
            t.stop();
            t.reset();
            t.start();
            counter++;
            //stm.printf("%ld\n", counter);
             
//*******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];
            }

//*******PRZYPISANIE DO KAMERY POZIOM*********//   
            for(int m=0; m<2; m++)
            {
             
                poziom[m]=bufor[m+11];
            }
            
//*******PRZYPISANIE DO KAMERY PION*********//   
            for(int n=0; n<2; n++)
            {
             
                pion[n]=bufor[n+14];
            }
            
//*******KONWERSJA CHAROW NA INTY*********//                    
            
            sscanf(prawa, "%3d", &p);
            sscanf(lewa, "%3d", &l);
            
            sscanf(poziom, "%2d", &po);
            sscanf(pion, "%2d", &pi);

 //********KOREKCJA***************//  
       
            if(abs(l)>100)
            {
                l=100;
            }
                        
            if(abs(p)>100)
            {
                p=100;
            }
            
            if(abs(po)>90)
            {
                po=90;
            }
                        
            if(abs(pi)>90)
            {
                pi=90;
            }
 //************WYPISYWANIE*******************//  
            
           // stm.printf("%ld\n", counter);
            for(int j=0; j<=BufferSize; j++) //wypisz
            {
                //stm.printf("%c", bufor[j]);
                stm.putc(bufor[j]);
            }
            //stm.printf("\n");
                        
            //stm.printf("liczba lewa: %d  \n", l);
            //stm.printf("liczba prawa: %d  \n", p);
                    
            if(bufor[1]=='-') l=-l;
            if(bufor[5]=='-') p=-p;
            if(bufor[10]=='-') po=-po;
            if(bufor[13]=='-') pi=-pi;
//**********ODPALANIE SILNIKOW******// 
            //stm.printf("poziom= %d\n", po);
            //stm.printf("pion= %d\n", pi);
            //stm.printf("lewy silnik= %d\n", l);
            //stm.printf("prawy silnik= %d\n", p);
            eng_left.move(l);
            eng_right.move(p);
            
            //for(int i=0; i<BufferSize; i++)
              //  {
                    stm.printf(bufor);
                //}            
//**********RUCH KAMERĄ******//
 
            cam_poziom.position(po);
            cam_pion.position(pi);                        
                 
        }
        i=0;
        cleanBuffer(bufor);
    }    
}