File content as of revision 1:1e01b12063cd:

//*****biblioteki*****

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

//*****piny*****

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

//*****deklaracja zmiennych*****

const int BufferSize=15;
char buffer1[BufferSize];
char buffer2[BufferSize];
char camove[2];
char tablicaf[4];
int wall=30;        // odleglosc od sciany w cm
int act_vertical=0, act_horizontal=0, move=0, val=0;
Timer t, t_sonar, t_fidu;
long distance;
volatile unsigned int speed_car = 0;

//*****funkcje*****

void cleanBuffer1()             // czyszczaca bufor
{
    for(int i=0; i<=BufferSize; i++)
        buffer1[i]=NULL;
}

void cleanBuffer2()             // czyszczaca bufor2
{
    for(int i=0; i<=BufferSize; i++)
        buffer2[i]=NULL;
}

int what_command(char* buffer1){             // rozpoznajaca komende
    if ((buffer1[0]=='f') && (distance>30))
        return 1;
    else if ((buffer1[0]=='t') && (distance>30))
        if (buffer1[2]=='l')
            return 2;
        else
            return 3;
    else if (buffer1[0]=='b')
        return 4;
    else if (buffer1[0]=='S')
        return 5;
    else if (buffer1[0]=='l')
        return 6;
    else if (buffer1[0]=='r')
        return 7;
    else if (buffer1[0]=='v')
        if (buffer1[4]=='l')
            return 8;
        else
            return 10;
    else if (buffer1[0]=='h')
        if (buffer1[4]=='l')
            return 9;
        else
            return 11;
    else if (buffer1[0]=='_')
        return 12;
    else
        return 0;
}

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);
        tab[3]= ';';
        return tab;
}

void hard_stop()             // zatrzymujaca robota
{
    eng_left.move(0);
    eng_right.move(0); 
}

int map(int value, int from_min, int from_max, int to_min, int to_max)             // mapujaca wartosc do skretu
{
    return (value - from_min) * (to_max - to_min)/(from_max - from_min) + to_min;
}

void move_camera_down(int val)             // sterujaca kamera
{
    act_vertical = act_vertical + val;
    if(act_vertical>90){
        act_vertical=90;
    }
    cam_vertical.position(act_vertical);
}

void move_camera_up(int val)             // sterujaca kamera
{
    act_vertical = act_vertical - val;
    if(act_vertical<-90){
        act_vertical=-90;
    }
    cam_vertical.position(act_vertical);
}

void move_camera_right(int val)             // sterujaca kamera
{
    act_horizontal = act_horizontal - val;
    if(act_horizontal<-90){
        act_horizontal=-90;
    }
    cam_horizontal.position(act_horizontal);
}

void move_camera_left(int val)             // sterujaca kamera
{
    act_horizontal = act_horizontal + val;
    if(act_horizontal>90){
        act_horizontal=90;
    }
    cam_horizontal.position(act_horizontal);
}

void set_camera_zero()             // zerujaca kamere
{
     cam_horizontal.position(-8);
     cam_vertical.position(-20);
     act_horizontal=-8;
     act_vertical=-20;       
}

void set_val()          // odczytująca wartosc
{
    for (int q=0; q<3; q++){
        buffer2[q]=buffer1[q+8];
        }
        sscanf(buffer2, "%3d", &val);
}

int main(){
    stm.baud(115200);
    cleanBuffer1();
    cleanBuffer2(); 
    set_camera_zero();
    t_sonar.start();
    t_fidu.start();
    int i=0;
    
    while(true){
        //stm.printf("Poczatek petli nr %d \n", i);
        if(t.read_ms()>500)         //jesli przez ponad 500ms nie ma nowej ramki, zatrzymujemy robota
            { 
            eng_left.move(0);
            eng_right.move(0);
            }
        if(t_sonar.read_ms()>1000)          //odczyt odleglosci co 1,2s
        {
            distance = sensor.distance();
            //stm.printf("distance z czujnika: %d\n",distance);
            
            t_sonar.stop();
            t_sonar.reset();
            t_sonar.start();
        }
        while(stm.readable()){
            
            /*stm.gets(buffer1, 13);
            stm.puts(buffer1);
            break;}*/

            for (int x=0; x<12; x++){
                if(t_fidu.read_ms()>1000){
                    stm.printf("urywam;");
                    t_fidu.stop();
                    t_fidu.reset();
                    break;}
                buffer1[x]=stm.getc();
                if(buffer1[x]==';'){
                t_fidu.stop();
                t_fidu.reset();
                t_fidu.start();
                    break;}
            }
            stm.puts(buffer1);
            break;}
        
        set_val();
        
        t.stop();
        t.reset();
        t.start();
        switch (what_command(buffer1))
        {
            case 1:
                eng_left.move(val);
                eng_right.move(val); 
                set_camera_zero();
                speed_car = val;
                break;
            case 2:
                eng_right.move(speed_car);
                eng_left.move(speed_car - map(val, 0, 100, 0, speed_car));
                cam_horizontal.position(7);
                cam_vertical.position(-20);
                break;
            case 3:
                eng_left.move(speed_car);
                eng_right.move(speed_car - map(val, 0, 100, 0, speed_car));
                cam_horizontal.position(-23);
                cam_vertical.position(-20);
                break;
            case 4:
                eng_left.move(-val);
                eng_right.move(-val); 
                set_camera_zero();
                speed_car = -val;
                break;
            case 5:
                hard_stop();
                speed_car = 0;
                break;
            case 6:
                eng_left.move(-val);
                eng_right.move(val); 
                break;
            case 7:
                eng_left.move(val);
                eng_right.move(-val);
                break;
            case 8:
                move_camera_down(val);
                break;
            case 9:
                move_camera_left(val);
                break;
            case 10:
                move_camera_up(val);
                break;
            case 11:
                move_camera_right(val);
                break;
            case 12:
                set_camera_zero();
                break;
            default:
                //stm.printf("unknown_command \n");
                break;
        }
        cleanBuffer1();
        cleanBuffer2();
        
        i++;
        
        //wait_ms(200);
    }
}