Dominik Święch / Mbed 2 deprecated Kola_Servo

main + engine

Dependencies:   Servo mbed

Fork of Komunikacja_read_write by Dominik Święch

main.cpp@3:0de9cb065bee, 2016-06-14 (annotated)

Committer:
yruiewyrui3
Date:
Tue Jun 14 07:39:51 2016 +0000
Revision:
3:0de9cb065bee
Parent:
2:35dd310320e5 
vol.2_milion

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yruiewyrui3 0:2294f5ee0610 1 #include "mbed.h"
yruiewyrui3 0:2294f5ee0610 2 #include "Engine.h"
yruiewyrui3 3:0de9cb065bee 3 #include "Servo.h"
yruiewyrui3 2:35dd310320e5 4
yruiewyrui3 3:0de9cb065bee 5 //*************** Deklaracja wszystkich portów potrzebnych do obsługi**************
yruiewyrui3 0:2294f5ee0610 6 Serial stm(PA_2, PA_3);
yruiewyrui3 3:0de9cb065bee 7 Servo cam_poziom(PB_6);
yruiewyrui3 3:0de9cb065bee 8 Servo cam_pion(PC_7);
yruiewyrui3 3:0de9cb065bee 9 Engine eng_left = Engine(PB_13, PB_4, PB_10);
yruiewyrui3 3:0de9cb065bee 10 Engine eng_right = Engine(PB_14, PB_5, PB_3);
yruiewyrui3 3:0de9cb065bee 11
yruiewyrui3 3:0de9cb065bee 12 //*************** Deklaracja zmiennych globalnych, tablic, bufora na ramke znaków **************
yruiewyrui3 2:35dd310320e5 13 const int BufferSize=17;
yruiewyrui3 2:35dd310320e5 14 char bufor[BufferSize];
yruiewyrui3 0:2294f5ee0610 15 char prawa[3];
yruiewyrui3 0:2294f5ee0610 16 char lewa[3];
yruiewyrui3 3:0de9cb065bee 17 char poziom[2];
yruiewyrui3 3:0de9cb065bee 18 char pion[2];
yruiewyrui3 0:2294f5ee0610 19 int p=0;
yruiewyrui3 0:2294f5ee0610 20 int l=0;
yruiewyrui3 3:0de9cb065bee 21 int pi=0;
yruiewyrui3 3:0de9cb065bee 22 int po=0;
yruiewyrui3 2:35dd310320e5 23 long counter=0;
yruiewyrui3 1:cad75f28ec58 24 Timer t;
yruiewyrui3 2:35dd310320e5 25
yruiewyrui3 3:0de9cb065bee 26 //*************** Funkcja czyszczaca bufor *****************
yruiewyrui3 3:0de9cb065bee 27 void cleanBuffer(char *buffor)
yruiewyrui3 2:35dd310320e5 28 {
yruiewyrui3 2:35dd310320e5 29 for(int i=0; i<BufferSize; i++)
yruiewyrui3 2:35dd310320e5 30 buffor[i]=NULL;
yruiewyrui3 3:0de9cb065bee 31 buffor[BufferSize]=NULL;
yruiewyrui3 2:35dd310320e5 32 }
yruiewyrui3 0:2294f5ee0610 33
yruiewyrui3 3:0de9cb065bee 34 //by Michal 8-06-2016
yruiewyrui3 3:0de9cb065bee 35 //***************OPIS FUNKCJI isCorrectPacket******************************************************//
yruiewyrui3 3:0de9cb065bee 36 // funkcja sprawdza czy ramka spelnia wymagania protokolu: @ZCCCZCCC$ZCCZCC& gdzie Z={+/-} C={0,1,...9} //
yruiewyrui3 3:0de9cb065bee 37 //*******************KONIEC OPISU******************************************************************//
yruiewyrui3 3:0de9cb065bee 38 //by Dominik 8-06-2016
yruiewyrui3 3:0de9cb065bee 39
yruiewyrui3 2:35dd310320e5 40 bool isCorrectPacket(char *buffor){
yruiewyrui3 2:35dd310320e5 41 if(bufor[0]!='@')
yruiewyrui3 2:35dd310320e5 42 {
yruiewyrui3 2:35dd310320e5 43 // stm.printf("ZLY ZNAK POCZATKU\n");
yruiewyrui3 2:35dd310320e5 44 stm.printf("blad\n");
yruiewyrui3 2:35dd310320e5 45 return false;
yruiewyrui3 2:35dd310320e5 46 }
yruiewyrui3 2:35dd310320e5 47 if((buffor[1] != '+') && (buffor[1] != '-'))
yruiewyrui3 2:35dd310320e5 48 {
yruiewyrui3 2:35dd310320e5 49 // stm.printf("ZLY ZNAK +/- LEWY\n");
yruiewyrui3 2:35dd310320e5 50 stm.printf("blad\n");
yruiewyrui3 2:35dd310320e5 51 return false;
yruiewyrui3 2:35dd310320e5 52 }
yruiewyrui3 2:35dd310320e5 53 if((buffor[5] != '+') && (buffor[5] != '-'))
yruiewyrui3 2:35dd310320e5 54 {
yruiewyrui3 2:35dd310320e5 55 // stm.printf("ZLY ZNAK +/- PRAWY\n");
yruiewyrui3 2:35dd310320e5 56 stm.printf("blad\n");
yruiewyrui3 2:35dd310320e5 57 return false;
yruiewyrui3 2:35dd310320e5 58 }
yruiewyrui3 2:35dd310320e5 59 if(bufor[9]!='$')
yruiewyrui3 2:35dd310320e5 60 {
yruiewyrui3 2:35dd310320e5 61 // stm.printf("ZLY ZNAK MIEDZY INFORMACJAMI\n");
yruiewyrui3 2:35dd310320e5 62 stm.printf("blad\n");
yruiewyrui3 2:35dd310320e5 63 return false;
yruiewyrui3 2:35dd310320e5 64 }
yruiewyrui3 3:0de9cb065bee 65 if(bufor[16]!='^')
yruiewyrui3 2:35dd310320e5 66 {
yruiewyrui3 2:35dd310320e5 67 // stm.printf("ZLY ZNAK KONCA\n");
yruiewyrui3 2:35dd310320e5 68 stm.printf("blad\n");
yruiewyrui3 2:35dd310320e5 69 return false;
yruiewyrui3 2:35dd310320e5 70 }
yruiewyrui3 2:35dd310320e5 71 for(int i=2; i< 5; i++){
yruiewyrui3 2:35dd310320e5 72 if(((int)buffor[i]) < 48 || ((int)buffor[i]) > 57)
yruiewyrui3 2:35dd310320e5 73 {
yruiewyrui3 2:35dd310320e5 74 // stm.printf("NA LEWE KOLO: NIE LICZBA\n");
yruiewyrui3 2:35dd310320e5 75 stm.printf("blad\n");
yruiewyrui3 2:35dd310320e5 76 return false;
yruiewyrui3 2:35dd310320e5 77 }
yruiewyrui3 2:35dd310320e5 78 if(((int)buffor[i+4]) < 48 || ((int)buffor[i+4]) > 57)
yruiewyrui3 2:35dd310320e5 79 {
yruiewyrui3 2:35dd310320e5 80 // stm.printf("NA PRAWE KOLO: NIE LICZBA\n");
yruiewyrui3 2:35dd310320e5 81 stm.printf("blad\n");
yruiewyrui3 2:35dd310320e5 82 return false;
yruiewyrui3 2:35dd310320e5 83 }
yruiewyrui3 3:0de9cb065bee 84 }
yruiewyrui3 3:0de9cb065bee 85 if((buffor[10] != '+') && (buffor[10] != '-'))
yruiewyrui3 3:0de9cb065bee 86 {
yruiewyrui3 3:0de9cb065bee 87 // stm.printf("ZLY ZNAK +/- KAMERA PION\n");
yruiewyrui3 3:0de9cb065bee 88 stm.printf("blad\n");
yruiewyrui3 3:0de9cb065bee 89 return false;
yruiewyrui3 3:0de9cb065bee 90 }
yruiewyrui3 3:0de9cb065bee 91 if((buffor[13] != '+') && (buffor[13] != '-'))
yruiewyrui3 3:0de9cb065bee 92 {
yruiewyrui3 3:0de9cb065bee 93 // stm.printf("ZLY ZNAK +/- KAMERA POZIOM\n");
yruiewyrui3 2:35dd310320e5 94 stm.printf("blad\n");
yruiewyrui3 2:35dd310320e5 95 return false;
yruiewyrui3 2:35dd310320e5 96 }
yruiewyrui3 3:0de9cb065bee 97 for(int i=1; i< 3; i++){
yruiewyrui3 3:0de9cb065bee 98 if(((int)buffor[i+10]) < 48 || ((int)buffor[i+10]) > 57)
yruiewyrui3 3:0de9cb065bee 99 {
yruiewyrui3 3:0de9cb065bee 100 // stm.printf("KAMERA POZIOM: NIE LICZBA\n");
yruiewyrui3 2:35dd310320e5 101 stm.printf("blad\n");
yruiewyrui3 2:35dd310320e5 102 return false;
yruiewyrui3 3:0de9cb065bee 103 }
yruiewyrui3 3:0de9cb065bee 104 if(((int)buffor[i+13]) < 48 || ((int)buffor[i+13]) > 57)
yruiewyrui3 3:0de9cb065bee 105 {
yruiewyrui3 3:0de9cb065bee 106 // stm.printf("KAMERA PION: NIE LICZBA\n");
yruiewyrui3 3:0de9cb065bee 107 stm.printf("blad\n");
yruiewyrui3 3:0de9cb065bee 108 return false;
yruiewyrui3 3:0de9cb065bee 109 }
yruiewyrui3 2:35dd310320e5 110 }
yruiewyrui3 3:0de9cb065bee 111
yruiewyrui3 3:0de9cb065bee 112
yruiewyrui3 3:0de9cb065bee 113 //stm.printf("pakiet poprawny\n");]
yruiewyrui3 3:0de9cb065bee 114
yruiewyrui3 2:35dd310320e5 115 return true;
yruiewyrui3 2:35dd310320e5 116 }
yruiewyrui3 2:35dd310320e5 117
yruiewyrui3 2:35dd310320e5 118
yruiewyrui3 0:2294f5ee0610 119 int main(){
yruiewyrui3 3:0de9cb065bee 120
yruiewyrui3 0:2294f5ee0610 121 int i = 0;
yruiewyrui3 0:2294f5ee0610 122 stm.baud(115200);
yruiewyrui3 3:0de9cb065bee 123
yruiewyrui3 0:2294f5ee0610 124 while(true){
yruiewyrui3 1:cad75f28ec58 125
yruiewyrui3 2:35dd310320e5 126 if(t.read_ms()>200){ //jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota
yruiewyrui3 1:cad75f28ec58 127 eng_left.move(0);
yruiewyrui3 1:cad75f28ec58 128 eng_right.move(0);
yruiewyrui3 2:35dd310320e5 129 //stm.printf("TIMEOUT-ROBOT STOP\n");
yruiewyrui3 1:cad75f28ec58 130 }
yruiewyrui3 1:cad75f28ec58 131
yruiewyrui3 3:0de9cb065bee 132 //if((bufor[16] == NULL)&&(stm.readable()))
yruiewyrui3 3:0de9cb065bee 133 if(bufor[16] == NULL)
yruiewyrui3 0:2294f5ee0610 134 {
yruiewyrui3 2:35dd310320e5 135 bufor[i] = stm.getc();
yruiewyrui3 2:35dd310320e5 136 if(i==0){
yruiewyrui3 2:35dd310320e5 137 if(bufor[i]=='@') //zaczynamy zapelniac bufor jak dostaniemy @
yruiewyrui3 2:35dd310320e5 138 ++i;
yruiewyrui3 2:35dd310320e5 139
yruiewyrui3 2:35dd310320e5 140 }else if(bufor[i]=='@'){ // i != 0 //interpretujemy jakby potencjalny poczatek ramki
yruiewyrui3 2:35dd310320e5 141 cleanBuffer(bufor);
yruiewyrui3 2:35dd310320e5 142 //stm.printf("blad\n");
yruiewyrui3 2:35dd310320e5 143 bufor[0]='@';
yruiewyrui3 2:35dd310320e5 144 i=1; //bo zerowy znak '@' juz zczytal
yruiewyrui3 2:35dd310320e5 145
yruiewyrui3 2:35dd310320e5 146 //end added part//
yruiewyrui3 3:0de9cb065bee 147
yruiewyrui3 2:35dd310320e5 148 }else
yruiewyrui3 2:35dd310320e5 149 i++;
yruiewyrui3 2:35dd310320e5 150
yruiewyrui3 0:2294f5ee0610 151 continue;
yruiewyrui3 2:35dd310320e5 152 }
yruiewyrui3 2:35dd310320e5 153
yruiewyrui3 2:35dd310320e5 154
yruiewyrui3 2:35dd310320e5 155
yruiewyrui3 2:35dd310320e5 156 if(isCorrectPacket(bufor))
yruiewyrui3 2:35dd310320e5 157 {
yruiewyrui3 1:cad75f28ec58 158 t.stop();
yruiewyrui3 1:cad75f28ec58 159 t.reset();
yruiewyrui3 1:cad75f28ec58 160 t.start();
yruiewyrui3 2:35dd310320e5 161 counter++;
yruiewyrui3 3:0de9cb065bee 162 //stm.printf("%ld\n", counter);
yruiewyrui3 2:35dd310320e5 163
yruiewyrui3 3:0de9cb065bee 164 //*******PRZYPISANIE DO TABLICY LEWEJ LICZBY*********//
yruiewyrui3 1:cad75f28ec58 165 for(int j=0; j<=2; j++)
yruiewyrui3 1:cad75f28ec58 166 {
yruiewyrui3 2:35dd310320e5 167 lewa[j]=bufor[j+2];
yruiewyrui3 1:cad75f28ec58 168 }
yruiewyrui3 3:0de9cb065bee 169
yruiewyrui3 3:0de9cb065bee 170 //*******PRZYPISANIE DO TABLICY PRAWEJ LICZBY*********//
yruiewyrui3 1:cad75f28ec58 171 for(int k=0; k<=2; k++)
yruiewyrui3 1:cad75f28ec58 172 {
yruiewyrui3 2:35dd310320e5 173
yruiewyrui3 2:35dd310320e5 174 prawa[k]=bufor[k+6];
yruiewyrui3 0:2294f5ee0610 175 }
yruiewyrui3 3:0de9cb065bee 176
yruiewyrui3 3:0de9cb065bee 177 //*******PRZYPISANIE DO KAMERY POZIOM*********//
yruiewyrui3 3:0de9cb065bee 178 for(int m=0; m<2; m++)
yruiewyrui3 3:0de9cb065bee 179 {
yruiewyrui3 3:0de9cb065bee 180
yruiewyrui3 3:0de9cb065bee 181 poziom[m]=bufor[m+11];
yruiewyrui3 3:0de9cb065bee 182 }
yruiewyrui3 3:0de9cb065bee 183
yruiewyrui3 3:0de9cb065bee 184 //*******PRZYPISANIE DO KAMERY PION*********//
yruiewyrui3 3:0de9cb065bee 185 for(int n=0; n<2; n++)
yruiewyrui3 3:0de9cb065bee 186 {
yruiewyrui3 3:0de9cb065bee 187
yruiewyrui3 3:0de9cb065bee 188 pion[n]=bufor[n+14];
yruiewyrui3 3:0de9cb065bee 189 }
yruiewyrui3 3:0de9cb065bee 190
yruiewyrui3 3:0de9cb065bee 191 //*******KONWERSJA CHAROW NA INTY*********//
yruiewyrui3 2:35dd310320e5 192
yruiewyrui3 1:cad75f28ec58 193 sscanf(prawa, "%3d", &p);
yruiewyrui3 1:cad75f28ec58 194 sscanf(lewa, "%3d", &l);
yruiewyrui3 3:0de9cb065bee 195
yruiewyrui3 3:0de9cb065bee 196 sscanf(poziom, "%2d", &po);
yruiewyrui3 3:0de9cb065bee 197 sscanf(pion, "%2d", &pi);
yruiewyrui3 3:0de9cb065bee 198
yruiewyrui3 3:0de9cb065bee 199 //********KOREKCJA***************//
yruiewyrui3 3:0de9cb065bee 200
yruiewyrui3 2:35dd310320e5 201 if(abs(l)>100)
yruiewyrui3 2:35dd310320e5 202 {
yruiewyrui3 1:cad75f28ec58 203 l=100;
yruiewyrui3 2:35dd310320e5 204 }
yruiewyrui3 2:35dd310320e5 205
yruiewyrui3 2:35dd310320e5 206 if(abs(p)>100)
yruiewyrui3 2:35dd310320e5 207 {
yruiewyrui3 1:cad75f28ec58 208 p=100;
yruiewyrui3 2:35dd310320e5 209 }
yruiewyrui3 1:cad75f28ec58 210
yruiewyrui3 3:0de9cb065bee 211 if(abs(po)>90)
yruiewyrui3 3:0de9cb065bee 212 {
yruiewyrui3 3:0de9cb065bee 213 po=90;
yruiewyrui3 3:0de9cb065bee 214 }
yruiewyrui3 3:0de9cb065bee 215
yruiewyrui3 3:0de9cb065bee 216 if(abs(pi)>90)
yruiewyrui3 3:0de9cb065bee 217 {
yruiewyrui3 3:0de9cb065bee 218 pi=90;
yruiewyrui3 3:0de9cb065bee 219 }
yruiewyrui3 3:0de9cb065bee 220 //************WYPISYWANIE*******************//
yruiewyrui3 3:0de9cb065bee 221
yruiewyrui3 2:35dd310320e5 222 // stm.printf("%ld\n", counter);
yruiewyrui3 2:35dd310320e5 223 for(int j=0; j<=BufferSize; j++) //wypisz
yruiewyrui3 2:35dd310320e5 224 {
yruiewyrui3 3:0de9cb065bee 225 //stm.printf("%c", bufor[j]);
yruiewyrui3 3:0de9cb065bee 226 stm.putc(bufor[j]);
yruiewyrui3 2:35dd310320e5 227 }
yruiewyrui3 3:0de9cb065bee 228 //stm.printf("\n");
yruiewyrui3 2:35dd310320e5 229
yruiewyrui3 2:35dd310320e5 230 //stm.printf("liczba lewa: %d \n", l);
yruiewyrui3 2:35dd310320e5 231 //stm.printf("liczba prawa: %d \n", p);
yruiewyrui3 2:35dd310320e5 232
yruiewyrui3 2:35dd310320e5 233 if(bufor[1]=='-') l=-l;
yruiewyrui3 2:35dd310320e5 234 if(bufor[5]=='-') p=-p;
yruiewyrui3 3:0de9cb065bee 235 if(bufor[10]=='-') po=-po;
yruiewyrui3 3:0de9cb065bee 236 if(bufor[13]=='-') pi=-pi;
yruiewyrui3 3:0de9cb065bee 237 //**********ODPALANIE SILNIKOW******//
yruiewyrui3 3:0de9cb065bee 238 //stm.printf("poziom= %d\n", po);
yruiewyrui3 3:0de9cb065bee 239 //stm.printf("pion= %d\n", pi);
yruiewyrui3 3:0de9cb065bee 240 //stm.printf("lewy silnik= %d\n", l);
yruiewyrui3 3:0de9cb065bee 241 //stm.printf("prawy silnik= %d\n", p);
yruiewyrui3 1:cad75f28ec58 242 eng_left.move(l);
yruiewyrui3 1:cad75f28ec58 243 eng_right.move(p);
yruiewyrui3 3:0de9cb065bee 244
yruiewyrui3 3:0de9cb065bee 245 //for(int i=0; i<BufferSize; i++)
yruiewyrui3 3:0de9cb065bee 246 // {
yruiewyrui3 3:0de9cb065bee 247 stm.printf(bufor);
yruiewyrui3 3:0de9cb065bee 248 //}
yruiewyrui3 3:0de9cb065bee 249 //**********RUCH KAMERĄ******//
yruiewyrui3 3:0de9cb065bee 250
yruiewyrui3 3:0de9cb065bee 251 cam_poziom.position(po);
yruiewyrui3 3:0de9cb065bee 252 cam_pion.position(pi);
yruiewyrui3 2:35dd310320e5 253
yruiewyrui3 0:2294f5ee0610 254 }
yruiewyrui3 2:35dd310320e5 255 i=0;
yruiewyrui3 2:35dd310320e5 256 cleanBuffer(bufor);
yruiewyrui3 0:2294f5ee0610 257 }
yruiewyrui3 0:2294f5ee0610 258 }