File content as of revision 30:39a53ed11b7d:

#include "mbed.h"
#include "Servo.h"

#include "servo_bewegung.h"

#define RIGHT         1200
#define MID           1000
#define MAX           2000
#define LEFT          800
#define STEP          25
//Time delay between steps in milliseconds
#define TIME          100


void bewegung(int servo_id){
    switch(servo_id) {
    case 1:{
        
        Servo servo1(PA_0/*PB_2*/);
        servo1.Enable(MID,MAX);
    
        //Servo Bewegung
        for (int pos = MID; pos > LEFT; pos -= STEP) {
            servo1.SetPosition(pos);
            thread_sleep_for(TIME);
        }
        for(int j=0; j<2; j++){
            for (int pos = LEFT; pos < RIGHT; pos += STEP) {
                servo1.SetPosition(pos);
                thread_sleep_for(TIME);
            }
            for (int pos = RIGHT; pos > LEFT; pos -= STEP) {
                servo1.SetPosition(pos);
                thread_sleep_for(TIME);
            }
        }
        for (int pos = LEFT; pos < MID; pos += STEP) {
            servo1.SetPosition(pos);
            thread_sleep_for(TIME);
        }
        thread_sleep_for(TIME);
        servo1.Disable();
        break;
        }
    case 2:{
        
        Servo servo2(PA_1/*PC_8*/);
        servo2.Enable(MID,MAX);
        
        //Servo Bewegung
        for (int pos = MID; pos > LEFT; pos -= STEP) {
            servo2.SetPosition(pos);
            thread_sleep_for(TIME);
        }
        for(int j=0; j<2; j++){
            for (int pos = LEFT; pos < RIGHT; pos += STEP) {
                servo2.SetPosition(pos);
                thread_sleep_for(TIME);
            }
            for (int pos = RIGHT; pos > LEFT; pos -= STEP) {
                servo2.SetPosition(pos);
                thread_sleep_for(TIME);
            }
        }
        for (int pos = LEFT; pos < MID; pos += STEP) {
            servo2.SetPosition(pos);
            thread_sleep_for(TIME);
        }
        thread_sleep_for(TIME);
        servo2.Disable();
        break;
        }
    case 3:{
        
        Servo servo3(PA_4/*PC_6*/);
        servo3.Enable(MID,MAX);

        //Servo Bewegung
        for (int pos = MID; pos > LEFT; pos -= STEP) {
            servo3.SetPosition(pos);
            thread_sleep_for(TIME);
        }
        for(int j=0; j<2; j++){
            for (int pos = LEFT; pos < RIGHT; pos += STEP) {
                servo3.SetPosition(pos);
                thread_sleep_for(TIME);
            }
            for (int pos = RIGHT; pos > LEFT; pos -= STEP) {
                servo3.SetPosition(pos);
                thread_sleep_for(TIME);
            }
        }
        for (int pos = LEFT; pos < MID; pos += STEP) {
            servo3.SetPosition(pos);
            thread_sleep_for(TIME);
        }
        thread_sleep_for(TIME);
        servo3.Disable();
        break;
        }
    default:{
        printf("servo not found");
        break;
        }
    }
}