Diff: main.cpp

Revision:

5:a9da7bc89b24

Parent:

4:bd1db680e5a1

Child:

6:ee243782bf51

--- a/main.cpp	Fri Oct 14 10:22:22 2016 +0000
+++ b/main.cpp	Mon Oct 17 14:25:12 2016 +0000
@@ -4,23 +4,30 @@
 #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 (MOETEN UITEINDELIJK DE POSITIE IN CALIBRATIE ZIJN)
-volatile float x = -200.0;    
-volatile float y = 200.0; 
+// 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)
-volatile const float Bar = 200.0;    // lengte van de armen 
+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 = 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 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;
@@ -31,31 +38,70 @@
 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
-volatile bool BicepsLinks = true;
+// 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 = 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 () {
-    // 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
+        // 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");
     }
-    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;
@@ -63,8 +109,8 @@
     else if (x < -200) {
         x = -200;
     }
-    if (y > 300) {
-        y = 300;
+    if (y > 306) {
+        y = 306;
     }
     else if (y < 50) {
         y = 50;         // GOKJE, UITPROBEREN
@@ -105,10 +151,22 @@
     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
+// 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;
@@ -132,18 +190,34 @@
     //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 (FlipperAan) {   // als FlipperAan is true dan de beweging gaan maken
-        pc.printf("De flipper moet bewegen\r\n");
+    if (BeginnenMetFlippen == true) {
+        // SJOERDS DEEL 
     }
-    else {              // als FlipperUit dan doe niks
+    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