main.cpp

00001 #include "mbed.h"
00002 #include "Engine.h"
00003 #include "Servo.h"
00004 #include "hcsr04.h"
00005  
00006 //*************** Deklaracja wszystkich portów potrzebnych do obsługi**************
00007 Serial stm(PA_2, PA_3);
00008 Servo cam_poziom = Servo(PB_6);
00009 Servo cam_pion = Servo(PC_7);
00010 Engine eng_left = Engine(PB_13, PB_4, PB_10);
00011 Engine eng_right = Engine(PB_14, PB_5, PB_3);
00012 
00013 
00014 //*************** Deklaracja zmiennych globalnych, tablic, bufora na ramke znaków **************
00015 HCSR04 sensor(PB_9, PB_8, 11770);
00016 const int BufferSize=10;
00017 char bufor[BufferSize];
00018 char prawa[3];
00019 char lewa[3];
00020 char dodaj_poziom[2];
00021 char dodaj_pion[2];
00022 int akt_pion=0, akt_poziom=0;
00023 int l=0;
00024 int p=0;
00025 int po=0;
00026 int pi=0;
00027 Timer t, t_sonar, czas;
00028 long distance;
00029 int counter=0, wypisz_czas;
00030 
00031 //*************** Funkcja czyszczaca bufor *****************
00032 void cleanBuffer(char *buffor)
00033  {
00034     for(int i=0; i<BufferSize; i++)     
00035         buffor[i]=NULL;
00036         buffor[BufferSize]=NULL;
00037 }
00038 
00039 char* intToChar(int value, char tab[3])
00040 {
00041         tab[0]= (char)(value/100 + 48);
00042         tab[1]= (char)((value-(100*(value/100)))/10 + 48);
00043         tab[2]= (char)(value-(100*(value/100))-(10*((value-(100*(value/100)))/10)) + 48);
00044         /*stm.printf("Tablica z distance to: ");
00045         for (int i=0; i<3; ++i)
00046             stm.printf("%c",tab[i]);
00047         stm.printf("\n");*/
00048         return tab;
00049 }
00050 
00051 void send2rpi(char tag, int wartosc){
00052     char tablicaf[3]; 
00053     stm.putc(tag);
00054     intToChar(wartosc, tablicaf);
00055     for (int f=0; f<3; f++){
00056          stm.putc(tablicaf[f]);
00057     }       
00058 }
00059 
00060 //***************OPIS FUNKCJI isCorrectPacket******************************************************//
00061 //  funkcja sprawdza czy ramka spelnia wymagania protokolu: @ZCCCZCCC$ lub &ZCCCZCCC$ gdzie Z={+/-} C={0,1,...9}  //
00062 //*******************KONIEC OPISU******************************************************************//
00063 
00064 bool isCorrectPacket(char *buffor){
00065     if (bufor[0]!='@')
00066     {
00067        // stm.printf("ZLY ZNAK POCZATKU\n");       
00068         return false;
00069     }
00070     if((buffor[1] != '+') && (buffor[1] != '-'))
00071         {
00072        // stm.printf("ZLY ZNAK +/- LEWY\n");       
00073         return false;
00074     }
00075     if((buffor[5] != '+') && (buffor[5] != '-'))
00076     {
00077        // stm.printf("ZLY ZNAK +/- PRAWY\n");       
00078         return false;
00079     }
00080     if((bufor[9]!='$')&&(bufor[9]!='&'))
00081     {
00082        // stm.printf("ZLY ZNAK KONCA\n");
00083        return false;
00084     }
00085     for(int i=2; i< 5; i++){
00086         if(((int)buffor[i]) < 48 || ((int)buffor[i]) > 57) 
00087         {
00088        // stm.printf("NA LEWE KOLO: NIE LICZBA\n");
00089         return false;
00090         } 
00091         if(((int)buffor[i+4]) < 48 || ((int)buffor[i+4]) > 57) 
00092             {
00093        // stm.printf("NA PRAWE KOLO: NIE LICZBA\n");       
00094         return false;
00095         } 
00096     }
00097     //stm.printf("pakiet poprawny\n");]
00098     return true;
00099 }
00100  
00101 void actualize_speed()
00102 {
00103         //*******PRZYPISANIE DO TABLICY LEWEJ LICZBY*********//
00104             for(int j=0; j<=2; j++){
00105                 lewa[j]=bufor[j+2];   
00106             }
00107         //*******PRZYPISANIE DO TABLICY PRAWEJ LICZBY*********//   
00108             for(int k=0; k<=2; k++){
00109                 prawa[k]=bufor[k+6];
00110             }
00111         //*******KONWERSJA CHAROW NA INTY*********//                    
00112             sscanf(lewa, "%3d", &l);
00113             sscanf(prawa, "%3d", &p);
00114         //********KOREKCJA***************//  
00115             if(abs(l)>100){
00116                 l=100;
00117             }
00118             if(abs(p)>100){
00119                 p=100;
00120             }        
00121         //******KOREKCJA ZNAKU***********//
00122             if(bufor[1]=='-') l=-l;
00123             if(bufor[5]=='-') p=-p;    
00124     
00125 }
00126 
00127 
00128 void move_wheels()
00129 {
00130             eng_left.move(l);
00131             eng_right.move(p);
00132 }
00133 
00134 void actualize_servo_values()
00135 {    
00136         //*******PRZYPISANIE DO KAMERY POZIOM*********//   
00137             for(int m=0; m<2; m++){             
00138                 dodaj_poziom[m]=bufor[m+3];
00139             }
00140         //*******PRZYPISANIE DO KAMERY PION*********//   
00141             for(int n=0; n<2; n++){
00142                 dodaj_pion[n]=bufor[n+7];
00143             }
00144         //*******KONWERSJA CHAROW NA INTY*********//                    
00145             sscanf(dodaj_poziom, "%2d", &po);
00146             sscanf(dodaj_pion, "%2d", &pi);
00147         //********KOREKCJA***************// 
00148             if(bufor[1]=='-') po=-po;
00149             if(bufor[5]=='-') pi=-pi;
00150             
00151             akt_poziom = akt_poziom + po;
00152             akt_pion = akt_pion + pi;
00153             
00154             if(akt_poziom>90)
00155                 akt_poziom=90;
00156             else if (akt_poziom<-90)
00157                 akt_poziom=-90;
00158                                    
00159             if(akt_pion>90)
00160                 akt_pion=90;
00161             else if (akt_pion<-90)
00162                 akt_poziom=-90;                      
00163 }
00164 
00165 
00166 void move_camera()
00167 {               
00168             cam_pion.SetPosition(akt_pion*5+1500);  
00169             cam_poziom.SetPosition(akt_poziom*5+1500);
00170              /*while(1) {
00171                   for (int pos = 1000; pos < 2100; pos += 10) {
00172                      cam_pion.SetPosition(pos);  
00173                      stm.printf("position %d\n", pos);
00174                      wait_ms(10);
00175                   }
00176                   for (int pos = 2100; pos > 1000; pos -= 10) {
00177                       cam_pion.SetPosition(pos); 
00178                       wait_ms(10); 
00179                   }
00180               }*/
00181 }
00182 
00183 
00184 
00185 int main(){
00186     int i = 0;
00187     cleanBuffer(bufor);
00188     stm.baud(115200);
00189     t_sonar.start();
00190     distance = 0;
00191     
00192     cam_poziom.Enable(1450,20000);
00193     cam_pion.Enable(1450,20000);
00194     move_camera();
00195     czas.start();
00196     
00197     while(true)
00198     {
00199         
00200         if(t.read_ms()>500){ //jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota
00201             eng_left.move(0);
00202             eng_right.move(0);
00203             }
00204             
00205         if(t_sonar.read_ms()>1200){
00206             distance = sensor.distance(); 
00207             t_sonar.stop();
00208             t_sonar.reset();
00209             t_sonar.start(); 
00210             }
00211         if(bufor[9] == NULL)
00212         {
00213             bufor[i] = stm.getc();           
00214             if(i==0)
00215             {
00216                 if(bufor[i]=='@') //zaczynamy zapelniac bufor jak dostaniemy @
00217                    ++i;    
00218             }
00219             else if(bufor[i]=='@')
00220             { // i != 0 //interpretujemy jakby potencjalny poczatek ramki
00221                 cleanBuffer(bufor);
00222                 bufor[0]='@';
00223                 i=1; //bo zerowy znak '@' juz zczytal
00224             }
00225             else i++;
00226              
00227             continue;
00228         }         
00229         if(isCorrectPacket(bufor))
00230         {   
00231             t.stop();
00232             t.reset();
00233             t.start();   
00234             counter++;
00235             if(czas.read_ms()>1000){
00236                 stm.printf("counter %d\n", counter);
00237                 counter=0;
00238                 czas.stop();
00239                 czas.reset();
00240                 czas.start();   
00241             }           
00242         }
00243         if(bufor[9]=='$'){
00244             actualize_speed();
00245             if(distance>25){
00246                 move_wheels();
00247             }
00248             else if (distance<=25){
00249                 if(l<=0 && p<=0 || l*p<0){                
00250                     move_wheels();
00251                 }
00252                 else{
00253                 eng_left.move(0);
00254                 eng_right.move(0);                
00255                 }
00256             }
00257         }
00258         else if(bufor[9]=='&'){            
00259             actualize_servo_values();
00260             move_camera();
00261            /* counter++;
00262             if(czas.read_ms()>1000){
00263                // stm.printf("counter %d\n", counter);
00264                 counter=0;
00265                 czas.stop();
00266                 czas.reset();
00267                 czas.start();   
00268             }*/
00269         }
00270         i=0;
00271         //send2rpi('s', distance);
00272         cleanBuffer(bufor);
00273         
00274     }    
00275 }