File content as of revision 2:35dd310320e5:

#include "mbed.h"
#include "Engine.h"
 
Serial stm(PA_2, PA_3);
const int BufferSize=17;
char bufor[BufferSize];
char prawa[3];
char lewa[3];
int p=0;
int l=0;
long counter=0;
Timer t;
 
 
 void cleanBuffer(char *buffor)
 {
    for(int i=0; i<BufferSize; i++)     
        buffor[i]=NULL;
}

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(((int)buffor[i+8]) < 48 || ((int)buffor[i+8]) > 57) 
            {
       // stm.printf("KAMERA GORA/DOL: NIE LICZBA\n");
       stm.printf("blad\n");
        return false;
        }
        if(((int)buffor[i+11]) < 48 || ((int)buffor[i+11]) > 57) 
            {
       // stm.printf("KAMERA LEWO/PRAWO: NIE LICZBA\n");
       stm.printf("blad\n");
        return false;
        }  
    }
    //stm.printf("pakiet poprawny\n");
    return true;
}
 
 
int main(){
    Engine eng_left = Engine(PB_13, PB_4, PB_10);
    Engine eng_right = Engine(PB_14, PB_5, PB_3);
    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[9] == NULL)&&(stm.readable()))
        if(bufor[9] == 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);
             
            
            for(int j=0; j<=2; j++)
            {
                lewa[j]=bufor[j+2];   
            }
                    
            for(int k=0; k<=2; k++)
            {
             
                prawa[k]=bufor[k+6];
            }
                    
            
            sscanf(prawa, "%3d", &p);
            sscanf(lewa, "%3d", &l);
                  
            if(abs(l)>100)
            {
                l=100;
            }
                        
            if(abs(p)>100)
            {
                p=100;
            }
            
           // stm.printf("%ld\n", counter);
            for(int j=0; j<=BufferSize; j++) //wypisz
            {
                stm.printf("%c", 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;
                        
            eng_left.move(l);
            eng_right.move(p);
                        
                 
        }
        i=0;
        cleanBuffer(bufor);
    }    
}