Diff: main.cpp

Revision:

0:5919ea7b3b90

diff -r 000000000000 -r 5919ea7b3b90 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Jun 21 07:45:32 2016 +0000
@@ -0,0 +1,189 @@
+#include "mbed.h"
+#include "Engine.h"
+#include "Servo.h"
+#include "hcsr04.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 **************
+HCSR04 sensor(PB_9, PB_8, 11770);
+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, t_sonar;
+float distance;
+ 
+//*************** 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]!='@')
+    {
+       // 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]!='$')&&(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);
+}
+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+7];
+            }
+        //*******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);      
+}
+
+int main(){
+    int i = 0;
+    cleanBuffer(bufor);
+    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(t_sonar.read_ms()>1200){
+            t_sonar.stop();
+            t_sonar.reset();
+            t_sonar.start(); 
+            distance = sensor.distance(); 
+            stm.printf("dystans  %d  \n",distance);  
+            }
+        if(bufor[9] == NULL){
+            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);
+                bufor[0]='@';
+                i=1; //bo zerowy znak '@' juz zczytal
+                }
+            else i++;
+        continue;
+        }         
+        if(isCorrectPacket(bufor)){   
+            t.stop();
+            t.reset();
+            t.start();                    
+        }
+        if(bufor[9]=='$'){
+        move_wheels();
+        }
+        else if(bufor[9]=='&'){
+        move_camera();
+        }
+        i=0;
+        cleanBuffer(bufor);
+    }    
+}
\ No newline at end of file