File content as of revision 12:515bacd56d07:

#include "Servo.h"
#include "mbed.h"
#include "FastPWM.h"
#define M_PI 3.14159265358979323846  /* pi */
#include <math.h>
Serial pc(USBTX, USBRX);
Ticker servo_ticker;
Servo mijnServo(D7);
DigitalIn servo_button_pressed(SW3);
//InterruptIn button(SW3);
//int Position= mijnServo.read();
//bool servo_button_pressed;

//void flag(void){
 //   servo_button_pressed=!servo_button_pressed;
//}

void servo()
{
    double theta_s= 500;       // dit zijn allemaal waarden tussen de 500-2400
    double theta_sout;          // dit zijn allemaal waarden tussen de 500-2400
    double q1=0; //     0.2*M_PI;               // dit is in rad uit I.K
    double q2=0;      //0.5*M_PI;               // 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;
            if (theta_sout>=2400)
            {   theta_sout=2400;}
            if (theta_sout<=500)
            {   theta_sout=500;}
            else 
            {   theta_sout;
            }
            mijnServo.SetPosition(theta_sout);
            pc.printf("De servo staat op %f graden. in de klapstand\n\r", theta_sout);
        }
    else 
        {
            theta_sout=theta_s-q1_ser+q2_ser;
             if (theta_sout>=2400)
            {   theta_sout=2400;}
            if (theta_sout<=500)
            {   theta_sout=500;}
            else {   theta_sout;
            }
            mijnServo.SetPosition(theta_sout);
            pc.printf("De servo staat op %f graden.\n\r", theta_sout);
        }
}
int main(void)
    {   
    mijnServo.Enable(1500,20000);
    servo();
    servo_ticker.attach(servo,0.5);
    //button.fall(flag);
    while(1){}
    }