main.cpp

00001 #include "mbed.h"
00002 #include "Engine.h"
00003 #include "Servo.h"
00004  
00005 //*************** Deklaracja wszystkich portów potrzebnych do obsługi**************
00006 Serial stm(PA_2, PA_3);
00007 Servo cam_poziom(PB_6);
00008 Servo cam_pion(PC_7);
00009 Engine eng_left = Engine(PB_13, PB_4, PB_10);
00010 Engine eng_right = Engine(PB_14, PB_5, PB_3);
00011 
00012 //*************** Deklaracja zmiennych globalnych, tablic, bufora na ramke znaków **************
00013 const int BufferSize=10;
00014 char bufor[BufferSize];
00015 char prawa[3];
00016 char lewa[3];
00017 char poziom[2];
00018 char pion[2];
00019 int l=0;
00020 int p=0;
00021 int po=0;
00022 int pi=0;
00023 Timer t;
00024  
00025 //*************** Funkcja czyszczaca bufor *****************
00026 void cleanBuffer(char *buffor)
00027  {
00028     for(int i=0; i<BufferSize; i++)     
00029         buffor[i]=NULL;
00030         buffor[BufferSize]=NULL;
00031 }
00032 
00033 //***************OPIS FUNKCJI isCorrectPacket******************************************************//
00034 //  funkcja sprawdza czy ramka spelnia wymagania protokolu: @ZCCCZCCC$ lub &ZCCCZCCC$ gdzie Z={+/-} C={0,1,...9}  //
00035 //*******************KONIEC OPISU******************************************************************//
00036 
00037 bool isCorrectPacket(char *buffor){
00038     if ((bufor[0]!='@') && (bufor[0]!='&'))
00039     {
00040        // stm.printf("ZLY ZNAK POCZATKU\n");
00041        stm.printf("blad\n");
00042         return false;
00043     }
00044     if((buffor[1] != '+') && (buffor[1] != '-'))
00045         {
00046        // stm.printf("ZLY ZNAK +/- LEWY\n");
00047        stm.printf("blad\n");
00048         return false;
00049     }
00050     if((buffor[5] != '+') && (buffor[5] != '-'))
00051     {
00052        // stm.printf("ZLY ZNAK +/- PRAWY\n");
00053        stm.printf("blad\n");
00054         return false;
00055     }
00056     if(bufor[9]!='$')
00057     {
00058        // stm.printf("ZLY ZNAK KONCA\n");
00059        stm.printf("blad\n");
00060         return false;
00061     }
00062     for(int i=2; i< 5; i++){
00063         if(((int)buffor[i]) < 48 || ((int)buffor[i]) > 57) 
00064         {
00065        // stm.printf("NA LEWE KOLO: NIE LICZBA\n");
00066        stm.printf("blad\n");
00067         return false;
00068         } 
00069         if(((int)buffor[i+4]) < 48 || ((int)buffor[i+4]) > 57) 
00070             {
00071        // stm.printf("NA PRAWE KOLO: NIE LICZBA\n");
00072        stm.printf("blad\n");
00073         return false;
00074         } 
00075     }
00076     //stm.printf("pakiet poprawny\n");]
00077     return true;
00078 }
00079  
00080 void move_wheels()
00081 {
00082         //*******PRZYPISANIE DO TABLICY LEWEJ LICZBY*********//
00083             for(int j=0; j<=2; j++){
00084                 lewa[j]=bufor[j+2];   
00085             }
00086         //*******PRZYPISANIE DO TABLICY PRAWEJ LICZBY*********//   
00087             for(int k=0; k<=2; k++){
00088                 prawa[k]=bufor[k+6];
00089             }
00090         //*******KONWERSJA CHAROW NA INTY*********//                    
00091             sscanf(lewa, "%3d", &l);
00092             sscanf(prawa, "%3d", &p);
00093         //********KOREKCJA***************//  
00094             if(abs(l)>100){
00095                 l=100;
00096             }
00097             if(abs(p)>100){
00098                 p=100;
00099             }
00100         //************WYPISYWANIE*******************//  
00101             for(int j=0; j<=BufferSize; j++){
00102                 stm.putc(bufor[j]);
00103             }
00104         //******KOREKCJA ZNAKU***********//
00105             if(bufor[1]=='-') l=-l;
00106             if(bufor[5]=='-') p=-p;
00107         //**********ODPALANIE SILNIKOW******// 
00108             eng_left.move(l);
00109             eng_right.move(p);
00110             
00111             stm.printf(bufor);
00112 }
00113 void move_camera()
00114 {
00115         //*******PRZYPISANIE DO KAMERY POZIOM*********//   
00116             for(int m=0; m<2; m++){             
00117                 poziom[m]=bufor[m+3];
00118             }
00119         //*******PRZYPISANIE DO KAMERY PION*********//   
00120             for(int n=0; n<2; n++){
00121                 pion[n]=bufor[n+3];
00122             }
00123         //*******KONWERSJA CHAROW NA INTY*********//                    
00124             sscanf(poziom, "%2d", &po);
00125             sscanf(pion, "%2d", &pi);
00126         //********KOREKCJA***************//  
00127             if(abs(po)>90){
00128                 po=90;
00129             }                        
00130             if(abs(pi)>90){
00131                 pi=90;
00132             }
00133         //************WYPISYWANIE*******************//  
00134             for(int j=0; j<=BufferSize; j++){
00135                 stm.putc(bufor[j]);
00136             }                    
00137             if(bufor[1]=='-') po=-po;
00138             if(bufor[5]=='-') pi=-pi;
00139         //**********RUCH KAMERĄ******//
00140             cam_poziom.position(po);
00141             cam_pion.position(pi);      
00142 }
00143 void fill_buffer()
00144 {
00145     bufor[i] = stm.getc();            
00146     if(i==0){
00147         if((bufor[i]=='@') || (bufor[i]=='&')) //zaczynamy zapelniac bufor jak dostaniemy @
00148         ++i;  
00149         }
00150     else if(bufor[i]=='@'){ // i != 0 //interpretujemy jakby potencjalny poczatek ramki
00151         cleanBuffer(bufor);
00152         //stm.printf("blad\n");
00153         bufor[0]='@';
00154         i=1; //bo zerowy znak '@' juz zczytal
00155         }
00156     else if (bufor[i]=='&'){ // i != 0 //interpretujemy jakby potencjalny poczatek ramki
00157         cleanBuffer(bufor);
00158         //stm.printf("blad\n");
00159         bufor[0]='&';
00160         i=1; //bo zerowy znak '@' juz zczytal
00161         }
00162     else
00163         i++;
00164 }
00165  
00166 int main(){
00167     int i = 0;
00168     stm.baud(115200);
00169     while(true){
00170         if(t.read_ms()>200){ //jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota
00171             eng_left.move(0);
00172             eng_right.move(0);
00173             }
00174         //if((bufor[9] == NULL)&&(stm.readable()))
00175         if(bufor[9] == NULL){
00176             fill_buffer();
00177             continue;
00178         }           
00179         if(isCorrectPacket(bufor)){   
00180             t.stop();
00181             t.reset();
00182             t.start();                    
00183         }
00184         if(bufor[0]=='@'){
00185         move_wheels();
00186         }
00187         else if(bufor[0]=='&'){
00188         move_camera();
00189         }
00190         i=0;
00191         cleanBuffer(bufor);
00192     }    
00193 }