File content as of revision 0:5919ea7b3b90:

#include "mbed.h"
#include "Engine.h"
#include "Servo.h"
#include "hcsr04.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 **************
HCSR04 sensor(PB_9, PB_8, 11770);
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, t_sonar;
float distance;
 
//*************** 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]!='@')
    {
       // 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]!='$')&&(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);
}
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+7];
            }
        //*******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);      
}

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(t_sonar.read_ms()>1200){
            t_sonar.stop();
            t_sonar.reset();
            t_sonar.start(); 
            distance = sensor.distance(); 
            stm.printf("dystans  %d  \n",distance);  
            }
        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);
                bufor[0]='@';
                i=1; //bo zerowy znak '@' juz zczytal
                }
            else i++;
        continue;
        }         
        if(isCorrectPacket(bufor)){   
            t.stop();
            t.reset();
            t.start();                    
        }
        if(bufor[9]=='$'){
        move_wheels();
        }
        else if(bufor[9]=='&'){
        move_camera();
        }
        i=0;
        cleanBuffer(bufor);
    }    
}