File content as of revision 5:87151d829108:

#include "mbed.h"
#include "Engine.h"
#include "Servo.h"
#include "hcsr04.h"

Serial stm(PA_2, PA_3);
Servo cam_horizontal(PB_6);
Servo cam_vertical(PC_7);
Engine eng_left = Engine(PB_13, PB_4, PB_10);
Engine eng_right = Engine(PB_14, PB_5, PB_3);
HCSR04 sensor(PB_9, PB_8, 11770);

const int BufferSize=10;
char buffer[BufferSize];
char right[3];
char left[3];
char add_horizontal[2];
char add_vertical[2];
int act_vertical=-20, act_horizontal=-5;
int l=0, r=0, h=0, v=0;
Timer t, t_sonar;
long distance;


void cleanBuffer()
{
    for(int i=0; i<=BufferSize; i++)
        buffer[i]=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);
        return tab;
}

//**************************OPIS FUNKCJI isCorrectPacket******************************************************//
//  funkcja sprawdza czy ramka spelnia wymagania protokolu: @ZCCCZCCC$ lub &ZCCCZCCC$ gdzie Z={+/-} C={0,1,...9} 

bool isCorrectPacket(){
    if (buffer[0]!='@')
        return false;
        
    if((buffer[1] != '+') && (buffer[1] != '-'))
        return false;
    
    if((buffer[5] != '+') && (buffer[5] != '-'))
        return false;
    
    if((buffer[9]!='$')&&(buffer[9]!='&'))
        return false;
    
    for(int i=2; i< 5; i++){
        if(((int)buffer[i]) < 48 || ((int)buffer[i]) > 57)
            return false;
        if(((int)buffer[i+4]) < 48 || ((int)buffer[i+4]) > 57)
            return false;
    }    
    return true;
}


void actualize_speed()
{
        //*******PRZYPISANIE DO TABLICY LEWEJ LICZBY*********//
            for(int j=0; j<=2; j++){
                left[j]=buffer[j+2];
            }
        //*******PRZYPISANIE DO TABLICY PRAWEJ LICZBY********//
            for(int k=0; k<=2; k++){
                right[k]=buffer[k+6];
            }
        //*******KONWERSJA CHAROW NA INTY********************//
            sscanf(left, "%3d", &l);
            sscanf(right, "%3d", &r);
            if(abs(l)>100)
                l=100;
            
            if(abs(r)>100)
                r=100;
            
            if(buffer[1]=='-') l=-l;
            if(buffer[5]=='-') r=-r;         
}

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

void actualize_servo_val()
{
        //*******PRZYPISANIE DO KAMERY POZIOM*********//
            for(int m=0; m<2; m++){
                add_horizontal[m]=buffer[m+3];
            }
        //*******PRZYPISANIE DO KAMERY PION**********//
            for(int n=0; n<2; n++){
                add_vertical[n]=buffer[n+7];
            }
        //*******KONWERSJA CHAROW NA INTY************//
            sscanf(add_horizontal, "%2d", &h);
            sscanf(add_vertical, "%2d", &v);
     
            if(buffer[1]=='-') h=-h;
            if(buffer[5]=='-') v=-v;

            act_horizontal = act_horizontal + h;
            act_vertical = act_vertical + v;

            if(act_horizontal>90)
                act_horizontal=90;
            else if (act_horizontal<-90)
                act_horizontal=-90;

            if(act_vertical>90)
                act_vertical=90;
            else if (act_vertical<-90)
                act_vertical=-90;
}

void move_camera()
{
     cam_horizontal.position(act_horizontal);
     cam_vertical.position(act_vertical);      
}


int main(){
    int i = 0;
    stm.baud(115200);
    cleanBuffer();   
    move_camera();
    t_sonar.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()>1000)//dczyt odleglosci co 1,2s
        {
            distance = sensor.distance();
            stm.printf("%d\n",distance);
            /*intToChar(distance, tablicaf);
            for (int f=0; f<3; f++)
            {
                stm.putc(tablicaf[f]);
            }*/
            t_sonar.stop();
            t_sonar.reset();
            t_sonar.start();
        }
        if(buffer[9] == NULL){
            buffer[i] = stm.getc();
            if(i==0){
                if(buffer[i]=='@') 
                   ++i;
            }
            else if(buffer[i]=='@'){ 
                cleanBuffer();
                buffer[0]='@';
                i=1; 
                }
            else i++;
            continue;
        }
        if(isCorrectPacket()){
            t.stop();
            t.reset();
            t.start();
        }
        if(buffer[9]=='$'){
            actualize_speed();
            if(distance>25)
                move_wheels();
            else if (distance<=25){
                if(l<0 && r<0 || l*r<0){
                    move_wheels();
                }
                else{
                eng_left.move(0);
                eng_right.move(0);
                }
            }
        }
        else if(buffer[9]=='&'){
            actualize_servo_val();
            move_camera();
        }
        i=0;
        cleanBuffer();
    }
}