File content as of revision 0:2e73d45d090e:

#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);
    }    
}