File content as of revision 0:46a1642a10c8:

#include "mbed.h"
#include "math.h"
#define SERIAL_BAUD 115200

Serial pc(USBTX,USBRX);
Ticker TickerAfstandBerekenen;
// in te voeren x en y waardes (de positie waar we heen willen)
volatile float x = -30.0;    
volatile float y = 200.0; 

// waardes
volatile const float a = 50.0;       // de afstand van gekozen assenstelsel tot de armas (assenstelsel precies in het midden)
volatile const float Bar = 200.0;    // lengte van de armen 
//volatile float D1 = 9.0;


// functie om D1 en D2 te berekenen
float AfstandBerekenen ()
{
    float BV1 = sqrt(pow((a+x),2) + pow(y,2));  // diagonaal (afstand van armas tot locatie) berekenen
    float BV2 = sqrt(pow((x-a),2) + pow(y,2));
    
    float D1 = pow(BV1,2)/(2*BV1);     // berekenen van de afstand oorsprong tot diagonaal
    float D2 = pow(BV2,2)/(2*BV2);
    
    pc.baud(SERIAL_BAUD);
    pc.printf("D1 = %f en D2 = %f\r\n", D1, D2);
    
    return D1;
    //return D2;
}

/*
void MotorhoekBerekenen (){
    if (x > -a){
        float MotorHoek1 = 180 - atan(y/(x+a)) - acos(
*/ 
int main()
{
    
    //TickerAfstandBerekenen.attach(&AfstandBerekenen, 0.1);
    float D1 = AfstandBerekenen();
    //float D2 = AfstandBerekenen(x,y);
    pc.baud(SERIAL_BAUD);
    pc.printf("D1 = %f \r\n", D1, D2);
    //AfstandBerekenen();
    while (true) {
        
    }
}