File content as of revision 4:bd1db680e5a1:

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

Serial pc(USBTX,USBRX);
Ticker TickerVeranderenXY;
Ticker TickerAfstandBerekenen;
Ticker TickerMotorHoekBerekenen;
Ticker TickerBerekenenBewegingsHoek;
Ticker TickerFlipperBewegen;

// in te voeren x en y waardes (MOETEN UITEINDELIJK DE POSITIE IN CALIBRATIE ZIJN)
volatile float x = -200.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 VorigeHoek1 = 0.5f*pi;     // begin hoek van 90 graden
volatile float VorigeHoek2 = 0.5f*pi; 
volatile float step = 1;    // EEN GOK VOOR HET STAPJE EN MOET DUS NOG AANGEPAST

volatile float Dia1 = 0.0;
volatile float Dia2 = 0.0;
volatile float MotorHoek1 = 0.0;
volatile float MotorHoek2 = 0.0;
volatile float BewegingsHoek1 = 0.0;
volatile float BewegingsHoek2 = 0.0;
volatile bool Richting1 = true;     // cw = true, ccw = false
volatile bool Richting2 = true;     
volatile bool FlipperAan = false;   // als true dan flipper aanzetten

// waardes die komen van Charlotte: true is aangespannen, false is niet aangespannen
volatile bool BicepsLinks = true;
volatile bool BicepsRechts = true;
volatile bool Been = true;

// het berekenen van de nieuwe x en y en zorgen dat deze niet buiten het bereik komen
void VeranderenXY () {
    // de beweging
    if (BicepsLinks==true && BicepsRechts==true && Been==true) {   
        FlipperAan = true;  // flipper aan        
    }
    else if (BicepsLinks==true && BicepsRechts==false && Been==false) {
        x = x - step;       // naar links
    }
    else if (BicepsLinks==false && BicepsRechts==true && Been==false) {
        x = x + step;       // naar rechts
    }
    else if (BicepsLinks==true && BicepsRechts==true && Been==false) {
        y = y + step;       // naar voren
    }
    else if (BicepsLinks==false && BicepsRechts==true && Been==true) {
        y = y - step;       // naar achter
    }
    
    // Grenswaardes     LET OP: ARMEN MISSCHIEN GEBLOKKEERD DOOR BALK AAN DE BINNENKANT
    if (x > 200) {
        x = 200;
    }
    else if (x < -200) {
        x = -200;
    }
    if (y > 300) {
        y = 300;
    }
    else if (y < 50) {
        y = 50;         // GOKJE, UITPROBEREN
    }
    pc.printf("x = %f, y = %f\r\n", x, y);
}

// 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.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);
    }
    // twee if loop doorlopen voor motor 2
    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);
}        

// het verschil uitrekenen tussen waar we zijn en waar we heen willen
void BerekenenBewegingsHoek () {
    // Hoek 1: grootte beweging en richting
    BewegingsHoek1 = VorigeHoek1 - MotorHoek1;
    VorigeHoek1 = MotorHoek1;
    if (BewegingsHoek1 > 0) {
        Richting1 = false;      // ccw
    }
    else {
        Richting1 = true;       // cw
    }
    
    // Hoek 2: grootte beweging en richting
    BewegingsHoek2 = VorigeHoek2 - MotorHoek2;
    VorigeHoek2 = MotorHoek2;
    if (BewegingsHoek2 > 0) {
        Richting2 = true;       // cw
    }
    else {
        Richting2 = false;      // ccw
    }
    //pc.printf("Beweging 1 = %f, Beweging 2 = %f\r\n", BewegingsHoek1, BewegingsHoek2);
}  

void FlipperBewegen () {
    if (FlipperAan) {   // als FlipperAan is true dan de beweging gaan maken
        pc.printf("De flipper moet bewegen\r\n");
    }
    else {              // als FlipperUit dan doe niks
    }
}
        
int main() {
    pc.baud(SERIAL_BAUD);
    pc.printf("\r\n Nieuwe code uitproberen :) \r\n");
    
    TickerVeranderenXY.attach(&VeranderenXY,1);                     // ticker voor het veranderen van de x en y waardes
    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
    TickerBerekenenBewegingsHoek.attach(&BerekenenBewegingsHoek,1); // ticker om grote en richting vd beweging te berekenen
    TickerFlipperBewegen.attach(&FlipperBewegen,1);                 // ticker om de flipper te laten bewegen
    
    while (true) {
        
    }
}