Dominik Święch / Mbed 2 deprecated Komunikacja_read_write

main + engine

Dependencies:   mbed

Fork of AktalnyCZOSN by Dominik Święch

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?

UserRevisionLine numberNew 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 }