Dominik Święch
/
AktalnyCZOSN
main + engine
main.cpp
- Committer:
- yruiewyrui3
- Date:
- 2016-06-02
- Revision:
- 1:cad75f28ec58
- Parent:
- 0:2294f5ee0610
File content as of revision 1:cad75f28ec58:
#include "mbed.h" #include "Engine.h" Serial stm(PA_2, PA_3); char bufor[8]; char prawa[3]; char lewa[3]; int p=0; int l=0; Timer t; int main(){ Engine eng_left = Engine(PB_13, PB_4, PB_10); Engine eng_right = Engine(PB_14, PB_5, PB_3); int i = 0; stm.baud(115200); while(true){ if(t.read_ms()>200){ eng_left.move(0); eng_right.move(0); } if((bufor[7] == NULL)&&(stm.readable())) { bufor[i] = stm.getc(); ++i; continue; } if(bufor[7] != NULL) { t.stop(); t.reset(); t.start(); for(int j=0; j<=2; j++) { lewa[j]=bufor[j+1]; } for(int k=0; k<=2; k++) { prawa[k]=bufor[k+5]; } sscanf(prawa, "%3d", &p); sscanf(lewa, "%3d", &l); if(abs(l)>100) { l=100; } if(abs(p)>100) { p=100; } stm.printf("liczba lewa: %d \n", l); stm.printf("liczba prawa: %d \n", p); if(bufor[0]=='-') l=-l; if(bufor[4]=='-') p=-p; eng_left.move(l); eng_right.move(p); if ((l==0) && (p==0)) { stm.printf("Pale gume w miejscu bo kazali mi stac"); } else if ((l>0)&& (p>0)){ stm.printf("Jade do przodu. Predkosc lewej strony to %d, a prawej strony to %d \n", l, p); } else if ((l<0)&& (p<0)){ stm.printf("Jade do tylu. Predkosc lewej strony to %d, a prawej strony to %d \n", l, p); } else if ((l>=0)&& (p<=0)){ stm.printf("Obracam sie w prawo. Predkosc lewej strony to %d, a prawej strony to %d \n", l, p); } else if ((l<=0)&& (p>=0)){ stm.printf("Obracam sie w lewo. Predkosc lewej strony to %d, a prawej strony to %d \n", l, p); } i=0; for (int j=0; j<8; j++) { bufor[j]=NULL; } } } }