nowe + funkcje
Revision 0:2e73d45d090e, committed 2016-06-14
- Comitter:
- yruiewyrui3
- Date:
- Tue Jun 14 12:32:39 2016 +0000
- Commit message:
- nowe + funkcje
Changed in this revision
diff -r 000000000000 -r 2e73d45d090e Engine.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Engine.cpp Tue Jun 14 12:32:39 2016 +0000 @@ -0,0 +1,44 @@ +#include "Engine.h" + +// przelicza procenty na pwn: +// - wartosc poczatkowa - 0.2377, +// - prog - 0.0077 +float Engine::getPwm(float speed){ + if (speed == 0) + return 0; + else if (speed > 0) + return 0.2377f + (fabs(speed) - 1) * 0.0077f; + else + return 0.406f + (fabs(speed) - 1) * 0.006f; + } + +void Engine::move(int speed){ + if (speed > 100) + speed =100; + else if (speed < -100) + speed = -100; + + if (speed == 0){ + //soft stop + _fwd = 0; + _rev = 0; + return; + } + + if (speed < 0){ + _fwd = 0; + _rev = 1; + } + + else{ + _fwd = 1; + _rev = 0; + } + + _pwm = getPwm(speed); +} + +//hard stop +void Engine::stop(){ + _pwm = 0; +} \ No newline at end of file
diff -r 000000000000 -r 2e73d45d090e Engine.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Engine.h Tue Jun 14 12:32:39 2016 +0000 @@ -0,0 +1,22 @@ +#ifndef ENGINE_H +#define ENGINE_H + +#include "mbed.h" + + +class Engine{ + private: + PwmOut _pwm; + DigitalOut _fwd; + DigitalOut _rev; + + float getPwm(float val); + + public: + Engine(PinName pwm, PinName fwd, PinName rev): _pwm(pwm), _fwd(fwd), _rev(rev){} + void move(int speed); // silniki pracuja z okreslona predkoscia (speed ma zakres od -1 do 1) + void stop(); //hard stop +}; + + +#endif \ No newline at end of file
diff -r 000000000000 -r 2e73d45d090e Servo.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Tue Jun 14 12:32:39 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/Servo/#36b69a7ced07
diff -r 000000000000 -r 2e73d45d090e main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Jun 14 12:32:39 2016 +0000 @@ -0,0 +1,193 @@ +#include "mbed.h" +#include "Engine.h" +#include "Servo.h" + +//*************** Deklaracja wszystkich portów potrzebnych do obsługi************** +Serial stm(PA_2, PA_3); +Servo cam_poziom(PB_6); +Servo cam_pion(PC_7); +Engine eng_left = Engine(PB_13, PB_4, PB_10); +Engine eng_right = Engine(PB_14, PB_5, PB_3); + +//*************** Deklaracja zmiennych globalnych, tablic, bufora na ramke znaków ************** +const int BufferSize=10; +char bufor[BufferSize]; +char prawa[3]; +char lewa[3]; +char poziom[2]; +char pion[2]; +int l=0; +int p=0; +int po=0; +int pi=0; +Timer t; + +//*************** Funkcja czyszczaca bufor ***************** +void cleanBuffer(char *buffor) + { + for(int i=0; i<BufferSize; i++) + buffor[i]=NULL; + buffor[BufferSize]=NULL; +} + +//***************OPIS FUNKCJI isCorrectPacket******************************************************// +// funkcja sprawdza czy ramka spelnia wymagania protokolu: @ZCCCZCCC$ lub &ZCCCZCCC$ gdzie Z={+/-} C={0,1,...9} // +//*******************KONIEC OPISU******************************************************************// + +bool isCorrectPacket(char *buffor){ + if ((bufor[0]!='@') && (bufor[0]!='&')) + { + // stm.printf("ZLY ZNAK POCZATKU\n"); + stm.printf("blad\n"); + return false; + } + if((buffor[1] != '+') && (buffor[1] != '-')) + { + // stm.printf("ZLY ZNAK +/- LEWY\n"); + stm.printf("blad\n"); + return false; + } + if((buffor[5] != '+') && (buffor[5] != '-')) + { + // stm.printf("ZLY ZNAK +/- PRAWY\n"); + stm.printf("blad\n"); + return false; + } + if(bufor[9]!='$') + { + // stm.printf("ZLY ZNAK KONCA\n"); + stm.printf("blad\n"); + return false; + } + for(int i=2; i< 5; i++){ + if(((int)buffor[i]) < 48 || ((int)buffor[i]) > 57) + { + // stm.printf("NA LEWE KOLO: NIE LICZBA\n"); + stm.printf("blad\n"); + return false; + } + if(((int)buffor[i+4]) < 48 || ((int)buffor[i+4]) > 57) + { + // stm.printf("NA PRAWE KOLO: NIE LICZBA\n"); + stm.printf("blad\n"); + return false; + } + } + //stm.printf("pakiet poprawny\n");] + return true; +} + +void move_wheels() +{ + //*******PRZYPISANIE DO TABLICY LEWEJ LICZBY*********// + for(int j=0; j<=2; j++){ + lewa[j]=bufor[j+2]; + } + //*******PRZYPISANIE DO TABLICY PRAWEJ LICZBY*********// + for(int k=0; k<=2; k++){ + prawa[k]=bufor[k+6]; + } + //*******KONWERSJA CHAROW NA INTY*********// + sscanf(lewa, "%3d", &l); + sscanf(prawa, "%3d", &p); + //********KOREKCJA***************// + if(abs(l)>100){ + l=100; + } + if(abs(p)>100){ + p=100; + } + //************WYPISYWANIE*******************// + for(int j=0; j<=BufferSize; j++){ + stm.putc(bufor[j]); + } + //******KOREKCJA ZNAKU***********// + if(bufor[1]=='-') l=-l; + if(bufor[5]=='-') p=-p; + //**********ODPALANIE SILNIKOW******// + eng_left.move(l); + eng_right.move(p); + + stm.printf(bufor); +} +void move_camera() +{ + //*******PRZYPISANIE DO KAMERY POZIOM*********// + for(int m=0; m<2; m++){ + poziom[m]=bufor[m+3]; + } + //*******PRZYPISANIE DO KAMERY PION*********// + for(int n=0; n<2; n++){ + pion[n]=bufor[n+3]; + } + //*******KONWERSJA CHAROW NA INTY*********// + sscanf(poziom, "%2d", &po); + sscanf(pion, "%2d", &pi); + //********KOREKCJA***************// + if(abs(po)>90){ + po=90; + } + if(abs(pi)>90){ + pi=90; + } + //************WYPISYWANIE*******************// + for(int j=0; j<=BufferSize; j++){ + stm.putc(bufor[j]); + } + if(bufor[1]=='-') po=-po; + if(bufor[5]=='-') pi=-pi; + //**********RUCH KAMERĄ******// + cam_poziom.position(po); + cam_pion.position(pi); +} +void fill_buffer() +{ + bufor[i] = stm.getc(); + if(i==0){ + if((bufor[i]=='@') || (bufor[i]=='&')) //zaczynamy zapelniac bufor jak dostaniemy @ + ++i; + } + else if(bufor[i]=='@'){ // i != 0 //interpretujemy jakby potencjalny poczatek ramki + cleanBuffer(bufor); + //stm.printf("blad\n"); + bufor[0]='@'; + i=1; //bo zerowy znak '@' juz zczytal + } + else if (bufor[i]=='&'){ // i != 0 //interpretujemy jakby potencjalny poczatek ramki + cleanBuffer(bufor); + //stm.printf("blad\n"); + bufor[0]='&'; + i=1; //bo zerowy znak '@' juz zczytal + } + else + i++; +} + +int main(){ + int i = 0; + stm.baud(115200); + while(true){ + if(t.read_ms()>200){ //jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota + eng_left.move(0); + eng_right.move(0); + } + //if((bufor[9] == NULL)&&(stm.readable())) + if(bufor[9] == NULL){ + fill_buffer(); + continue; + } + if(isCorrectPacket(bufor)){ + t.stop(); + t.reset(); + t.start(); + } + if(bufor[0]=='@'){ + move_wheels(); + } + else if(bufor[0]=='&'){ + move_camera(); + } + i=0; + cleanBuffer(bufor); + } +} \ No newline at end of file