Revision 3:0de9cb065bee, committed 2016-06-14

Comitter:

yruiewyrui3

Date:

Tue Jun 14 07:39:51 2016 +0000

Parent:

2:35dd310320e5

Commit message:

vol.2_milion

Changed in this revision

diff -r 35dd310320e5 -r 0de9cb065bee Servo.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Tue Jun 14 07:39:51 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/Servo/#36b69a7ced07

diff -r 35dd310320e5 -r 0de9cb065bee main.cpp
--- a/main.cpp	Wed Jun 08 10:26:19 2016 +0000
+++ b/main.cpp	Tue Jun 14 07:39:51 2016 +0000
@@ -1,23 +1,42 @@
 #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=17;
 char bufor[BufferSize];
 char prawa[3];
 char lewa[3];
+char poziom[2];
+char pion[2];
 int p=0;
 int l=0;
+int pi=0;
+int po=0;
 long counter=0;
 Timer t;
  
- 
- void cleanBuffer(char *buffor)
+//*************** Funkcja czyszczaca bufor *****************
+void cleanBuffer(char *buffor)
  {
     for(int i=0; i<BufferSize; i++)     
         buffor[i]=NULL;
+        buffor[BufferSize]=NULL;
 }
 
+//by Michal 8-06-2016
+    //***************OPIS FUNKCJI isCorrectPacket******************************************************//
+    //  funkcja sprawdza czy ramka spelnia wymagania protokolu: @ZCCCZCCC$ZCCZCC& gdzie Z={+/-} C={0,1,...9}  //
+    //*******************KONIEC OPISU******************************************************************//
+//by Dominik 8-06-2016  
+
 bool isCorrectPacket(char *buffor){
     if(bufor[0]!='@')
     {
@@ -43,7 +62,7 @@
        stm.printf("blad\n");
         return false;
     }
-    if(bufor[16]!='&')
+    if(bufor[16]!='^')
     {
        // stm.printf("ZLY ZNAK KONCA\n");
        stm.printf("blad\n");
@@ -62,30 +81,46 @@
        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");
+    }
+    if((buffor[10] != '+') && (buffor[10] != '-'))
+        {
+       // stm.printf("ZLY ZNAK +/- KAMERA PION\n");
+       stm.printf("blad\n");
+        return false;
+        }
+    if((buffor[13] != '+') && (buffor[13] != '-'))
+        {
+       // stm.printf("ZLY ZNAK +/- KAMERA POZIOM\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");
+    for(int i=1; i< 3; i++){
+        if(((int)buffor[i+10]) < 48 || ((int)buffor[i+10]) > 57) 
+        {
+       // stm.printf("KAMERA POZIOM: NIE LICZBA\n");
        stm.printf("blad\n");
         return false;
-        }  
+        } 
+        if(((int)buffor[i+13]) < 48 || ((int)buffor[i+13]) > 57) 
+        {
+       // stm.printf("KAMERA PION: NIE LICZBA\n");
+       stm.printf("blad\n");
+        return false;
+        } 
     }
-    //stm.printf("pakiet poprawny\n");
+        
+    
+    //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){ //jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota
@@ -94,8 +129,8 @@
             //stm.printf("TIMEOUT-ROBOT STOP\n");
             }
         
-        //if((bufor[9] == NULL)&&(stm.readable()))
-        if(bufor[9] == NULL)
+        //if((bufor[16] == NULL)&&(stm.readable()))
+        if(bufor[16] == NULL)
         {
             bufor[i] = stm.getc();            
             if(i==0){
@@ -109,7 +144,7 @@
                     i=1; //bo zerowy znak '@' juz zczytal
                 
 //end added part//
-
+ 
             }else
                 i++;
             
@@ -124,24 +159,45 @@
             t.reset();
             t.start();
             counter++;
-            stm.printf("%ld\n", counter);
+            //stm.printf("%ld\n", counter);
              
-            
+//*******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];
             }
-                    
+
+//*******PRZYPISANIE DO KAMERY POZIOM*********//   
+            for(int m=0; m<2; m++)
+            {
+             
+                poziom[m]=bufor[m+11];
+            }
+            
+//*******PRZYPISANIE DO KAMERY PION*********//   
+            for(int n=0; n<2; n++)
+            {
+             
+                pion[n]=bufor[n+14];
+            }
+            
+//*******KONWERSJA CHAROW NA INTY*********//                    
             
             sscanf(prawa, "%3d", &p);
             sscanf(lewa, "%3d", &l);
-                  
+            
+            sscanf(poziom, "%2d", &po);
+            sscanf(pion, "%2d", &pi);
+
+ //********KOREKCJA***************//  
+       
             if(abs(l)>100)
             {
                 l=100;
@@ -152,22 +208,48 @@
                 p=100;
             }
             
+            if(abs(po)>90)
+            {
+                po=90;
+            }
+                        
+            if(abs(pi)>90)
+            {
+                pi=90;
+            }
+ //************WYPISYWANIE*******************//  
+            
            // stm.printf("%ld\n", counter);
             for(int j=0; j<=BufferSize; j++) //wypisz
             {
-                stm.printf("%c", bufor[j]);
+                //stm.printf("%c", bufor[j]);
+                stm.putc(bufor[j]);
             }
-            stm.printf("\n");
+            //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;
-                        
+            if(bufor[10]=='-') po=-po;
+            if(bufor[13]=='-') pi=-pi;
+//**********ODPALANIE SILNIKOW******// 
+            //stm.printf("poziom= %d\n", po);
+            //stm.printf("pion= %d\n", pi);
+            //stm.printf("lewy silnik= %d\n", l);
+            //stm.printf("prawy silnik= %d\n", p);
             eng_left.move(l);
             eng_right.move(p);
-                        
+            
+            //for(int i=0; i<BufferSize; i++)
+              //  {
+                    stm.printf(bufor);
+                //}            
+//**********RUCH KAMERĄ******//
+ 
+            cam_poziom.position(po);
+            cam_pion.position(pi);                        
                  
         }
         i=0;