Dominik Święch
/
Komunikacja_read_write
main + engine
Fork of AktalnyCZOSN by
main.cpp@2:35dd310320e5, 2016-06-08 (annotated)
- Committer:
- yruiewyrui3
- Date:
- Wed Jun 08 10:26:19 2016 +0000
- Revision:
- 2:35dd310320e5
- Parent:
- 1:cad75f28ec58
.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yruiewyrui3 | 0:2294f5ee0610 | 1 | #include "mbed.h" |
yruiewyrui3 | 0:2294f5ee0610 | 2 | #include "Engine.h" |
yruiewyrui3 | 2:35dd310320e5 | 3 | |
yruiewyrui3 | 0:2294f5ee0610 | 4 | Serial stm(PA_2, PA_3); |
yruiewyrui3 | 2:35dd310320e5 | 5 | const int BufferSize=17; |
yruiewyrui3 | 2:35dd310320e5 | 6 | char bufor[BufferSize]; |
yruiewyrui3 | 0:2294f5ee0610 | 7 | char prawa[3]; |
yruiewyrui3 | 0:2294f5ee0610 | 8 | char lewa[3]; |
yruiewyrui3 | 0:2294f5ee0610 | 9 | int p=0; |
yruiewyrui3 | 0:2294f5ee0610 | 10 | int l=0; |
yruiewyrui3 | 2:35dd310320e5 | 11 | long counter=0; |
yruiewyrui3 | 1:cad75f28ec58 | 12 | Timer t; |
yruiewyrui3 | 2:35dd310320e5 | 13 | |
yruiewyrui3 | 2:35dd310320e5 | 14 | |
yruiewyrui3 | 2:35dd310320e5 | 15 | void cleanBuffer(char *buffor) |
yruiewyrui3 | 2:35dd310320e5 | 16 | { |
yruiewyrui3 | 2:35dd310320e5 | 17 | for(int i=0; i<BufferSize; i++) |
yruiewyrui3 | 2:35dd310320e5 | 18 | buffor[i]=NULL; |
yruiewyrui3 | 2:35dd310320e5 | 19 | } |
yruiewyrui3 | 0:2294f5ee0610 | 20 | |
yruiewyrui3 | 2:35dd310320e5 | 21 | bool isCorrectPacket(char *buffor){ |
yruiewyrui3 | 2:35dd310320e5 | 22 | if(bufor[0]!='@') |
yruiewyrui3 | 2:35dd310320e5 | 23 | { |
yruiewyrui3 | 2:35dd310320e5 | 24 | // stm.printf("ZLY ZNAK POCZATKU\n"); |
yruiewyrui3 | 2:35dd310320e5 | 25 | stm.printf("blad\n"); |
yruiewyrui3 | 2:35dd310320e5 | 26 | return false; |
yruiewyrui3 | 2:35dd310320e5 | 27 | } |
yruiewyrui3 | 2:35dd310320e5 | 28 | if((buffor[1] != '+') && (buffor[1] != '-')) |
yruiewyrui3 | 2:35dd310320e5 | 29 | { |
yruiewyrui3 | 2:35dd310320e5 | 30 | // stm.printf("ZLY ZNAK +/- LEWY\n"); |
yruiewyrui3 | 2:35dd310320e5 | 31 | stm.printf("blad\n"); |
yruiewyrui3 | 2:35dd310320e5 | 32 | return false; |
yruiewyrui3 | 2:35dd310320e5 | 33 | } |
yruiewyrui3 | 2:35dd310320e5 | 34 | if((buffor[5] != '+') && (buffor[5] != '-')) |
yruiewyrui3 | 2:35dd310320e5 | 35 | { |
yruiewyrui3 | 2:35dd310320e5 | 36 | // stm.printf("ZLY ZNAK +/- PRAWY\n"); |
yruiewyrui3 | 2:35dd310320e5 | 37 | stm.printf("blad\n"); |
yruiewyrui3 | 2:35dd310320e5 | 38 | return false; |
yruiewyrui3 | 2:35dd310320e5 | 39 | } |
yruiewyrui3 | 2:35dd310320e5 | 40 | if(bufor[9]!='$') |
yruiewyrui3 | 2:35dd310320e5 | 41 | { |
yruiewyrui3 | 2:35dd310320e5 | 42 | // stm.printf("ZLY ZNAK MIEDZY INFORMACJAMI\n"); |
yruiewyrui3 | 2:35dd310320e5 | 43 | stm.printf("blad\n"); |
yruiewyrui3 | 2:35dd310320e5 | 44 | return false; |
yruiewyrui3 | 2:35dd310320e5 | 45 | } |
yruiewyrui3 | 2:35dd310320e5 | 46 | if(bufor[16]!='&') |
yruiewyrui3 | 2:35dd310320e5 | 47 | { |
yruiewyrui3 | 2:35dd310320e5 | 48 | // stm.printf("ZLY ZNAK KONCA\n"); |
yruiewyrui3 | 2:35dd310320e5 | 49 | stm.printf("blad\n"); |
yruiewyrui3 | 2:35dd310320e5 | 50 | return false; |
yruiewyrui3 | 2:35dd310320e5 | 51 | } |
yruiewyrui3 | 2:35dd310320e5 | 52 | for(int i=2; i< 5; i++){ |
yruiewyrui3 | 2:35dd310320e5 | 53 | if(((int)buffor[i]) < 48 || ((int)buffor[i]) > 57) |
yruiewyrui3 | 2:35dd310320e5 | 54 | { |
yruiewyrui3 | 2:35dd310320e5 | 55 | // stm.printf("NA LEWE KOLO: NIE LICZBA\n"); |
yruiewyrui3 | 2:35dd310320e5 | 56 | stm.printf("blad\n"); |
yruiewyrui3 | 2:35dd310320e5 | 57 | return false; |
yruiewyrui3 | 2:35dd310320e5 | 58 | } |
yruiewyrui3 | 2:35dd310320e5 | 59 | if(((int)buffor[i+4]) < 48 || ((int)buffor[i+4]) > 57) |
yruiewyrui3 | 2:35dd310320e5 | 60 | { |
yruiewyrui3 | 2:35dd310320e5 | 61 | // stm.printf("NA PRAWE KOLO: NIE LICZBA\n"); |
yruiewyrui3 | 2:35dd310320e5 | 62 | stm.printf("blad\n"); |
yruiewyrui3 | 2:35dd310320e5 | 63 | return false; |
yruiewyrui3 | 2:35dd310320e5 | 64 | } |
yruiewyrui3 | 2:35dd310320e5 | 65 | if(((int)buffor[i+8]) < 48 || ((int)buffor[i+8]) > 57) |
yruiewyrui3 | 2:35dd310320e5 | 66 | { |
yruiewyrui3 | 2:35dd310320e5 | 67 | // stm.printf("KAMERA GORA/DOL: NIE LICZBA\n"); |
yruiewyrui3 | 2:35dd310320e5 | 68 | stm.printf("blad\n"); |
yruiewyrui3 | 2:35dd310320e5 | 69 | return false; |
yruiewyrui3 | 2:35dd310320e5 | 70 | } |
yruiewyrui3 | 2:35dd310320e5 | 71 | if(((int)buffor[i+11]) < 48 || ((int)buffor[i+11]) > 57) |
yruiewyrui3 | 2:35dd310320e5 | 72 | { |
yruiewyrui3 | 2:35dd310320e5 | 73 | // stm.printf("KAMERA LEWO/PRAWO: NIE LICZBA\n"); |
yruiewyrui3 | 2:35dd310320e5 | 74 | stm.printf("blad\n"); |
yruiewyrui3 | 2:35dd310320e5 | 75 | return false; |
yruiewyrui3 | 2:35dd310320e5 | 76 | } |
yruiewyrui3 | 2:35dd310320e5 | 77 | } |
yruiewyrui3 | 2:35dd310320e5 | 78 | //stm.printf("pakiet poprawny\n"); |
yruiewyrui3 | 2:35dd310320e5 | 79 | return true; |
yruiewyrui3 | 2:35dd310320e5 | 80 | } |
yruiewyrui3 | 2:35dd310320e5 | 81 | |
yruiewyrui3 | 2:35dd310320e5 | 82 | |
yruiewyrui3 | 0:2294f5ee0610 | 83 | int main(){ |
yruiewyrui3 | 0:2294f5ee0610 | 84 | Engine eng_left = Engine(PB_13, PB_4, PB_10); |
yruiewyrui3 | 0:2294f5ee0610 | 85 | Engine eng_right = Engine(PB_14, PB_5, PB_3); |
yruiewyrui3 | 0:2294f5ee0610 | 86 | int i = 0; |
yruiewyrui3 | 0:2294f5ee0610 | 87 | stm.baud(115200); |
yruiewyrui3 | 2:35dd310320e5 | 88 | |
yruiewyrui3 | 0:2294f5ee0610 | 89 | while(true){ |
yruiewyrui3 | 1:cad75f28ec58 | 90 | |
yruiewyrui3 | 2:35dd310320e5 | 91 | if(t.read_ms()>200){ //jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota |
yruiewyrui3 | 1:cad75f28ec58 | 92 | eng_left.move(0); |
yruiewyrui3 | 1:cad75f28ec58 | 93 | eng_right.move(0); |
yruiewyrui3 | 2:35dd310320e5 | 94 | //stm.printf("TIMEOUT-ROBOT STOP\n"); |
yruiewyrui3 | 1:cad75f28ec58 | 95 | } |
yruiewyrui3 | 1:cad75f28ec58 | 96 | |
yruiewyrui3 | 2:35dd310320e5 | 97 | //if((bufor[9] == NULL)&&(stm.readable())) |
yruiewyrui3 | 2:35dd310320e5 | 98 | if(bufor[9] == NULL) |
yruiewyrui3 | 0:2294f5ee0610 | 99 | { |
yruiewyrui3 | 2:35dd310320e5 | 100 | bufor[i] = stm.getc(); |
yruiewyrui3 | 2:35dd310320e5 | 101 | if(i==0){ |
yruiewyrui3 | 2:35dd310320e5 | 102 | if(bufor[i]=='@') //zaczynamy zapelniac bufor jak dostaniemy @ |
yruiewyrui3 | 2:35dd310320e5 | 103 | ++i; |
yruiewyrui3 | 2:35dd310320e5 | 104 | |
yruiewyrui3 | 2:35dd310320e5 | 105 | }else if(bufor[i]=='@'){ // i != 0 //interpretujemy jakby potencjalny poczatek ramki |
yruiewyrui3 | 2:35dd310320e5 | 106 | cleanBuffer(bufor); |
yruiewyrui3 | 2:35dd310320e5 | 107 | //stm.printf("blad\n"); |
yruiewyrui3 | 2:35dd310320e5 | 108 | bufor[0]='@'; |
yruiewyrui3 | 2:35dd310320e5 | 109 | i=1; //bo zerowy znak '@' juz zczytal |
yruiewyrui3 | 2:35dd310320e5 | 110 | |
yruiewyrui3 | 2:35dd310320e5 | 111 | //end added part// |
yruiewyrui3 | 2:35dd310320e5 | 112 | |
yruiewyrui3 | 2:35dd310320e5 | 113 | }else |
yruiewyrui3 | 2:35dd310320e5 | 114 | i++; |
yruiewyrui3 | 2:35dd310320e5 | 115 | |
yruiewyrui3 | 0:2294f5ee0610 | 116 | continue; |
yruiewyrui3 | 2:35dd310320e5 | 117 | } |
yruiewyrui3 | 2:35dd310320e5 | 118 | |
yruiewyrui3 | 2:35dd310320e5 | 119 | |
yruiewyrui3 | 2:35dd310320e5 | 120 | |
yruiewyrui3 | 2:35dd310320e5 | 121 | if(isCorrectPacket(bufor)) |
yruiewyrui3 | 2:35dd310320e5 | 122 | { |
yruiewyrui3 | 1:cad75f28ec58 | 123 | t.stop(); |
yruiewyrui3 | 1:cad75f28ec58 | 124 | t.reset(); |
yruiewyrui3 | 1:cad75f28ec58 | 125 | t.start(); |
yruiewyrui3 | 2:35dd310320e5 | 126 | counter++; |
yruiewyrui3 | 2:35dd310320e5 | 127 | stm.printf("%ld\n", counter); |
yruiewyrui3 | 2:35dd310320e5 | 128 | |
yruiewyrui3 | 2:35dd310320e5 | 129 | |
yruiewyrui3 | 1:cad75f28ec58 | 130 | for(int j=0; j<=2; j++) |
yruiewyrui3 | 1:cad75f28ec58 | 131 | { |
yruiewyrui3 | 2:35dd310320e5 | 132 | lewa[j]=bufor[j+2]; |
yruiewyrui3 | 1:cad75f28ec58 | 133 | } |
yruiewyrui3 | 2:35dd310320e5 | 134 | |
yruiewyrui3 | 1:cad75f28ec58 | 135 | for(int k=0; k<=2; k++) |
yruiewyrui3 | 1:cad75f28ec58 | 136 | { |
yruiewyrui3 | 2:35dd310320e5 | 137 | |
yruiewyrui3 | 2:35dd310320e5 | 138 | prawa[k]=bufor[k+6]; |
yruiewyrui3 | 0:2294f5ee0610 | 139 | } |
yruiewyrui3 | 2:35dd310320e5 | 140 | |
yruiewyrui3 | 2:35dd310320e5 | 141 | |
yruiewyrui3 | 1:cad75f28ec58 | 142 | sscanf(prawa, "%3d", &p); |
yruiewyrui3 | 1:cad75f28ec58 | 143 | sscanf(lewa, "%3d", &l); |
yruiewyrui3 | 2:35dd310320e5 | 144 | |
yruiewyrui3 | 2:35dd310320e5 | 145 | if(abs(l)>100) |
yruiewyrui3 | 2:35dd310320e5 | 146 | { |
yruiewyrui3 | 1:cad75f28ec58 | 147 | l=100; |
yruiewyrui3 | 2:35dd310320e5 | 148 | } |
yruiewyrui3 | 2:35dd310320e5 | 149 | |
yruiewyrui3 | 2:35dd310320e5 | 150 | if(abs(p)>100) |
yruiewyrui3 | 2:35dd310320e5 | 151 | { |
yruiewyrui3 | 1:cad75f28ec58 | 152 | p=100; |
yruiewyrui3 | 2:35dd310320e5 | 153 | } |
yruiewyrui3 | 1:cad75f28ec58 | 154 | |
yruiewyrui3 | 2:35dd310320e5 | 155 | // stm.printf("%ld\n", counter); |
yruiewyrui3 | 2:35dd310320e5 | 156 | for(int j=0; j<=BufferSize; j++) //wypisz |
yruiewyrui3 | 2:35dd310320e5 | 157 | { |
yruiewyrui3 | 2:35dd310320e5 | 158 | stm.printf("%c", bufor[j]); |
yruiewyrui3 | 2:35dd310320e5 | 159 | } |
yruiewyrui3 | 2:35dd310320e5 | 160 | stm.printf("\n"); |
yruiewyrui3 | 2:35dd310320e5 | 161 | |
yruiewyrui3 | 2:35dd310320e5 | 162 | //stm.printf("liczba lewa: %d \n", l); |
yruiewyrui3 | 2:35dd310320e5 | 163 | //stm.printf("liczba prawa: %d \n", p); |
yruiewyrui3 | 2:35dd310320e5 | 164 | |
yruiewyrui3 | 2:35dd310320e5 | 165 | if(bufor[1]=='-') l=-l; |
yruiewyrui3 | 2:35dd310320e5 | 166 | if(bufor[5]=='-') p=-p; |
yruiewyrui3 | 2:35dd310320e5 | 167 | |
yruiewyrui3 | 1:cad75f28ec58 | 168 | eng_left.move(l); |
yruiewyrui3 | 1:cad75f28ec58 | 169 | eng_right.move(p); |
yruiewyrui3 | 2:35dd310320e5 | 170 | |
yruiewyrui3 | 2:35dd310320e5 | 171 | |
yruiewyrui3 | 0:2294f5ee0610 | 172 | } |
yruiewyrui3 | 2:35dd310320e5 | 173 | i=0; |
yruiewyrui3 | 2:35dd310320e5 | 174 | cleanBuffer(bufor); |
yruiewyrui3 | 0:2294f5ee0610 | 175 | } |
yruiewyrui3 | 0:2294f5ee0610 | 176 | } |