Diff: main.cpp

Revision:

0:2e73d45d090e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Jun 14 12:32:39 2016 +0000
@@ -0,0 +1,193 @@
+#include "mbed.h"
+#include "Engine.h"
+#include "Servo.h"
+ 
+//*************** Deklaracja wszystkich portów potrzebnych do obsługi**************
+Serial stm(PA_2, PA_3);
+Servo cam_poziom(PB_6);
+Servo cam_pion(PC_7);
+Engine eng_left = Engine(PB_13, PB_4, PB_10);
+Engine eng_right = Engine(PB_14, PB_5, PB_3);
+
+//*************** Deklaracja zmiennych globalnych, tablic, bufora na ramke znaków **************
+const int BufferSize=10;
+char bufor[BufferSize];
+char prawa[3];
+char lewa[3];
+char poziom[2];
+char pion[2];
+int l=0;
+int p=0;
+int po=0;
+int pi=0;
+Timer t;
+ 
+//*************** Funkcja czyszczaca bufor *****************
+void cleanBuffer(char *buffor)
+ {
+    for(int i=0; i<BufferSize; i++)     
+        buffor[i]=NULL;
+        buffor[BufferSize]=NULL;
+}
+
+//***************OPIS FUNKCJI isCorrectPacket******************************************************//
+//  funkcja sprawdza czy ramka spelnia wymagania protokolu: @ZCCCZCCC$ lub &ZCCCZCCC$ gdzie Z={+/-} C={0,1,...9}  //
+//*******************KONIEC OPISU******************************************************************//
+
+bool isCorrectPacket(char *buffor){
+    if ((bufor[0]!='@') && (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 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;
+        } 
+    }
+    //stm.printf("pakiet poprawny\n");]
+    return true;
+}
+ 
+void move_wheels()
+{
+        //*******PRZYPISANIE DO TABLICY LEWEJ LICZBY*********//
+            for(int j=0; j<=2; j++){
+                lewa[j]=bufor[j+2];   
+            }
+        //*******PRZYPISANIE DO TABLICY PRAWEJ LICZBY*********//   
+            for(int k=0; k<=2; k++){
+                prawa[k]=bufor[k+6];
+            }
+        //*******KONWERSJA CHAROW NA INTY*********//                    
+            sscanf(lewa, "%3d", &l);
+            sscanf(prawa, "%3d", &p);
+        //********KOREKCJA***************//  
+            if(abs(l)>100){
+                l=100;
+            }
+            if(abs(p)>100){
+                p=100;
+            }
+        //************WYPISYWANIE*******************//  
+            for(int j=0; j<=BufferSize; j++){
+                stm.putc(bufor[j]);
+            }
+        //******KOREKCJA ZNAKU***********//
+            if(bufor[1]=='-') l=-l;
+            if(bufor[5]=='-') p=-p;
+        //**********ODPALANIE SILNIKOW******// 
+            eng_left.move(l);
+            eng_right.move(p);
+            
+            stm.printf(bufor);
+}
+void move_camera()
+{
+        //*******PRZYPISANIE DO KAMERY POZIOM*********//   
+            for(int m=0; m<2; m++){             
+                poziom[m]=bufor[m+3];
+            }
+        //*******PRZYPISANIE DO KAMERY PION*********//   
+            for(int n=0; n<2; n++){
+                pion[n]=bufor[n+3];
+            }
+        //*******KONWERSJA CHAROW NA INTY*********//                    
+            sscanf(poziom, "%2d", &po);
+            sscanf(pion, "%2d", &pi);
+        //********KOREKCJA***************//  
+            if(abs(po)>90){
+                po=90;
+            }                        
+            if(abs(pi)>90){
+                pi=90;
+            }
+        //************WYPISYWANIE*******************//  
+            for(int j=0; j<=BufferSize; j++){
+                stm.putc(bufor[j]);
+            }                    
+            if(bufor[1]=='-') po=-po;
+            if(bufor[5]=='-') pi=-pi;
+        //**********RUCH KAMERĄ******//
+            cam_poziom.position(po);
+            cam_pion.position(pi);      
+}
+void fill_buffer()
+{
+    bufor[i] = stm.getc();            
+    if(i==0){
+        if((bufor[i]=='@') || (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
+        }
+    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
+        }
+    else
+        i++;
+}
+ 
+int main(){
+    int i = 0;
+    stm.baud(115200);
+    while(true){
+        if(t.read_ms()>200){ //jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota
+            eng_left.move(0);
+            eng_right.move(0);
+            }
+        //if((bufor[9] == NULL)&&(stm.readable()))
+        if(bufor[9] == NULL){
+            fill_buffer();
+            continue;
+        }           
+        if(isCorrectPacket(bufor)){   
+            t.stop();
+            t.reset();
+            t.start();                    
+        }
+        if(bufor[0]=='@'){
+        move_wheels();
+        }
+        else if(bufor[0]=='&'){
+        move_camera();
+        }
+        i=0;
+        cleanBuffer(bufor);
+    }    
+}
\ No newline at end of file