File content as of revision 3:ac6f86c0db6e:

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

Serial pc(USBTX,USBRX);
Ticker TickerAfstandBerekenen;
Ticker TickerMotorHoekBerekenen;

// 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 const float pi = 3.14159265359; 

volatile float Dia1 = 0.0;
volatile float Dia2 = 0.0;
volatile float MotorHoek1 = 0.0;
volatile float MotorHoek2 = 0.0;

//NOG ERGENS ZORGEN DAT X EN Y NIET BUITEN HET BEREIKBARE GEBIED KOMEN 

// functie om D1 en D2 te berekenen
void AfstandBerekenen (){
    float BV1 = sqrt(pow((a+x),2) + pow(y,2));  // diagonaal (afstand van armas tot locatie) berekenen
    Dia1 = pow(BV1,2)/(2*BV1);     // berekenen van de afstand oorsprong tot diagonaal
    
    float BV2 = sqrt(pow((x-a),2) + pow(y,2)); // zelfde nog een keer doen maar nu voor de rechter arm 
    Dia2 = pow(BV2,2)/(2*BV2); 
    
    pc.baud(SERIAL_BAUD);
    pc.printf("Dia1 = %f, Dia2 = %f\r\n", Dia1, Dia2);
}

// functie om de motorhoek te berekenen
void MotorHoekBerekenen (){
    // eerst if loop doorlopen voor motor 1
    if (x > -a) {
        MotorHoek1 = pi - atan(y/(x+a)) - acos(Dia1/Bar);
    }
    else if (x > -a) {
        MotorHoek1 = pi - (pi + atan(y/(x+a))) - acos(Dia1/Bar);
    }
    else {  // als x=-a
        MotorHoek1 = 0.5f*pi - acos(Dia1/Bar);
    }
    
    if (x < a) {
        MotorHoek2 = pi + atan(y/(x-a)) - acos(Dia2/Bar);
    }
    else if (x > a) {
        MotorHoek2 = pi - (pi - atan(y/(x-a))) - acos(Dia2/Bar);
    }
    else {  // als x=a
        MotorHoek2 = 0.5f*pi - acos(Dia1/Bar);
    }
    pc.printf("MotorHoek1 = %f en MotorHoek2 = %f \r\n", MotorHoek1, MotorHoek2);
}        

int main() {
    pc.baud(SERIAL_BAUD);
    pc.printf("\r\n Nieuwe code uitproberen :) \r\n");
    
    TickerAfstandBerekenen.attach(&AfstandBerekenen,1);     // ticker om de waardes van dia1 en dia 2 te berekenen
    TickerMotorHoekBerekenen.attach(&MotorHoekBerekenen,1); // ticker om de motorhoek te berekenen
    
    while (true) {
        
    }
}