Diff: main.cpp

Revision:

0:2294f5ee0610

Child:

1:cad75f28ec58

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jun 01 10:47:53 2016 +0000
@@ -0,0 +1,78 @@
+#include "mbed.h"
+#include "Engine.h"
+
+Serial stm(PA_2, PA_3);
+char bufor[8];
+char prawa[3];
+char lewa[3];
+int p=0;
+int l=0;
+
+int main(){
+    Engine eng_left = Engine(PB_13, PB_4, PB_10);
+    Engine eng_right = Engine(PB_14, PB_5, PB_3);
+    int i = 0;
+    stm.baud(115200);
+    
+    while(true){
+        bufor[i] = stm.getc();
+        if(bufor[7] == NULL)
+        {
+            ++i;
+            continue;
+        }
+    
+        /*for (int j=0; j<8; j++)
+        {
+            stm.putc(bufor[j]);
+        }*/
+        for(int j=0; j<=2; j++)
+        {
+            lewa[j]=bufor[j+1];   
+        }
+        for(int k=0; k<=2; k++)
+        {
+            prawa[k]=bufor[k+5]; 
+        }
+        sscanf(prawa, "%3d", &p);
+        sscanf(lewa, "%3d", &l);
+        
+         if(abs(l)>100) {
+            l=100;
+        }
+        if(abs(p)>100) {
+            p=100;
+        }
+        
+        stm.printf("liczba lewa: %d  \n", l);
+        stm.printf("liczba prawa: %d  \n", p);
+        
+        if(bufor[0]=='-') l=-l;
+        if(bufor[4]=='-') p=-p;
+        
+        eng_left.move(l);
+        eng_right.move(p);
+      
+        if ((l==0) && (p==0)) {
+            stm.printf("Pale gume w miejscu bo kazali mi stac");    
+            }
+        else if ((l>0)&& (p>0)){            
+            stm.printf("Jade do przodu. Predkosc lewej strony to %d, a prawej strony to %d \n", l, p);
+            }
+        else if ((l<0)&& (p<0)){
+            stm.printf("Jade do tylu. Predkosc lewej strony to %d, a prawej strony to %d \n", l, p);                
+            }
+        else if ((l>=0)&& (p<=0)){
+            stm.printf("Obracam sie w prawo. Predkosc lewej strony to %d, a prawej strony to %d \n", l, p);
+            }
+        else if  ((l<=0)&& (p>=0)){
+            stm.printf("Obracam sie w lewo. Predkosc lewej strony to %d, a prawej strony to %d \n", l, p);
+            }        
+        
+        i=0;
+        for (int j=0; j<8; j++)
+        {
+            bufor[j]=NULL;
+        }
+    }    
+}
\ No newline at end of file