File content as of revision 5:a9da7bc89b24:

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

Serial pc(USBTX,USBRX);
InterruptIn Button1(PTC6);          // DIT WORDT EEN ANDERE BUTTON

// tickers
Ticker TickerCaseBepalen;
Ticker TickerVeranderenXY;
Ticker TickerAfstandBerekenen;
Ticker TickerMotorHoekBerekenen;
Ticker TickerBerekenenBewegingsHoek;
Ticker TickerFlipperBewegen;

// in te voeren x en y waardes 
volatile float x = 0.0;    
volatile float y = 305.5; 
volatile float vorigex = 0.0;
volatile float vorigey = 0.0;

// waardes
volatile const float a = 50.0;       // de afstand van gekozen assenstelsel tot de armas (assenstelsel precies in het midden)   KEERTJE NAMETEN
volatile const float Bar = 200.0;    // lengte van de armen                                                                     KEERTJE NAMETEN
volatile const float pi = 3.14159265359; 
volatile float VorigeHoek1 = 1.0/3.0*pi;     // begin hoek van 90 graden
volatile float VorigeHoek2 = 1.0/3.0*pi; 
volatile float Step = 1;    // EEN GOK VOOR HET STAPJE EN MOET DUS NOG AANGEPAST
volatile float MaxHoek = pi - 30*pi/180;   // de hoek voordat arm het opstakel raakt (max hoek is 24.9 tussen arm en opstakel)

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
bool BezigMetCalibreren = true;     // we beginnen met calibreren
bool BeginnenMetFlippen = false;    // om de flipper aan te zetten
bool BezigMetFlippen = false;       // om verandering in x en y tegen te houden tijdens het flippen. DIT WORDT VERANDERD IN SJOERDS DEEL (NAAM CONTROLEREN)

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

// nodig voor het switchstatement
enum states {Flipper, Links, Rechts, Voor, Achter, GeenSignaal};
states MyState = Links;

// het bepalen van welke case we hebben
void CaseBepalen () {
    if (BicepsLinks==true && BicepsRechts==true && Been==true && BezigMetFlippen == false) {   
        MyState = Flipper;      // flipper bewegen
    }
    else if (BicepsLinks==true && BicepsRechts==false && Been==false && BezigMetFlippen == false) {
        MyState = Links;        // naar links bewegen
    }
    else if (BicepsLinks==false && BicepsRechts==true && Been==false && BezigMetFlippen == false) {
        MyState = Rechts;       // naar rechts bewegen
    }
    else if (BicepsLinks==true && BicepsRechts==true && Been==false && BezigMetFlippen == false) {
        MyState = Voor;         // naar voren bewegen
    }
    else if (BicepsLinks==false && BicepsRechts==true && Been==true && BezigMetFlippen == false) {
        MyState = Achter;       // naar achter bewegen
    }
    else if (BicepsLinks==false && BicepsRechts==false && Been==false || BezigMetFlippen==true) {
        MyState = GeenSignaal;
    }
}    

// het berekenen van de nieuwe x en y en zorgen dat deze niet buiten het bereik komen
void VeranderenXY () {
        // bepalen hoe de x en y waardes gaan veranderen
    switch(MyState) {
        case Flipper :
            BeginnenMetFlippen = true;     // we zijn aan het flipper dus geen andere dingen doen
            break;
        case Links : 
            vorigex = x;    // ervoor zorgen dat als de motor te ver draait de x niet verder telt
            x = x - Step;
            break;
        case Rechts :
            vorigex = x;
            x = x + Step;
            break;
        case Voor :
            vorigey = y;    // ervoor zorgen dat als de motor te ver draait de y niet verder telt
            y = y + Step;
            break;
        case Achter :
            vorigey = y;
            y = y - Step;
            break;
        case GeenSignaal :
            break;
        default :
            pc.printf("Er is geen verandering in x en y\r\n");
    }
            
    // Grenswaardes     LET OP: ARMEN MISSCHIEN GEBLOKKEERD DOOR BALK AAN DE BINNENKANT
    if (x > 200) {
        x = 200;
    }
    else if (x < -200) {
        x = -200;
    }
    if (y > 306) {
        y = 306;
    }
    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);
    }
    
    // limiten (als de hoek te groot wordt, reset dan de hoek en de x en y waardes)
    if (MotorHoek1 >= MaxHoek) {    // is de maximale hoek in radialen
        MotorHoek1 = VorigeHoek1;
        x = vorigex;
        y = vorigey;
    }    
    if (MotorHoek2 >= MaxHoek) {
        MotorHoek2 = VorigeHoek2;
        x = vorigex;
        y = vorigey;
    }
    //pc.printf("MotorHoek1 = %f en MotorHoek2 = %f \r\n", MotorHoek1, MotorHoek2);
}        

// het verschil uitrekenen tussen waar we zijn en waar we heen willen       OVERLAP MET SJOERD
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);
}  

// als BeginnenMetFlippen aan staat voeren we deze functie uit. Zolang deze bezig is is BezigMetFlippen true
void FlipperBewegen () {
    if (BeginnenMetFlippen == true) {
        // SJOERDS DEEL 
    }
    else {      // als BezigMetFlippen == false dan doe niks
    }
}

// als de button ingedrukt wordt dan stoppen we met calibreren        
void ButtonIndrukken () {
        BezigMetCalibreren = false;
}        
        
int main() {
    pc.baud(SERIAL_BAUD);
    pc.printf("\r\n Nieuwe code uitproberen :) \r\n");
    
    // calibreren gebeurt hier, zodat de tickers nog niet lopen en dit dus geen rekenkracht kost en je niet gebonden bent aan tijden
    // we gaan door met calibreren tot we de button indrukken
    while (BezigMetCalibreren == true) {
        // potmeter dingen aanpassen
        
        pc.printf("calibreren is bezig\r\n");
        Button1.fall(&ButtonIndrukken);
    }
    
    TickerCaseBepalen.attach(&CaseBepalen, 1);                      // ticker om te bepalen welke case we gaan gebruiken
    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) {
        
    }
}