File content as of revision 0:b6608b36efd7:

#include "robot.h"

//float speed = 0.02;

/*void GoToPos1(float speed){
    //1. Arm vorne heben
    for(float offset = 0.0; (offset + 0.0011)<0.0018; offset+=0.00002) {
        arm_vorne.pulsewidth(0.0011 + offset); // servo position determined by a pulsewidth between 1-2ms
        wait(0.02);
    }
    arm_vorne.pulsewidth(P1_ARM_VORNE);
    arm_hinten.pulsewidth(P1_ARM_HINTEN);
    klaue.pulsewidth(P1_KLAUE);
    heben1.pulsewidth(P1_HEBEN);
    heben2.pulsewidth(P1_HEBEN);
}*/

float Drive(PwmOut servo, float posAkt, float posNeu, float speed) {
    if(posAkt < posNeu){
        for(float offset=0.0; (offset + posAkt)<=posNeu; offset+=0.00001) {
            servo.pulsewidth(posAkt + offset); // servo position determined by a pulsewidth between 1-2ms
            wait(speed);
        }
    } else {
        for(float offset=0.0; (posAkt - offset)>=posNeu; offset+=0.00001) {
            servo.pulsewidth(posAkt - offset); // servo position determined by a pulsewidth between 1-2ms
            wait(speed);
        }
    }
    return posNeu;
}
float Drive(PwmOut servo1, PwmOut servo2, float posAkt, float posNeu, float speed) {
    if(posAkt < posNeu){
        for(float offset=0.0; (offset + posAkt)<=posNeu; offset+=0.00001) {
            servo1.pulsewidth(posAkt + offset); // servo position determined by a pulsewidth between 1-2ms
            servo2.pulsewidth(posAkt + offset); // servo position determined by a pulsewidth between 1-2ms
            wait(speed);
        }
    } else {
        for(float offset=0.0; (posAkt - offset)>=posNeu; offset+=0.00001) {
            servo1.pulsewidth(posAkt - offset); // servo position determined by a pulsewidth between 1-2ms
            servo2.pulsewidth(posAkt - offset); // servo position determined by a pulsewidth between 1-2ms
            wait(speed);
        }
    }
    return posNeu;
}

/*void Welcome(void) {
    for(float offset=0.0; (offset + 0.0011)<0.0018; offset+=0.00002) {
        arm_vorne.pulsewidth(0.0011 + offset); // servo position determined by a pulsewidth between 1-2ms
        wait(0.02);
    }
    for(float offset=0.0; (0.0018 - offset)>0.0011; offset+=0.00002) {
        arm_vorne.pulsewidth(0.0018 - offset); // servo position determined by a pulsewidth between 1-2ms
        wait(0.02);
    }
}*/