File content as of revision 4:84f836aaf390:

#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 = Servo(PB_6);
Servo cam_pion = Servo(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 dodaj_poziom[2];
char dodaj_pion[2];
int akt_pion=0, akt_poziom=0;
int l=0;
int p=0;
int po=0;
int pi=0;
Timer t, t_sonar, czas;
long distance;
int counter=0, wypisz_czas;

//*************** Funkcja czyszczaca bufor *****************
void cleanBuffer(char *buffor)
 {
    for(int i=0; i<BufferSize; i++)     
        buffor[i]=NULL;
        buffor[BufferSize]=NULL;
}

char* intToChar(int value, char tab[3])
{
        tab[0]= (char)(value/100 + 48);
        tab[1]= (char)((value-(100*(value/100)))/10 + 48);
        tab[2]= (char)(value-(100*(value/100))-(10*((value-(100*(value/100)))/10)) + 48);
        /*stm.printf("Tablica z distance to: ");
        for (int i=0; i<3; ++i)
            stm.printf("%c",tab[i]);
        stm.printf("\n");*/
        return tab;
}

void send2rpi(char tag, int wartosc){
    char tablicaf[3]; 
    stm.putc(tag);
    intToChar(wartosc, tablicaf);
    for (int f=0; f<3; f++){
         stm.putc(tablicaf[f]);
    }       
}

//***************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");       
        return false;
    }
    if((buffor[1] != '+') && (buffor[1] != '-'))
        {
       // stm.printf("ZLY ZNAK +/- LEWY\n");       
        return false;
    }
    if((buffor[5] != '+') && (buffor[5] != '-'))
    {
       // stm.printf("ZLY ZNAK +/- PRAWY\n");       
        return false;
    }
    if((bufor[9]!='$')&&(bufor[9]!='&'))
    {
       // stm.printf("ZLY ZNAK KONCA\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");
        return false;
        } 
        if(((int)buffor[i+4]) < 48 || ((int)buffor[i+4]) > 57) 
            {
       // stm.printf("NA PRAWE KOLO: NIE LICZBA\n");       
        return false;
        } 
    }
    //stm.printf("pakiet poprawny\n");]
    return true;
}
 
void actualize_speed()
{
        //*******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;
            }        
        //******KOREKCJA ZNAKU***********//
            if(bufor[1]=='-') l=-l;
            if(bufor[5]=='-') p=-p;    
    
}


void move_wheels()
{
            eng_left.move(l);
            eng_right.move(p);
}

void actualize_servo_values()
{    
        //*******PRZYPISANIE DO KAMERY POZIOM*********//   
            for(int m=0; m<2; m++){             
                dodaj_poziom[m]=bufor[m+3];
            }
        //*******PRZYPISANIE DO KAMERY PION*********//   
            for(int n=0; n<2; n++){
                dodaj_pion[n]=bufor[n+7];
            }
        //*******KONWERSJA CHAROW NA INTY*********//                    
            sscanf(dodaj_poziom, "%2d", &po);
            sscanf(dodaj_pion, "%2d", &pi);
        //********KOREKCJA***************// 
            if(bufor[1]=='-') po=-po;
            if(bufor[5]=='-') pi=-pi;
            
            akt_poziom = akt_poziom + po;
            akt_pion = akt_pion + pi;
            
            if(akt_poziom>90)
                akt_poziom=90;
            else if (akt_poziom<-90)
                akt_poziom=-90;
                                   
            if(akt_pion>90)
                akt_pion=90;
            else if (akt_pion<-90)
                akt_poziom=-90;                      
}


void move_camera()
{               
            cam_pion.SetPosition(akt_pion*5+1500);  
            cam_poziom.SetPosition(akt_poziom*5+1500);
             /*while(1) {
                  for (int pos = 1000; pos < 2100; pos += 10) {
                     cam_pion.SetPosition(pos);  
                     stm.printf("position %d\n", pos);
                     wait_ms(10);
                  }
                  for (int pos = 2100; pos > 1000; pos -= 10) {
                      cam_pion.SetPosition(pos); 
                      wait_ms(10); 
                  }
              }*/
}



int main(){
    int i = 0;
    cleanBuffer(bufor);
    stm.baud(115200);
    t_sonar.start();
    distance = 0;
    
    cam_poziom.Enable(1450,20000);
    cam_pion.Enable(1450,20000);
    move_camera();
    czas.start();
    
    while(true)
    {
        
        if(t.read_ms()>500){ //jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota
            eng_left.move(0);
            eng_right.move(0);
            }
            
        if(t_sonar.read_ms()>1200){
            distance = sensor.distance(); 
            t_sonar.stop();
            t_sonar.reset();
            t_sonar.start(); 
            }
        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();   
            counter++;
            if(czas.read_ms()>1000){
                stm.printf("counter %d\n", counter);
                counter=0;
                czas.stop();
                czas.reset();
                czas.start();   
            }           
        }
        if(bufor[9]=='$'){
            actualize_speed();
            if(distance>25){
                move_wheels();
            }
            else if (distance<=25){
                if(l<=0 && p<=0 || l*p<0){                
                    move_wheels();
                }
                else{
                eng_left.move(0);
                eng_right.move(0);                
                }
            }
        }
        else if(bufor[9]=='&'){            
            actualize_servo_values();
            move_camera();
           /* counter++;
            if(czas.read_ms()>1000){
               // stm.printf("counter %d\n", counter);
                counter=0;
                czas.stop();
                czas.reset();
                czas.start();   
            }*/
        }
        i=0;
        //send2rpi('s', distance);
        cleanBuffer(bufor);
        
    }    
}