Diff: suncokret.h

Revision:

4:e360384c268b

Parent:

3:bd92b23688ae

--- a/suncokret.h	Fri Feb 07 19:27:04 2020 +0000
+++ b/suncokret.h	Mon Jun 08 17:48:51 2020 +0000
@@ -1,36 +1,27 @@
 #include "mbed.h"
-#define DULJINA_PULSA1 0.001
+#define VRIJEME_KRETANJA 0.1
 #define STANI 0
 #define CW 0.0035
 #define CCW 0.006
+
 Serial pc(SERIAL_TX, SERIAL_RX);
+
 AnalogIn in0(A0);
 AnalogIn in1(A1);
 AnalogIn in2(A2);
 AnalogIn in3(A3);
 DigitalOut ledica(D4);
 
-Timeout cekanje;
-
-PwmOut pwmd(D5);
-PwmOut pwmg(D6);
-
 float fotootp1=0, fotootp2=0, fotootp3=0, fotootp4=0;
-//float gore, dolje, lijevo, desno, sum;
-float ref, refV, refM;
-
+float a12, a13, a14, a21, a23, a24, a31, a32, a34, a41, a42, a43;
 int pozicija;
 
-/*
-void sumiranje(){
-    gore = fotootp1 + fotootp2;
-    dolje = fotootp3 + fotootp4;
-    lijevo = fotootp1 + fotootp3;
-    desno = fotootp2 + fotootp4;
-    
-    sum = fotootp1+fotootp2+fotootp3+fotootp4;
-    }
-*/
+PwmOut pwmd(D5);
+PwmOut pwmg(D9);
+
+Timeout cekanje;
+
+
 void ocitanje(){
        /* fotootp1 = in0.read();
         fotootp2 = in2.read();
@@ -46,16 +37,77 @@
     fotootp4 = in3.read();
     printf("\n\r%f\n\n", fotootp4);
     wait(2);
-        
-        //sumiranje();
-
     }
     
+void zakljucivanje(){
+    a12= fotootp1 - fotootp2;
+    a13= fotootp1 - fotootp3;
+    a14= fotootp1 - fotootp4;
+    
+    a21 = fotootp2 - fotootp1;
+    a23 = fotootp2 - fotootp3;
+    a24 = fotootp2 - fotootp4;
+    
+    a31 = fotootp3 - fotootp1;
+    a32 = fotootp3 - fotootp2;
+    a34 = fotootp3 - fotootp4;
+    
+    a41 = fotootp4 - fotootp1;
+    a42 = fotootp4 - fotootp2;
+    a43 = fotootp4 - fotootp3; 
+    
+    if (a12 >= 0.2 && a13 >= 0.2 && a14 >= 0.2){
+        pozicija = 1;
+        }
+        
+    else if (a21 >= 0.2 && a23 >= 0.2 && a24 >= 0.2){
+        pozicija = 2;
+        }
+        
+    else if (a31 >= 0.2 && a32 >= 0.2 && a34 >= 0.2){
+        pozicija = 3;
+        }
+        
+    else if (a41 >= 0.2 && a42 >= 0.2 && a43 >= 0.2){
+        pozicija = 4;
+        }
+        
+    else if (!(a12 >= 0.2 )&& a13 >= 0.2 && a14 >= 0.2){
+        pozicija = 5;
+        }
+        
+    else if (a21 >= 0.2 && !(a23 >= 0.2) && a24 >= 0.2){
+        pozicija = 6;
+        }
+        
+    else if (a31 >= 0.2 && a32 >= 0.2 && !(a34 >= 0.2)){
+        pozicija = 7;
+        }
+    
+    else if (!(a41 >= 0.2) && a42 >= 0.2 && a43 >= 0.2){
+        pozicija = 8;
+        }
+    else {
+        pozicija = 9;
+    }
+        
+
+}
+
+//OUTPUT     ------------------------------------
+void stani(){
+    pwmg.period_ms(20);
+    while(1){
+        pwmg.pulsewidth(STANI);
+        break;
+        }
+    }
     
 void gore(){
     pwmg.period_ms(20);
     while(1){
         pwmg.pulsewidth(CCW);
+        break;
         }
     }
     
@@ -63,6 +115,7 @@
     pwmd.period_ms(20);
     while(1){
         pwmd.pulsewidth(CCW);
+        break;
         }
     }
     
@@ -70,6 +123,7 @@
         pwmg.period_ms(20);
     while(1){
         pwmg.pulsewidth(CW);
+        break;
         }
     }
     
@@ -77,23 +131,53 @@
     pwmd.period_ms(20);
     while(1){
         pwmd.pulsewidth(CW);
+        break;
         }
     }
     
+//OUTPUT -------------------------
 
 void pozicioniranje(){
     switch(pozicija){
         case 1:
-            cekanje.attach(&gore, 0.1);
+            cekanje.attach(&gore, VRIJEME_KRETANJA);
+            pc.printf("\n\nNALAZIMO SE U POZICIJI %d\n", pozicija);
             break;
         case 2:
-            cekanje.attach(&desno, 0.1);
+            cekanje.attach(&desno, VRIJEME_KRETANJA);
+            pc.printf("\n\nNALAZIMO SE U POZICIJI %d\n", pozicija);
             break;
         case 3:
-            cekanje.attach(&dolje, 0.1);
+            cekanje.attach(&dolje, VRIJEME_KRETANJA);
+            pc.printf("\n\nNALAZIMO SE U POZICIJI %d\n", pozicija);
             break;
         case 4:
-            cekanje.attach(&lijevo, 0.1);
+            cekanje.attach(&lijevo, VRIJEME_KRETANJA);
+            pc.printf("\n\nNALAZIMO SE U POZICIJI %d\n", pozicija);
+            break;          
+        case 5:
+            cekanje.attach(&desno, VRIJEME_KRETANJA);
+            cekanje.attach(&gore, VRIJEME_KRETANJA);
+            pc.printf("\n\nNALAZIMO SE U POZICIJI %d\n", pozicija);
+            break;
+        case 6:
+            cekanje.attach(&desno, VRIJEME_KRETANJA);
+            cekanje.attach(&dolje, VRIJEME_KRETANJA);
+            pc.printf("\n\nNALAZIMO SE U POZICIJI %d\n", pozicija);
+            break;
+        case 7:
+            cekanje.attach(&lijevo, VRIJEME_KRETANJA);
+            cekanje.attach(&dolje, VRIJEME_KRETANJA);
+            pc.printf("\n\nNALAZIMO SE U POZICIJI %d\n", pozicija);
+            break;
+        case 8:
+            cekanje.attach(&lijevo, VRIJEME_KRETANJA);
+            cekanje.attach(&gore, VRIJEME_KRETANJA);
+            pc.printf("\n\nNALAZIMO SE U POZICIJI %d\n", pozicija);
+            break;
+        case 9:
+            cekanje.attach(&stani, VRIJEME_KRETANJA);
+            pc.printf("\n\nNALAZIMO SE U POZICIJI %d\n", pozicija);
             break;
         default:
             pc.printf("pogresna informacija");
@@ -101,24 +185,16 @@
         }
     }
 
-void logika(){
-    
-    }
+/*   
+
+
+
 
 void trazenje(){
-    pwmd.period_ms(20);
     while(1){
-        //ocitanje();
-        //pozicioniranje();
-        //logika();
-        refV = ref + 0.2;
-        refM = ref - 0.2;
-        //if(sum >= refV & sum <= refV){
-        //pwmd.pulsewidth(0.020);
-        pwmd=0.9;
-        ledica=1;
-        pc.printf("AAAAAAAAAAA");
-           // }
+            //logika za trazenje (spremanje neke vrijednosti) // 3 timera jedan koji broji kolko treba vremena, 
+                                                    //jedan se reseta kada nađe najvecu i jedan za odbrojavanje do pozicije
+            //logika();
         }
     }
-    
+*/