Diff: main.cpp

Revision:

3:1a391dd91a35

Parent:

2:26a9f90142a0

Child:

4:84f836aaf390

--- a/main.cpp	Tue Jun 21 09:19:17 2016 +0000
+++ b/main.cpp	Fri Jun 24 13:43:08 2016 +0000
@@ -16,15 +16,15 @@
 char bufor[BufferSize];
 char prawa[3];
 char lewa[3];
-char poziom[2];
-char pion[2];
+char dodaj_poziom[2];
+char dodaj_pion[2];
+int akt_pion=0, akt_poziom=0;
 int l=0;
 int p=0;
 int po=0;
 int pi=0;
 Timer t, t_sonar;
 long distance;
-
  
 //*************** Funkcja czyszczaca bufor *****************
 void cleanBuffer(char *buffor)
@@ -34,6 +34,27 @@
         buffor[BufferSize]=NULL;
 }
 
+char* intToChar(int value, char tab[3])
+{
+        tab[0]= (char)(value/100 + 48);
+        tab[1]= (char)((value-(100*(value/100)))/10 + 48);
+        tab[2]= (char)(value-(100*(value/100))-(10*((value-(100*(value/100)))/10)) + 48);
+        /*stm.printf("Tablica z distance to: ");
+        for (int i=0; i<3; ++i)
+            stm.printf("%c",tab[i]);
+        stm.printf("\n");*/
+        return tab;
+}
+
+void send2rpi(char tag, int wartosc){
+    char tablicaf[3]; 
+    stm.putc(tag);
+    intToChar(wartosc, tablicaf);
+    for (int f=0; f<3; f++){
+         stm.putc(tablicaf[f]);
+    }       
+}
+
 //***************OPIS FUNKCJI isCorrectPacket******************************************************//
 //  funkcja sprawdza czy ramka spelnia wymagania protokolu: @ZCCCZCCC$ lub &ZCCCZCCC$ gdzie Z={+/-} C={0,1,...9}  //
 //*******************KONIEC OPISU******************************************************************//
@@ -41,39 +62,33 @@
 bool isCorrectPacket(char *buffor){
     if (bufor[0]!='@')
     {
-       // stm.printf("ZLY ZNAK POCZATKU\n");
-       stm.printf("blad\n");
+       // stm.printf("ZLY ZNAK POCZATKU\n");       
         return false;
     }
     if((buffor[1] != '+') && (buffor[1] != '-'))
         {
-       // stm.printf("ZLY ZNAK +/- LEWY\n");
-       stm.printf("blad\n");
+       // stm.printf("ZLY ZNAK +/- LEWY\n");       
         return false;
     }
     if((buffor[5] != '+') && (buffor[5] != '-'))
     {
-       // stm.printf("ZLY ZNAK +/- PRAWY\n");
-       stm.printf("blad\n");
+       // stm.printf("ZLY ZNAK +/- PRAWY\n");       
         return false;
     }
     if((bufor[9]!='$')&&(bufor[9]!='&'))
     {
        // stm.printf("ZLY ZNAK KONCA\n");
-       stm.printf("blad\n");
-        return false;
+       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");
+       // stm.printf("NA PRAWE KOLO: NIE LICZBA\n");       
         return false;
         } 
     }
@@ -81,7 +96,7 @@
     return true;
 }
  
-void move_wheels()
+void actualize_speed()
 {
         //*******PRZYPISANIE DO TABLICY LEWEJ LICZBY*********//
             for(int j=0; j<=2; j++){
@@ -100,92 +115,127 @@
             }
             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******// 
+            if(bufor[5]=='-') p=-p;    
+    
+}
+
+
+void move_wheels()
+{
             eng_left.move(l);
             eng_right.move(p);
 }
-void move_camera()
-{
+
+void actualize_servo_values()
+{    
         //*******PRZYPISANIE DO KAMERY POZIOM*********//   
             for(int m=0; m<2; m++){             
-                poziom[m]=bufor[m+3];
+                dodaj_poziom[m]=bufor[m+3];
             }
         //*******PRZYPISANIE DO KAMERY PION*********//   
             for(int n=0; n<2; n++){
-                pion[n]=bufor[n+7];
+                dodaj_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]);
-            }                    
+            sscanf(dodaj_poziom, "%2d", &po);
+            sscanf(dodaj_pion, "%2d", &pi);
+        //********KOREKCJA***************// 
             if(bufor[1]=='-') po=-po;
             if(bufor[5]=='-') pi=-pi;
-        //**********RUCH KAMERĄ******//
-            cam_poziom.position(po);
-            cam_pion.position(pi);      
+            
+            akt_poziom = akt_poziom + po;
+            akt_pion = akt_pion + pi;
+            
+            if(akt_poziom>90)
+                akt_poziom=90;
+            else if (akt_poziom<-90)
+                akt_poziom=-90;
+                                   
+            if(akt_pion>90)
+                akt_pion=90;
+            else if (akt_pion<-90)
+                akt_poziom=-90;                
 }
 
+
+void move_camera()
+{               
+            cam_poziom.position(akt_poziom);
+            cam_pion.position(akt_pion);    
+}
+
+
+
 int main(){
     int i = 0;
     cleanBuffer(bufor);
     stm.baud(115200);
     t_sonar.start();
-    while(true){
-        if(t.read_ms()>200){ //jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota
+    distance = 0;
+    move_camera();
+    
+    while(true)
+    {
+        
+        if(t.read_ms()>500){ //jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota
             eng_left.move(0);
             eng_right.move(0);
             }
         if(t_sonar.read_ms()>1200){
             distance = sensor.distance(); 
-            stm.printf("dystans  %d  \n",distance);
             t_sonar.stop();
             t_sonar.reset();
             t_sonar.start(); 
             }
-        if(bufor[9] == NULL){
+        if(bufor[9] == NULL)
+        {
             bufor[i] = stm.getc();           
-            if(i==0){
+            if(i==0)
+            {
                 if(bufor[i]=='@') //zaczynamy zapelniac bufor jak dostaniemy @
                    ++i;    
             }
-            else if(bufor[i]=='@'){ // i != 0 //interpretujemy jakby potencjalny poczatek ramki
+            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;
+             
+            continue;
         }         
-        if(isCorrectPacket(bufor)){   
+        if(isCorrectPacket(bufor))
+        {   
             t.stop();
             t.reset();
-            t.start();                    
+            t.start();              
         }
         if(bufor[9]=='$'){
-        move_wheels();
+            actualize_speed();
+            if(distance>25){
+                move_wheels();
+            }
+            else if (distance<=25){
+                if(l<=0 && p<=0 || l*p<0){                
+                    move_wheels();
+                }
+                else{
+                eng_left.move(0);
+                eng_right.move(0);                
+                }
+            }
         }
         else if(bufor[9]=='&'){
-        move_camera();
+            actualize_servo_values();
+            move_camera();
         }
         i=0;
+        //send2rpi('s', distance);
         cleanBuffer(bufor);
+        
     }    
-}
\ No newline at end of file
+}