Dominik Święch / Mbed 2 deprecated Kola_Servo

main + engine

Dependencies:   Servo mbed

Fork of Komunikacja_read_write by Dominik Święch

main.cpp@0:2294f5ee0610, 2016-06-01 (annotated)

Committer:
yruiewyrui3
Date:
Wed Jun 01 10:47:53 2016 +0000
Revision:
0:2294f5ee0610
Child:
1:cad75f28ec58
Aktualny CZOSN

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 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 0:2294f5ee0610 10
yruiewyrui3 0:2294f5ee0610 11 int main(){
yruiewyrui3 0:2294f5ee0610 12 Engine eng_left = Engine(PB_13, PB_4, PB_10);
yruiewyrui3 0:2294f5ee0610 13 Engine eng_right = Engine(PB_14, PB_5, PB_3);
yruiewyrui3 0:2294f5ee0610 14 int i = 0;
yruiewyrui3 0:2294f5ee0610 15 stm.baud(115200);
yruiewyrui3 0:2294f5ee0610 16
yruiewyrui3 0:2294f5ee0610 17 while(true){
yruiewyrui3 0:2294f5ee0610 18 bufor[i] = stm.getc();
yruiewyrui3 0:2294f5ee0610 19 if(bufor[7] == NULL)
yruiewyrui3 0:2294f5ee0610 20 {
yruiewyrui3 0:2294f5ee0610 21 ++i;
yruiewyrui3 0:2294f5ee0610 22 continue;
yruiewyrui3 0:2294f5ee0610 23 }
yruiewyrui3 0:2294f5ee0610 24
yruiewyrui3 0:2294f5ee0610 25 /*for (int j=0; j<8; j++)
yruiewyrui3 0:2294f5ee0610 26 {
yruiewyrui3 0:2294f5ee0610 27 stm.putc(bufor[j]);
yruiewyrui3 0:2294f5ee0610 28 }*/
yruiewyrui3 0:2294f5ee0610 29 for(int j=0; j<=2; j++)
yruiewyrui3 0:2294f5ee0610 30 {
yruiewyrui3 0:2294f5ee0610 31 lewa[j]=bufor[j+1];
yruiewyrui3 0:2294f5ee0610 32 }
yruiewyrui3 0:2294f5ee0610 33 for(int k=0; k<=2; k++)
yruiewyrui3 0:2294f5ee0610 34 {
yruiewyrui3 0:2294f5ee0610 35 prawa[k]=bufor[k+5];
yruiewyrui3 0:2294f5ee0610 36 }
yruiewyrui3 0:2294f5ee0610 37 sscanf(prawa, "%3d", &p);
yruiewyrui3 0:2294f5ee0610 38 sscanf(lewa, "%3d", &l);
yruiewyrui3 0:2294f5ee0610 39
yruiewyrui3 0:2294f5ee0610 40 if(abs(l)>100) {
yruiewyrui3 0:2294f5ee0610 41 l=100;
yruiewyrui3 0:2294f5ee0610 42 }
yruiewyrui3 0:2294f5ee0610 43 if(abs(p)>100) {
yruiewyrui3 0:2294f5ee0610 44 p=100;
yruiewyrui3 0:2294f5ee0610 45 }
yruiewyrui3 0:2294f5ee0610 46
yruiewyrui3 0:2294f5ee0610 47 stm.printf("liczba lewa: %d \n", l);
yruiewyrui3 0:2294f5ee0610 48 stm.printf("liczba prawa: %d \n", p);
yruiewyrui3 0:2294f5ee0610 49
yruiewyrui3 0:2294f5ee0610 50 if(bufor[0]=='-') l=-l;
yruiewyrui3 0:2294f5ee0610 51 if(bufor[4]=='-') p=-p;
yruiewyrui3 0:2294f5ee0610 52
yruiewyrui3 0:2294f5ee0610 53 eng_left.move(l);
yruiewyrui3 0:2294f5ee0610 54 eng_right.move(p);
yruiewyrui3 0:2294f5ee0610 55
yruiewyrui3 0:2294f5ee0610 56 if ((l==0) && (p==0)) {
yruiewyrui3 0:2294f5ee0610 57 stm.printf("Pale gume w miejscu bo kazali mi stac");
yruiewyrui3 0:2294f5ee0610 58 }
yruiewyrui3 0:2294f5ee0610 59 else if ((l>0)&& (p>0)){
yruiewyrui3 0:2294f5ee0610 60 stm.printf("Jade do przodu. Predkosc lewej strony to %d, a prawej strony to %d \n", l, p);
yruiewyrui3 0:2294f5ee0610 61 }
yruiewyrui3 0:2294f5ee0610 62 else if ((l<0)&& (p<0)){
yruiewyrui3 0:2294f5ee0610 63 stm.printf("Jade do tylu. Predkosc lewej strony to %d, a prawej strony to %d \n", l, p);
yruiewyrui3 0:2294f5ee0610 64 }
yruiewyrui3 0:2294f5ee0610 65 else if ((l>=0)&& (p<=0)){
yruiewyrui3 0:2294f5ee0610 66 stm.printf("Obracam sie w prawo. Predkosc lewej strony to %d, a prawej strony to %d \n", l, p);
yruiewyrui3 0:2294f5ee0610 67 }
yruiewyrui3 0:2294f5ee0610 68 else if ((l<=0)&& (p>=0)){
yruiewyrui3 0:2294f5ee0610 69 stm.printf("Obracam sie w lewo. Predkosc lewej strony to %d, a prawej strony to %d \n", l, p);
yruiewyrui3 0:2294f5ee0610 70 }
yruiewyrui3 0:2294f5ee0610 71
yruiewyrui3 0:2294f5ee0610 72 i=0;
yruiewyrui3 0:2294f5ee0610 73 for (int j=0; j<8; j++)
yruiewyrui3 0:2294f5ee0610 74 {
yruiewyrui3 0:2294f5ee0610 75 bufor[j]=NULL;
yruiewyrui3 0:2294f5ee0610 76 }
yruiewyrui3 0:2294f5ee0610 77 }
yruiewyrui3 0:2294f5ee0610 78 }