Diff: main.cpp

Revision:

4:bd1db680e5a1

Parent:

3:ac6f86c0db6e

Child:

5:a9da7bc89b24

--- a/main.cpp	Thu Oct 13 14:33:26 2016 +0000
+++ b/main.cpp	Fri Oct 14 10:22:22 2016 +0000
@@ -1,27 +1,76 @@
 #include "mbed.h"
 #include "math.h"
+#include "iostream"
 #define SERIAL_BAUD 115200
-#include "iostream"
 
 Serial pc(USBTX,USBRX);
+Ticker TickerVeranderenXY;
 Ticker TickerAfstandBerekenen;
 Ticker TickerMotorHoekBerekenen;
+Ticker TickerBerekenenBewegingsHoek;
+Ticker TickerFlipperBewegen;
 
-// in te voeren x en y waardes (de positie waar we heen willen)
-volatile float x = -30.0;    
+// 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
 
-//NOG ERGENS ZORGEN DAT X EN Y NIET BUITEN HET BEREIKBARE GEBIED KOMEN 
+// 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 (){
@@ -31,8 +80,7 @@
     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);
+    //pc.printf("Dia1 = %f, Dia2 = %f\r\n", Dia1, Dia2);
 }
 
 // functie om de motorhoek te berekenen
@@ -47,7 +95,7 @@
     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);
     }
@@ -57,15 +105,50 @@
     else {  // als x=a
         MotorHoek2 = 0.5f*pi - acos(Dia1/Bar);
     }
-    pc.printf("MotorHoek1 = %f en MotorHoek2 = %f \r\n", MotorHoek1, MotorHoek2);
+    //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");
     
-    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
+    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) {