File content as of revision 11:bb036c288cd7:

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

Serial pc(USBTX, USBRX);
Servo mijnServo(D7);
DigitalIn servo_button_pressed(D1);
//int Position= mijnServo.read();


void servo()
{
    double theta_s= 1450;       // dit zijn allemaal waarden tussen de 500-2400
   double theta_sout;          // dit zijn allemaal waarden tussen de 500-2400
    double q1;                  // dit is in rad uit I.K
    double q2;                  // dit is in rad uit I.K
    double q1_ser;              // dit is 500-2400
    double q2_ser;              // dit is 500-2400

    q1_ser= q1*604.7887837;
    q2_ser= q2*604.7887837;

    if  (servo_button_pressed.read() == false) 
        { 
            theta_sout=theta_s-q1_ser+q2_ser+400;
            mijnServo.SetPosition(theta_sout);
            pc.printf("De servo staat op %f graden.\n\r", theta_sout);
        }
    else 
        {
            theta_sout=theta_s-q1_ser+q2_ser;
            mijnServo.SetPosition(theta_sout);
            pc.printf("De servo staat op %f graden.\n\r", theta_sout);
        }
}
int main(void)
    {   servo();
        mijnServo.Enable(1500,20000);
    }