nowe + funkcje na lux
Dependencies: Servo_fullrange mbed
Fork of Kola_Servo_nowe by
main.cpp@2:325a730cd438, 2016-06-15 (annotated)
- Committer:
- yruiewyrui3
- Date:
- Wed Jun 15 12:33:51 2016 +0000
- Revision:
- 2:325a730cd438
- Parent:
- 1:da4c204737bf
lux
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 | 2:325a730cd438 | 38 | if (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 | 2:325a730cd438 | 56 | if((bufor[9]!='$')&&(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 | void move_camera() |
yruiewyrui3 | 0:2e73d45d090e | 112 | { |
yruiewyrui3 | 0:2e73d45d090e | 113 | //*******PRZYPISANIE DO KAMERY POZIOM*********// |
yruiewyrui3 | 0:2e73d45d090e | 114 | for(int m=0; m<2; m++){ |
yruiewyrui3 | 0:2e73d45d090e | 115 | poziom[m]=bufor[m+3]; |
yruiewyrui3 | 0:2e73d45d090e | 116 | } |
yruiewyrui3 | 0:2e73d45d090e | 117 | //*******PRZYPISANIE DO KAMERY PION*********// |
yruiewyrui3 | 0:2e73d45d090e | 118 | for(int n=0; n<2; n++){ |
yruiewyrui3 | 0:2e73d45d090e | 119 | pion[n]=bufor[n+3]; |
yruiewyrui3 | 0:2e73d45d090e | 120 | } |
yruiewyrui3 | 0:2e73d45d090e | 121 | //*******KONWERSJA CHAROW NA INTY*********// |
yruiewyrui3 | 0:2e73d45d090e | 122 | sscanf(poziom, "%2d", &po); |
yruiewyrui3 | 0:2e73d45d090e | 123 | sscanf(pion, "%2d", &pi); |
yruiewyrui3 | 0:2e73d45d090e | 124 | //********KOREKCJA***************// |
yruiewyrui3 | 0:2e73d45d090e | 125 | if(abs(po)>90){ |
yruiewyrui3 | 0:2e73d45d090e | 126 | po=90; |
yruiewyrui3 | 0:2e73d45d090e | 127 | } |
yruiewyrui3 | 0:2e73d45d090e | 128 | if(abs(pi)>90){ |
yruiewyrui3 | 0:2e73d45d090e | 129 | pi=90; |
yruiewyrui3 | 0:2e73d45d090e | 130 | } |
yruiewyrui3 | 0:2e73d45d090e | 131 | //************WYPISYWANIE*******************// |
yruiewyrui3 | 0:2e73d45d090e | 132 | for(int j=0; j<=BufferSize; j++){ |
yruiewyrui3 | 0:2e73d45d090e | 133 | stm.putc(bufor[j]); |
yruiewyrui3 | 0:2e73d45d090e | 134 | } |
yruiewyrui3 | 0:2e73d45d090e | 135 | if(bufor[1]=='-') po=-po; |
yruiewyrui3 | 0:2e73d45d090e | 136 | if(bufor[5]=='-') pi=-pi; |
yruiewyrui3 | 0:2e73d45d090e | 137 | //**********RUCH KAMERĄ******// |
yruiewyrui3 | 0:2e73d45d090e | 138 | cam_poziom.position(po); |
yruiewyrui3 | 0:2e73d45d090e | 139 | cam_pion.position(pi); |
yruiewyrui3 | 0:2e73d45d090e | 140 | } |
yruiewyrui3 | 2:325a730cd438 | 141 | |
yruiewyrui3 | 0:2e73d45d090e | 142 | int main(){ |
yruiewyrui3 | 0:2e73d45d090e | 143 | int i = 0; |
yruiewyrui3 | 1:da4c204737bf | 144 | cleanBuffer(bufor); |
yruiewyrui3 | 0:2e73d45d090e | 145 | stm.baud(115200); |
yruiewyrui3 | 0:2e73d45d090e | 146 | while(true){ |
yruiewyrui3 | 0:2e73d45d090e | 147 | if(t.read_ms()>200){ //jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota |
yruiewyrui3 | 0:2e73d45d090e | 148 | eng_left.move(0); |
yruiewyrui3 | 0:2e73d45d090e | 149 | eng_right.move(0); |
yruiewyrui3 | 0:2e73d45d090e | 150 | } |
yruiewyrui3 | 2:325a730cd438 | 151 | if(bufor[9] == NULL){ |
yruiewyrui3 | 2:325a730cd438 | 152 | bufor[i] = stm.getc(); |
yruiewyrui3 | 2:325a730cd438 | 153 | if(i==0){ |
yruiewyrui3 | 2:325a730cd438 | 154 | if(bufor[i]=='@') //zaczynamy zapelniac bufor jak dostaniemy @ |
yruiewyrui3 | 2:325a730cd438 | 155 | ++i; |
yruiewyrui3 | 2:325a730cd438 | 156 | } |
yruiewyrui3 | 2:325a730cd438 | 157 | else if(bufor[i]=='@'){ // i != 0 //interpretujemy jakby potencjalny poczatek ramki |
yruiewyrui3 | 2:325a730cd438 | 158 | cleanBuffer(bufor); |
yruiewyrui3 | 2:325a730cd438 | 159 | bufor[0]='@'; |
yruiewyrui3 | 2:325a730cd438 | 160 | i=1; //bo zerowy znak '@' juz zczytal |
yruiewyrui3 | 2:325a730cd438 | 161 | } |
yruiewyrui3 | 2:325a730cd438 | 162 | else i++; |
yruiewyrui3 | 2:325a730cd438 | 163 | continue; |
yruiewyrui3 | 2:325a730cd438 | 164 | } |
yruiewyrui3 | 0:2e73d45d090e | 165 | if(isCorrectPacket(bufor)){ |
yruiewyrui3 | 0:2e73d45d090e | 166 | t.stop(); |
yruiewyrui3 | 0:2e73d45d090e | 167 | t.reset(); |
yruiewyrui3 | 0:2e73d45d090e | 168 | t.start(); |
yruiewyrui3 | 0:2e73d45d090e | 169 | } |
yruiewyrui3 | 2:325a730cd438 | 170 | if(bufor[9]=='$'){ |
yruiewyrui3 | 0:2e73d45d090e | 171 | move_wheels(); |
yruiewyrui3 | 0:2e73d45d090e | 172 | } |
yruiewyrui3 | 2:325a730cd438 | 173 | else if(bufor[9]=='&'){ |
yruiewyrui3 | 0:2e73d45d090e | 174 | move_camera(); |
yruiewyrui3 | 0:2e73d45d090e | 175 | } |
yruiewyrui3 | 0:2e73d45d090e | 176 | i=0; |
yruiewyrui3 | 0:2e73d45d090e | 177 | cleanBuffer(bufor); |
yruiewyrui3 | 0:2e73d45d090e | 178 | } |
yruiewyrui3 | 0:2e73d45d090e | 179 | } |