Dominik Święch
/
AktalnyCZOSN
main + engine
main.cpp@1:cad75f28ec58, 2016-06-02 (annotated)
- Committer:
- yruiewyrui3
- Date:
- Thu Jun 02 09:11:20 2016 +0000
- Revision:
- 1:cad75f28ec58
- Parent:
- 0:2294f5ee0610
0.2s bez nowego komunikatu, zatrzymanie
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 | 0:2294f5ee0610 | 3 | |
yruiewyrui3 | 0:2294f5ee0610 | 4 | Serial stm(PA_2, PA_3); |
yruiewyrui3 | 0:2294f5ee0610 | 5 | char bufor[8]; |
yruiewyrui3 | 0:2294f5ee0610 | 6 | char prawa[3]; |
yruiewyrui3 | 0:2294f5ee0610 | 7 | char lewa[3]; |
yruiewyrui3 | 0:2294f5ee0610 | 8 | int p=0; |
yruiewyrui3 | 0:2294f5ee0610 | 9 | int l=0; |
yruiewyrui3 | 1:cad75f28ec58 | 10 | Timer t; |
yruiewyrui3 | 0:2294f5ee0610 | 11 | |
yruiewyrui3 | 0:2294f5ee0610 | 12 | int main(){ |
yruiewyrui3 | 0:2294f5ee0610 | 13 | Engine eng_left = Engine(PB_13, PB_4, PB_10); |
yruiewyrui3 | 0:2294f5ee0610 | 14 | Engine eng_right = Engine(PB_14, PB_5, PB_3); |
yruiewyrui3 | 0:2294f5ee0610 | 15 | int i = 0; |
yruiewyrui3 | 0:2294f5ee0610 | 16 | stm.baud(115200); |
yruiewyrui3 | 0:2294f5ee0610 | 17 | |
yruiewyrui3 | 0:2294f5ee0610 | 18 | while(true){ |
yruiewyrui3 | 1:cad75f28ec58 | 19 | |
yruiewyrui3 | 1:cad75f28ec58 | 20 | if(t.read_ms()>200){ |
yruiewyrui3 | 1:cad75f28ec58 | 21 | eng_left.move(0); |
yruiewyrui3 | 1:cad75f28ec58 | 22 | eng_right.move(0); |
yruiewyrui3 | 1:cad75f28ec58 | 23 | } |
yruiewyrui3 | 1:cad75f28ec58 | 24 | |
yruiewyrui3 | 1:cad75f28ec58 | 25 | if((bufor[7] == NULL)&&(stm.readable())) |
yruiewyrui3 | 0:2294f5ee0610 | 26 | { |
yruiewyrui3 | 1:cad75f28ec58 | 27 | bufor[i] = stm.getc(); |
yruiewyrui3 | 0:2294f5ee0610 | 28 | ++i; |
yruiewyrui3 | 0:2294f5ee0610 | 29 | continue; |
yruiewyrui3 | 0:2294f5ee0610 | 30 | } |
yruiewyrui3 | 1:cad75f28ec58 | 31 | if(bufor[7] != NULL) |
yruiewyrui3 | 0:2294f5ee0610 | 32 | { |
yruiewyrui3 | 1:cad75f28ec58 | 33 | t.stop(); |
yruiewyrui3 | 1:cad75f28ec58 | 34 | t.reset(); |
yruiewyrui3 | 1:cad75f28ec58 | 35 | t.start(); |
yruiewyrui3 | 1:cad75f28ec58 | 36 | |
yruiewyrui3 | 1:cad75f28ec58 | 37 | for(int j=0; j<=2; j++) |
yruiewyrui3 | 1:cad75f28ec58 | 38 | { |
yruiewyrui3 | 1:cad75f28ec58 | 39 | lewa[j]=bufor[j+1]; |
yruiewyrui3 | 1:cad75f28ec58 | 40 | } |
yruiewyrui3 | 1:cad75f28ec58 | 41 | |
yruiewyrui3 | 1:cad75f28ec58 | 42 | for(int k=0; k<=2; k++) |
yruiewyrui3 | 1:cad75f28ec58 | 43 | { |
yruiewyrui3 | 1:cad75f28ec58 | 44 | prawa[k]=bufor[k+5]; |
yruiewyrui3 | 0:2294f5ee0610 | 45 | } |
yruiewyrui3 | 1:cad75f28ec58 | 46 | |
yruiewyrui3 | 1:cad75f28ec58 | 47 | sscanf(prawa, "%3d", &p); |
yruiewyrui3 | 1:cad75f28ec58 | 48 | sscanf(lewa, "%3d", &l); |
yruiewyrui3 | 1:cad75f28ec58 | 49 | |
yruiewyrui3 | 1:cad75f28ec58 | 50 | if(abs(l)>100) { |
yruiewyrui3 | 1:cad75f28ec58 | 51 | l=100; |
yruiewyrui3 | 1:cad75f28ec58 | 52 | } |
yruiewyrui3 | 1:cad75f28ec58 | 53 | |
yruiewyrui3 | 1:cad75f28ec58 | 54 | if(abs(p)>100) { |
yruiewyrui3 | 1:cad75f28ec58 | 55 | p=100; |
yruiewyrui3 | 1:cad75f28ec58 | 56 | } |
yruiewyrui3 | 1:cad75f28ec58 | 57 | |
yruiewyrui3 | 1:cad75f28ec58 | 58 | stm.printf("liczba lewa: %d \n", l); |
yruiewyrui3 | 1:cad75f28ec58 | 59 | stm.printf("liczba prawa: %d \n", p); |
yruiewyrui3 | 1:cad75f28ec58 | 60 | |
yruiewyrui3 | 1:cad75f28ec58 | 61 | if(bufor[0]=='-') l=-l; |
yruiewyrui3 | 1:cad75f28ec58 | 62 | if(bufor[4]=='-') p=-p; |
yruiewyrui3 | 1:cad75f28ec58 | 63 | |
yruiewyrui3 | 1:cad75f28ec58 | 64 | eng_left.move(l); |
yruiewyrui3 | 1:cad75f28ec58 | 65 | eng_right.move(p); |
yruiewyrui3 | 1:cad75f28ec58 | 66 | |
yruiewyrui3 | 1:cad75f28ec58 | 67 | if ((l==0) && (p==0)) { |
yruiewyrui3 | 1:cad75f28ec58 | 68 | stm.printf("Pale gume w miejscu bo kazali mi stac"); |
yruiewyrui3 | 1:cad75f28ec58 | 69 | } |
yruiewyrui3 | 1:cad75f28ec58 | 70 | else if ((l>0)&& (p>0)){ |
yruiewyrui3 | 1:cad75f28ec58 | 71 | stm.printf("Jade do przodu. Predkosc lewej strony to %d, a prawej strony to %d \n", l, p); |
yruiewyrui3 | 1:cad75f28ec58 | 72 | } |
yruiewyrui3 | 1:cad75f28ec58 | 73 | else if ((l<0)&& (p<0)){ |
yruiewyrui3 | 1:cad75f28ec58 | 74 | stm.printf("Jade do tylu. Predkosc lewej strony to %d, a prawej strony to %d \n", l, p); |
yruiewyrui3 | 1:cad75f28ec58 | 75 | } |
yruiewyrui3 | 1:cad75f28ec58 | 76 | else if ((l>=0)&& (p<=0)){ |
yruiewyrui3 | 1:cad75f28ec58 | 77 | stm.printf("Obracam sie w prawo. Predkosc lewej strony to %d, a prawej strony to %d \n", l, p); |
yruiewyrui3 | 1:cad75f28ec58 | 78 | } |
yruiewyrui3 | 1:cad75f28ec58 | 79 | else if ((l<=0)&& (p>=0)){ |
yruiewyrui3 | 1:cad75f28ec58 | 80 | stm.printf("Obracam sie w lewo. Predkosc lewej strony to %d, a prawej strony to %d \n", l, p); |
yruiewyrui3 | 1:cad75f28ec58 | 81 | } |
yruiewyrui3 | 1:cad75f28ec58 | 82 | |
yruiewyrui3 | 1:cad75f28ec58 | 83 | i=0; |
yruiewyrui3 | 1:cad75f28ec58 | 84 | for (int j=0; j<8; j++) |
yruiewyrui3 | 1:cad75f28ec58 | 85 | { |
yruiewyrui3 | 1:cad75f28ec58 | 86 | bufor[j]=NULL; |
yruiewyrui3 | 0:2294f5ee0610 | 87 | } |
yruiewyrui3 | 0:2294f5ee0610 | 88 | } |
yruiewyrui3 | 0:2294f5ee0610 | 89 | } |
yruiewyrui3 | 0:2294f5ee0610 | 90 | } |