Diff: main.cpp

Revision:

2:263a53d72bb7

Parent:

1:224bbe370e19

Child:

3:08bf605844eb

--- a/main.cpp	Mon Oct 23 08:24:15 2017 +0000
+++ b/main.cpp	Mon Oct 23 10:13:26 2017 +0000
@@ -3,6 +3,7 @@
 
 DigitalOut led(LED_RED);
 DigitalOut ledd(LED_GREEN);
+DigitalOut leddd(LED_BLUE);
 InterruptIn button(SW2);
 Timer t;
 Serial pc(USBTX, USBRX);
@@ -21,6 +22,7 @@
 
 int tellerX(){
     ledd=1; 
+    leddd=1;
     pc.baud(115200);
     while(true){
     button.fall(ledtX);      
@@ -29,8 +31,9 @@
     }*/
     t.start();
     huidigetijdX=t.read();
-    if (huidigetijdX>5){
+    if (huidigetijdX>1){
         led=1;          // ga door naar het volgende programma
+        return 0;
         }
     }  
 }   
@@ -41,15 +44,16 @@
     Yin++;
     pc.printf("Yin is %i\n",Yin);
     ledd=0;
-    led=1;
+    leddd=1;
     wait(0.5);
     ledd=1;
-    led=0;
+    leddd=0;
 }  
 
 int tellerY(){
     t.reset();
-    ledd=1; 
+    ledd=1;
+    leddd=0; 
     pc.baud(115200);
     while(true){
     button.fall(ledtY);      
@@ -58,13 +62,14 @@
     }*/
     t.start();
     huidigetijdY=t.read();
-    if (huidigetijdY>5){
-        led=1;    
+    if (huidigetijdY>1){
+        leddd=1; 
+        button.fall(0);   
         return 0;      // ga door naar het volgende programma
         }
     }
 }
-
+int B;
 float Pox = 0;
 float Poy = 0;
 float Pbx = 0;
@@ -75,15 +80,25 @@
 float Pey=308;
 float diamtrklosje=20;
 float pi=3.14159265359;
+float omtrekklosje=diamtrklosje*pi;
 float Lou;
 float Lbu;
 float Lru;
+float dLod;
+float dLbd;
+float dLrd;
 float roto;
 float rotb;
 float rotr;
+float rotzo;
+float rotzb;
+float rotzr;
 float counto;
 float countb;
-float countr; 
+float countr;
+float countzo;
+float countzb;
+float countzr; 
 float Psx;
 float Psy;
 float Vex;
@@ -103,7 +118,7 @@
     return 0;
 }
 float turns(){
-    float omtrekklosje=diamtrklosje*pi;             // omzetten van touwlengte naar rotaties
+    
     roto=Lou/omtrekklosje; 
     rotb=Lbu/omtrekklosje; 
     rotr=Lru/omtrekklosje; 
@@ -130,7 +145,7 @@
     pc.printf("\n\r R=%f",R);*/
     return 0;
 } 
-
+ 
 float Ps(){
     Psx=(Xin)*30+121;
     Psy=(Yin)*30+308;     
@@ -148,9 +163,9 @@
     Pst();
     pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey);
     if((abs(Vex)<0.01f)&&(abs(Vey)<0.01f)){
-        pc.printf("i'm done bitches"); // hier moet komen te staan dat die door moet naar het volgende programma (zakken)
-        zakken;
-    }
+        B=5;
+        loop.detach();
+        }
 }
 
 int calculator(){
@@ -160,16 +175,32 @@
     return 0;
 }
 
-int zakken(){
-    pc.printf("zak zak zak zak zak maar lekker door door door door");
-    return 0;
+void zakker(){
+    while(1){
+        wait(1);
+    if(B==5){                       // hierdoor wacht dit programma totdat de beweging klaar is
+    
+    dLod=sqrt(pow(Lou,2)+pow(397.85,2))-Lou; //dit is wat je motoren moeten doen om te zakken
+    dLbd=sqrt(pow(Lbu,2)+pow(397.85,2))-Lbu;
+    dLrd=sqrt(pow(Lru,2)+pow(397.85,2))-Lru;
+    rotzo=dLod/omtrekklosje;
+    rotzb=dLbd/omtrekklosje;
+    rotzr=dLrd/omtrekklosje;
+    countzo=rotzo*4200;
+    countzb=rotzb*4200;
+    countzr=rotzr*4200;
+    
+    pc.printf("o=%.2fb=%.2fr=%.2f",countzo,countzb,countzr);       // hier moet komen te staan hoe het zakken gaat
+    }
+    }
 }
 
 int main()
 {
     tellerX();
     tellerY();
+    
     calculator();
-    //zakker();
+    zakker();
     return 0;
 }
\ No newline at end of file