nowe + funkcje na lux
Dependencies: Servo_fullrange mbed
Fork of Kola_Servo_nowe by
main.cpp@1:da4c204737bf, 2016-06-14 (annotated)
- Committer:
- yruiewyrui3
- Date:
- Tue Jun 14 13:32:33 2016 +0000
- Revision:
- 1:da4c204737bf
- Parent:
- 0:2e73d45d090e
- Child:
- 2:325a730cd438
.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yruiewyrui3 | 0:2e73d45d090e | 1 | #include "mbed.h" |
yruiewyrui3 | 0:2e73d45d090e | 2 | #include "Engine.h" |
yruiewyrui3 | 0:2e73d45d090e | 3 | #include "Servo.h" |
yruiewyrui3 | 0:2e73d45d090e | 4 | |
yruiewyrui3 | 0:2e73d45d090e | 5 | //*************** Deklaracja wszystkich portów potrzebnych do obsługi************** |
yruiewyrui3 | 0:2e73d45d090e | 6 | Serial stm(PA_2, PA_3); |
yruiewyrui3 | 0:2e73d45d090e | 7 | Servo cam_poziom(PB_6); |
yruiewyrui3 | 0:2e73d45d090e | 8 | Servo cam_pion(PC_7); |
yruiewyrui3 | 0:2e73d45d090e | 9 | Engine eng_left = Engine(PB_13, PB_4, PB_10); |
yruiewyrui3 | 0:2e73d45d090e | 10 | Engine eng_right = Engine(PB_14, PB_5, PB_3); |
yruiewyrui3 | 0:2e73d45d090e | 11 | |
yruiewyrui3 | 0:2e73d45d090e | 12 | //*************** Deklaracja zmiennych globalnych, tablic, bufora na ramke znaków ************** |
yruiewyrui3 | 0:2e73d45d090e | 13 | const int BufferSize=10; |
yruiewyrui3 | 0:2e73d45d090e | 14 | char bufor[BufferSize]; |
yruiewyrui3 | 0:2e73d45d090e | 15 | char prawa[3]; |
yruiewyrui3 | 0:2e73d45d090e | 16 | char lewa[3]; |
yruiewyrui3 | 0:2e73d45d090e | 17 | char poziom[2]; |
yruiewyrui3 | 0:2e73d45d090e | 18 | char pion[2]; |
yruiewyrui3 | 0:2e73d45d090e | 19 | int l=0; |
yruiewyrui3 | 0:2e73d45d090e | 20 | int p=0; |
yruiewyrui3 | 0:2e73d45d090e | 21 | int po=0; |
yruiewyrui3 | 0:2e73d45d090e | 22 | int pi=0; |
yruiewyrui3 | 0:2e73d45d090e | 23 | Timer t; |
yruiewyrui3 | 0:2e73d45d090e | 24 | |
yruiewyrui3 | 0:2e73d45d090e | 25 | //*************** Funkcja czyszczaca bufor ***************** |
yruiewyrui3 | 0:2e73d45d090e | 26 | void cleanBuffer(char *buffor) |
yruiewyrui3 | 0:2e73d45d090e | 27 | { |
yruiewyrui3 | 0:2e73d45d090e | 28 | for(int i=0; i<BufferSize; i++) |
yruiewyrui3 | 0:2e73d45d090e | 29 | buffor[i]=NULL; |
yruiewyrui3 | 0:2e73d45d090e | 30 | buffor[BufferSize]=NULL; |
yruiewyrui3 | 0:2e73d45d090e | 31 | } |
yruiewyrui3 | 0:2e73d45d090e | 32 | |
yruiewyrui3 | 0:2e73d45d090e | 33 | //***************OPIS FUNKCJI isCorrectPacket******************************************************// |
yruiewyrui3 | 0:2e73d45d090e | 34 | // funkcja sprawdza czy ramka spelnia wymagania protokolu: @ZCCCZCCC$ lub &ZCCCZCCC$ gdzie Z={+/-} C={0,1,...9} // |
yruiewyrui3 | 0:2e73d45d090e | 35 | //*******************KONIEC OPISU******************************************************************// |
yruiewyrui3 | 0:2e73d45d090e | 36 | |
yruiewyrui3 | 0:2e73d45d090e | 37 | bool isCorrectPacket(char *buffor){ |
yruiewyrui3 | 0:2e73d45d090e | 38 | if ((bufor[0]!='@') && (bufor[0]!='&')) |
yruiewyrui3 | 0:2e73d45d090e | 39 | { |
yruiewyrui3 | 0:2e73d45d090e | 40 | // stm.printf("ZLY ZNAK POCZATKU\n"); |
yruiewyrui3 | 0:2e73d45d090e | 41 | stm.printf("blad\n"); |
yruiewyrui3 | 0:2e73d45d090e | 42 | return false; |
yruiewyrui3 | 0:2e73d45d090e | 43 | } |
yruiewyrui3 | 0:2e73d45d090e | 44 | if((buffor[1] != '+') && (buffor[1] != '-')) |
yruiewyrui3 | 0:2e73d45d090e | 45 | { |
yruiewyrui3 | 0:2e73d45d090e | 46 | // stm.printf("ZLY ZNAK +/- LEWY\n"); |
yruiewyrui3 | 0:2e73d45d090e | 47 | stm.printf("blad\n"); |
yruiewyrui3 | 0:2e73d45d090e | 48 | return false; |
yruiewyrui3 | 0:2e73d45d090e | 49 | } |
yruiewyrui3 | 0:2e73d45d090e | 50 | if((buffor[5] != '+') && (buffor[5] != '-')) |
yruiewyrui3 | 0:2e73d45d090e | 51 | { |
yruiewyrui3 | 0:2e73d45d090e | 52 | // stm.printf("ZLY ZNAK +/- PRAWY\n"); |
yruiewyrui3 | 0:2e73d45d090e | 53 | stm.printf("blad\n"); |
yruiewyrui3 | 0:2e73d45d090e | 54 | return false; |
yruiewyrui3 | 0:2e73d45d090e | 55 | } |
yruiewyrui3 | 0:2e73d45d090e | 56 | if(bufor[9]!='$') |
yruiewyrui3 | 0:2e73d45d090e | 57 | { |
yruiewyrui3 | 0:2e73d45d090e | 58 | // stm.printf("ZLY ZNAK KONCA\n"); |
yruiewyrui3 | 0:2e73d45d090e | 59 | stm.printf("blad\n"); |
yruiewyrui3 | 0:2e73d45d090e | 60 | return false; |
yruiewyrui3 | 0:2e73d45d090e | 61 | } |
yruiewyrui3 | 0:2e73d45d090e | 62 | for(int i=2; i< 5; i++){ |
yruiewyrui3 | 0:2e73d45d090e | 63 | if(((int)buffor[i]) < 48 || ((int)buffor[i]) > 57) |
yruiewyrui3 | 0:2e73d45d090e | 64 | { |
yruiewyrui3 | 0:2e73d45d090e | 65 | // stm.printf("NA LEWE KOLO: NIE LICZBA\n"); |
yruiewyrui3 | 0:2e73d45d090e | 66 | stm.printf("blad\n"); |
yruiewyrui3 | 0:2e73d45d090e | 67 | return false; |
yruiewyrui3 | 0:2e73d45d090e | 68 | } |
yruiewyrui3 | 0:2e73d45d090e | 69 | if(((int)buffor[i+4]) < 48 || ((int)buffor[i+4]) > 57) |
yruiewyrui3 | 0:2e73d45d090e | 70 | { |
yruiewyrui3 | 0:2e73d45d090e | 71 | // stm.printf("NA PRAWE KOLO: NIE LICZBA\n"); |
yruiewyrui3 | 0:2e73d45d090e | 72 | stm.printf("blad\n"); |
yruiewyrui3 | 0:2e73d45d090e | 73 | return false; |
yruiewyrui3 | 0:2e73d45d090e | 74 | } |
yruiewyrui3 | 0:2e73d45d090e | 75 | } |
yruiewyrui3 | 0:2e73d45d090e | 76 | //stm.printf("pakiet poprawny\n");] |
yruiewyrui3 | 0:2e73d45d090e | 77 | return true; |
yruiewyrui3 | 0:2e73d45d090e | 78 | } |
yruiewyrui3 | 0:2e73d45d090e | 79 | |
yruiewyrui3 | 0:2e73d45d090e | 80 | void move_wheels() |
yruiewyrui3 | 0:2e73d45d090e | 81 | { |
yruiewyrui3 | 0:2e73d45d090e | 82 | //*******PRZYPISANIE DO TABLICY LEWEJ LICZBY*********// |
yruiewyrui3 | 0:2e73d45d090e | 83 | for(int j=0; j<=2; j++){ |
yruiewyrui3 | 0:2e73d45d090e | 84 | lewa[j]=bufor[j+2]; |
yruiewyrui3 | 0:2e73d45d090e | 85 | } |
yruiewyrui3 | 0:2e73d45d090e | 86 | //*******PRZYPISANIE DO TABLICY PRAWEJ LICZBY*********// |
yruiewyrui3 | 0:2e73d45d090e | 87 | for(int k=0; k<=2; k++){ |
yruiewyrui3 | 0:2e73d45d090e | 88 | prawa[k]=bufor[k+6]; |
yruiewyrui3 | 0:2e73d45d090e | 89 | } |
yruiewyrui3 | 0:2e73d45d090e | 90 | //*******KONWERSJA CHAROW NA INTY*********// |
yruiewyrui3 | 0:2e73d45d090e | 91 | sscanf(lewa, "%3d", &l); |
yruiewyrui3 | 0:2e73d45d090e | 92 | sscanf(prawa, "%3d", &p); |
yruiewyrui3 | 0:2e73d45d090e | 93 | //********KOREKCJA***************// |
yruiewyrui3 | 0:2e73d45d090e | 94 | if(abs(l)>100){ |
yruiewyrui3 | 0:2e73d45d090e | 95 | l=100; |
yruiewyrui3 | 0:2e73d45d090e | 96 | } |
yruiewyrui3 | 0:2e73d45d090e | 97 | if(abs(p)>100){ |
yruiewyrui3 | 0:2e73d45d090e | 98 | p=100; |
yruiewyrui3 | 0:2e73d45d090e | 99 | } |
yruiewyrui3 | 0:2e73d45d090e | 100 | //************WYPISYWANIE*******************// |
yruiewyrui3 | 0:2e73d45d090e | 101 | for(int j=0; j<=BufferSize; j++){ |
yruiewyrui3 | 0:2e73d45d090e | 102 | stm.putc(bufor[j]); |
yruiewyrui3 | 0:2e73d45d090e | 103 | } |
yruiewyrui3 | 0:2e73d45d090e | 104 | //******KOREKCJA ZNAKU***********// |
yruiewyrui3 | 0:2e73d45d090e | 105 | if(bufor[1]=='-') l=-l; |
yruiewyrui3 | 0:2e73d45d090e | 106 | if(bufor[5]=='-') p=-p; |
yruiewyrui3 | 0:2e73d45d090e | 107 | //**********ODPALANIE SILNIKOW******// |
yruiewyrui3 | 0:2e73d45d090e | 108 | eng_left.move(l); |
yruiewyrui3 | 0:2e73d45d090e | 109 | eng_right.move(p); |
yruiewyrui3 | 0:2e73d45d090e | 110 | |
yruiewyrui3 | 0:2e73d45d090e | 111 | stm.printf(bufor); |
yruiewyrui3 | 0:2e73d45d090e | 112 | } |
yruiewyrui3 | 0:2e73d45d090e | 113 | void move_camera() |
yruiewyrui3 | 0:2e73d45d090e | 114 | { |
yruiewyrui3 | 0:2e73d45d090e | 115 | //*******PRZYPISANIE DO KAMERY POZIOM*********// |
yruiewyrui3 | 0:2e73d45d090e | 116 | for(int m=0; m<2; m++){ |
yruiewyrui3 | 0:2e73d45d090e | 117 | poziom[m]=bufor[m+3]; |
yruiewyrui3 | 0:2e73d45d090e | 118 | } |
yruiewyrui3 | 0:2e73d45d090e | 119 | //*******PRZYPISANIE DO KAMERY PION*********// |
yruiewyrui3 | 0:2e73d45d090e | 120 | for(int n=0; n<2; n++){ |
yruiewyrui3 | 0:2e73d45d090e | 121 | pion[n]=bufor[n+3]; |
yruiewyrui3 | 0:2e73d45d090e | 122 | } |
yruiewyrui3 | 0:2e73d45d090e | 123 | //*******KONWERSJA CHAROW NA INTY*********// |
yruiewyrui3 | 0:2e73d45d090e | 124 | sscanf(poziom, "%2d", &po); |
yruiewyrui3 | 0:2e73d45d090e | 125 | sscanf(pion, "%2d", &pi); |
yruiewyrui3 | 0:2e73d45d090e | 126 | //********KOREKCJA***************// |
yruiewyrui3 | 0:2e73d45d090e | 127 | if(abs(po)>90){ |
yruiewyrui3 | 0:2e73d45d090e | 128 | po=90; |
yruiewyrui3 | 0:2e73d45d090e | 129 | } |
yruiewyrui3 | 0:2e73d45d090e | 130 | if(abs(pi)>90){ |
yruiewyrui3 | 0:2e73d45d090e | 131 | pi=90; |
yruiewyrui3 | 0:2e73d45d090e | 132 | } |
yruiewyrui3 | 0:2e73d45d090e | 133 | //************WYPISYWANIE*******************// |
yruiewyrui3 | 0:2e73d45d090e | 134 | for(int j=0; j<=BufferSize; j++){ |
yruiewyrui3 | 0:2e73d45d090e | 135 | stm.putc(bufor[j]); |
yruiewyrui3 | 0:2e73d45d090e | 136 | } |
yruiewyrui3 | 0:2e73d45d090e | 137 | if(bufor[1]=='-') po=-po; |
yruiewyrui3 | 0:2e73d45d090e | 138 | if(bufor[5]=='-') pi=-pi; |
yruiewyrui3 | 0:2e73d45d090e | 139 | //**********RUCH KAMERĄ******// |
yruiewyrui3 | 0:2e73d45d090e | 140 | cam_poziom.position(po); |
yruiewyrui3 | 0:2e73d45d090e | 141 | cam_pion.position(pi); |
yruiewyrui3 | 0:2e73d45d090e | 142 | } |
yruiewyrui3 | 1:da4c204737bf | 143 | void fill_buffer(int i) |
yruiewyrui3 | 0:2e73d45d090e | 144 | { |
yruiewyrui3 | 0:2e73d45d090e | 145 | bufor[i] = stm.getc(); |
yruiewyrui3 | 0:2e73d45d090e | 146 | if(i==0){ |
yruiewyrui3 | 0:2e73d45d090e | 147 | if((bufor[i]=='@') || (bufor[i]=='&')) //zaczynamy zapelniac bufor jak dostaniemy @ |
yruiewyrui3 | 0:2e73d45d090e | 148 | ++i; |
yruiewyrui3 | 0:2e73d45d090e | 149 | } |
yruiewyrui3 | 0:2e73d45d090e | 150 | else if(bufor[i]=='@'){ // i != 0 //interpretujemy jakby potencjalny poczatek ramki |
yruiewyrui3 | 0:2e73d45d090e | 151 | cleanBuffer(bufor); |
yruiewyrui3 | 0:2e73d45d090e | 152 | //stm.printf("blad\n"); |
yruiewyrui3 | 0:2e73d45d090e | 153 | bufor[0]='@'; |
yruiewyrui3 | 0:2e73d45d090e | 154 | i=1; //bo zerowy znak '@' juz zczytal |
yruiewyrui3 | 0:2e73d45d090e | 155 | } |
yruiewyrui3 | 0:2e73d45d090e | 156 | else if (bufor[i]=='&'){ // i != 0 //interpretujemy jakby potencjalny poczatek ramki |
yruiewyrui3 | 0:2e73d45d090e | 157 | cleanBuffer(bufor); |
yruiewyrui3 | 0:2e73d45d090e | 158 | //stm.printf("blad\n"); |
yruiewyrui3 | 0:2e73d45d090e | 159 | bufor[0]='&'; |
yruiewyrui3 | 0:2e73d45d090e | 160 | i=1; //bo zerowy znak '@' juz zczytal |
yruiewyrui3 | 0:2e73d45d090e | 161 | } |
yruiewyrui3 | 0:2e73d45d090e | 162 | else |
yruiewyrui3 | 0:2e73d45d090e | 163 | i++; |
yruiewyrui3 | 0:2e73d45d090e | 164 | } |
yruiewyrui3 | 0:2e73d45d090e | 165 | |
yruiewyrui3 | 0:2e73d45d090e | 166 | int main(){ |
yruiewyrui3 | 0:2e73d45d090e | 167 | int i = 0; |
yruiewyrui3 | 1:da4c204737bf | 168 | cleanBuffer(bufor); |
yruiewyrui3 | 0:2e73d45d090e | 169 | stm.baud(115200); |
yruiewyrui3 | 0:2e73d45d090e | 170 | while(true){ |
yruiewyrui3 | 0:2e73d45d090e | 171 | if(t.read_ms()>200){ //jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota |
yruiewyrui3 | 0:2e73d45d090e | 172 | eng_left.move(0); |
yruiewyrui3 | 0:2e73d45d090e | 173 | eng_right.move(0); |
yruiewyrui3 | 0:2e73d45d090e | 174 | } |
yruiewyrui3 | 0:2e73d45d090e | 175 | //if((bufor[9] == NULL)&&(stm.readable())) |
yruiewyrui3 | 1:da4c204737bf | 176 | while (bufor[9] == NULL){ |
yruiewyrui3 | 1:da4c204737bf | 177 | fill_buffer(i); |
yruiewyrui3 | 0:2e73d45d090e | 178 | } |
yruiewyrui3 | 0:2e73d45d090e | 179 | if(isCorrectPacket(bufor)){ |
yruiewyrui3 | 0:2e73d45d090e | 180 | t.stop(); |
yruiewyrui3 | 0:2e73d45d090e | 181 | t.reset(); |
yruiewyrui3 | 0:2e73d45d090e | 182 | t.start(); |
yruiewyrui3 | 0:2e73d45d090e | 183 | } |
yruiewyrui3 | 0:2e73d45d090e | 184 | if(bufor[0]=='@'){ |
yruiewyrui3 | 0:2e73d45d090e | 185 | move_wheels(); |
yruiewyrui3 | 0:2e73d45d090e | 186 | } |
yruiewyrui3 | 0:2e73d45d090e | 187 | else if(bufor[0]=='&'){ |
yruiewyrui3 | 0:2e73d45d090e | 188 | move_camera(); |
yruiewyrui3 | 0:2e73d45d090e | 189 | } |
yruiewyrui3 | 0:2e73d45d090e | 190 | i=0; |
yruiewyrui3 | 0:2e73d45d090e | 191 | cleanBuffer(bufor); |
yruiewyrui3 | 0:2e73d45d090e | 192 | } |
yruiewyrui3 | 0:2e73d45d090e | 193 | } |