Revision 2:35dd310320e5, committed 2016-06-08

Comitter:

yruiewyrui3

Date:

Wed Jun 08 10:26:19 2016 +0000

Parent:

1:cad75f28ec58

Commit message:

.

Changed in this revision

--- a/main.cpp	Thu Jun 02 09:11:20 2016 +0000
+++ b/main.cpp	Wed Jun 08 10:26:19 2016 +0000
@@ -1,90 +1,176 @@
 #include "mbed.h"
 #include "Engine.h"
-
+ 
 Serial stm(PA_2, PA_3);
-char bufor[8];
+const int BufferSize=17;
+char bufor[BufferSize];
 char prawa[3];
 char lewa[3];
 int p=0;
 int l=0;
+long counter=0;
 Timer t;
+ 
+ 
+ void cleanBuffer(char *buffor)
+ {
+    for(int i=0; i<BufferSize; i++)     
+        buffor[i]=NULL;
+}
 
+bool isCorrectPacket(char *buffor){
+    if(bufor[0]!='@')
+    {
+       // stm.printf("ZLY ZNAK POCZATKU\n");
+       stm.printf("blad\n");
+        return false;
+    }
+    if((buffor[1] != '+') && (buffor[1] != '-'))
+        {
+       // stm.printf("ZLY ZNAK +/- LEWY\n");
+       stm.printf("blad\n");
+        return false;
+    }
+    if((buffor[5] != '+') && (buffor[5] != '-'))
+    {
+       // stm.printf("ZLY ZNAK +/- PRAWY\n");
+       stm.printf("blad\n");
+        return false;
+    }
+    if(bufor[9]!='$')
+    {
+       // stm.printf("ZLY ZNAK MIEDZY INFORMACJAMI\n");
+       stm.printf("blad\n");
+        return false;
+    }
+    if(bufor[16]!='&')
+    {
+       // stm.printf("ZLY ZNAK KONCA\n");
+       stm.printf("blad\n");
+        return false;
+    }
+    for(int i=2; i< 5; i++){
+        if(((int)buffor[i]) < 48 || ((int)buffor[i]) > 57) 
+        {
+       // stm.printf("NA LEWE KOLO: NIE LICZBA\n");
+       stm.printf("blad\n");
+        return false;
+        } 
+        if(((int)buffor[i+4]) < 48 || ((int)buffor[i+4]) > 57) 
+            {
+       // stm.printf("NA PRAWE KOLO: NIE LICZBA\n");
+       stm.printf("blad\n");
+        return false;
+        } 
+        if(((int)buffor[i+8]) < 48 || ((int)buffor[i+8]) > 57) 
+            {
+       // stm.printf("KAMERA GORA/DOL: NIE LICZBA\n");
+       stm.printf("blad\n");
+        return false;
+        }
+        if(((int)buffor[i+11]) < 48 || ((int)buffor[i+11]) > 57) 
+            {
+       // stm.printf("KAMERA LEWO/PRAWO: NIE LICZBA\n");
+       stm.printf("blad\n");
+        return false;
+        }  
+    }
+    //stm.printf("pakiet poprawny\n");
+    return true;
+}
+ 
+ 
 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){
         
-        if(t.read_ms()>200){
+        if(t.read_ms()>200){ //jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota
             eng_left.move(0);
             eng_right.move(0);
+            //stm.printf("TIMEOUT-ROBOT STOP\n");
             }
         
-        if((bufor[7] == NULL)&&(stm.readable()))
+        //if((bufor[9] == NULL)&&(stm.readable()))
+        if(bufor[9] == NULL)
         {
-            bufor[i] = stm.getc();
-            ++i;
+            bufor[i] = stm.getc();            
+            if(i==0){
+                if(bufor[i]=='@') //zaczynamy zapelniac bufor jak dostaniemy @
+                   ++i;
+                
+            }else if(bufor[i]=='@'){ // i != 0 //interpretujemy jakby potencjalny poczatek ramki
+                    cleanBuffer(bufor);
+                    //stm.printf("blad\n");
+                    bufor[0]='@';
+                    i=1; //bo zerowy znak '@' juz zczytal
+                
+//end added part//
+
+            }else
+                i++;
+            
             continue;
-        }
-        if(bufor[7] != NULL)
-        {
+        }           
+        
+        
+        
+        if(isCorrectPacket(bufor))
+        {   
             t.stop();
             t.reset();
             t.start();
-                
+            counter++;
+            stm.printf("%ld\n", counter);
+             
+            
             for(int j=0; j<=2; j++)
             {
-                lewa[j]=bufor[j+1];   
+                lewa[j]=bufor[j+2];   
             }
-                
+                    
             for(int k=0; k<=2; k++)
             {
-                prawa[k]=bufor[k+5]; 
+             
+                prawa[k]=bufor[k+6];
             }
-                
+                    
+            
             sscanf(prawa, "%3d", &p);
             sscanf(lewa, "%3d", &l);
-            
-            if(abs(l)>100) {
+                  
+            if(abs(l)>100)
+            {
                 l=100;
-                }
-                
-            if(abs(p)>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;
-                
+           // stm.printf("%ld\n", counter);
+            for(int j=0; j<=BufferSize; j++) //wypisz
+            {
+                stm.printf("%c", bufor[j]);
+            }
+            stm.printf("\n");
+                        
+            //stm.printf("liczba lewa: %d  \n", l);
+            //stm.printf("liczba prawa: %d  \n", p);
+                    
+            if(bufor[1]=='-') l=-l;
+            if(bufor[5]=='-') 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;
-            }
+                        
+                 
         }
+        i=0;
+        cleanBuffer(bufor);
     }    
 }
\ No newline at end of file