File content as of revision 1:234edfc8cac1:

#include "mbed.h"
#include "hcsr04.h"

DigitalOut led_1(LED1);
DigitalOut led_2(LED2);
DigitalOut led_3(LED3);
DigitalOut led_4(LED4);
Serial pc(USBTX,USBRX);
Serial HC06(p13, p14);
HCSR04 sensor(p12, p11);
PwmOut servo1(p21),servo2(p22),servo3(p23),servo4(p24);
Ticker t1,t2,t3,t4,t5;

void servomovedown();
void servomoveup();
void automode();
void checkdistance();
void distancesecure();
void writedistance();



int wert,distancecheck=0;
int servotickerstatus =0,automovestatus=0,righttickerstatus=0,lefttickerstatus=0;
float power;
long distance,distance1,distance2,distance3;

int main()
{
    servo1.period_ms(20);
    servo2.period_ms(20);
    servo3.period_ms(20);
    servo4.period_ms(20);


    char serChar, count;
    pc.baud(115200);
    pc.printf("\r\n HC06 is activ!n");
    pc.printf("\r\nTry to get connection to HC06\r\n");
    led_1 = 1;
    wait(0.6);
    led_1 = 0;
    wait(0.6);
    led_1= led_2= 1;
    count=0;
    wert = 1500;

    HC06.baud(9600);
    HC06.printf("mbed started!\n");

    wert=1500;

    while(1) {
        if (HC06.readable()) {
            count++;
            serChar = HC06.getc();
            pc.putc(serChar);
            switch (serChar) {
                case '1': {
                    servo1.pulsewidth_us(1000);
                    servo2.pulsewidth_us(1000);
                    break;
                }
                case '2':
                case '3':
                case '4':
                case '5':
                case '6':
                case '7':
                case '8':
                case '9': {
                    int mal = serChar - '0';
                    HC06.printf("Der Wert ist: %d\n",mal);
                    servo1.pulsewidth_us(1140+(50*mal));
                    servo2.pulsewidth_us(1020+50*mal);
                    break;
                }
                case 'b': {
                    if(servotickerstatus == 1) {
                        t1.detach();
                        servotickerstatus=0;
                        break;
                    }
                    if(servotickerstatus == 0) {
                        servotickerstatus = 1;
                        t1.attach(&servomovedown, 0.01);
                        break;
                    }
                }
                //wert= wert+100;servo4.pulsewidth_us(wert); break;
                case 'f': {
                    if(servotickerstatus == 1) {
                        t1.detach();
                        servotickerstatus=0;
                        break;
                    }
                    if(servotickerstatus == 0) {
                        servotickerstatus = 1;
                        t1.attach(&servomoveup, 0.01);
                        break;
                    }
                }
                case 'l':
                {
                     if(lefttickerstatus == 1) {
                        servo3.pulsewidth_us(1730);
                        wait(1);
                        servo3.pulsewidth_us(1500);
                        lefttickerstatus=0;
                        break;
                    }
                    if(lefttickerstatus == 0) {
                        lefttickerstatus = 1;
                        servo3.pulsewidth_us(1150);
                        break;
                    }}
                case 'r':
                    { if(righttickerstatus == 1) {
                        servo3.pulsewidth_us(1150);
                        wait(1);
                        servo3.pulsewidth_us(1500);
                        righttickerstatus=0;
                        break;
                    }
                    if(righttickerstatus == 0) {
                        righttickerstatus = 1;
                        servo3.pulsewidth_us(1730);
                        break;}}
                case 's':
                    servo3.pulsewidth_us(1500);
                    break;
                case 'a':
                    if(automovestatus == 1) {
                        t3.detach();
                        t4.detach();
                        automovestatus=0;
                        break;
                    }
                    if(automovestatus == 0) {
                        automovestatus = 1;
                        automode();
                        break;
                    }
                default:
                    led_1=led_3=0;
                    led_2=led_4=1;
                    break;
            }
        }
    }
}

void servomovedown()
{
    wert = wert + 10;
    servo4.pulsewidth_us(wert);
    if (wert > 2200) {
        t1.detach();
        servotickerstatus=0;
    }
}

void servomoveup()
{
    wert = wert - 10;
    servo4.pulsewidth_us(wert);
    if (wert < 870) {
        t1.detach();
        servotickerstatus=0;
    }
}

void automode()
{
    servo4.pulsewidth_us(900);
    servo1.pulsewidth_us(1120);
    servo2.pulsewidth_us(1050);
    t4.attach(&distancesecure,0.04);
    t3.attach(&checkdistance, 0.2);
    t5.attach(&writedistance, 2);
    return;

}

void checkdistance()
{
   // if (distance > 150) {
     //   servo1.pulsewidth_us(1120);
      //  servo2.pulsewidth_us(1050);
   // } else {
        power = 1733-((distance*10)/3);
        servo1.pulsewidth_us(power);
        servo2.pulsewidth_us(power-80);
   // }
}

void writedistance()
{
     HC06.printf(" %d", distance);
     pc.printf("%d\n", distance);
    }

void distancesecure()
{
    distancecheck ++;
    if (distancecheck == 1) {
        distance1 = sensor.distance();
    }
    if (distancecheck == 2) {
        distance2 = sensor.distance();
    }
    if (distancecheck == 3) {
        distance3 = sensor.distance();
    }
    if (distancecheck == 4) {
        distancecheck = 0;
        distance = ((distance1+distance2+distance3)/3);
    }

}