Revision 0:5919ea7b3b90, committed 2016-06-21

Comitter:

yruiewyrui3

Date:

Tue Jun 21 07:45:32 2016 +0000

Child:

1:b57f208d1d95

Commit message:

.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Engine.cpp	Tue Jun 21 07:45:32 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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Engine.h	Tue Jun 21 07:45:32 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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HCSR04.lib	Tue Jun 21 07:45:32 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/antoniolinux/code/HCSR04/#0b3bdb910fe7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Tue Jun 21 07:45:32 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/Servo/#fa3e4bb7a4cc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Jun 21 07:45:32 2016 +0000
@@ -0,0 +1,189 @@
+#include "mbed.h"
+#include "Engine.h"
+#include "Servo.h"
+#include "hcsr04.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 **************
+HCSR04 sensor(PB_9, PB_8, 11770);
+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, t_sonar;
+float distance;
+ 
+//*************** 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]!='@')
+    {
+       // 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]!='$')&&(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);
+}
+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+7];
+            }
+        //*******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);      
+}
+
+int main(){
+    int i = 0;
+    cleanBuffer(bufor);
+    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(t_sonar.read_ms()>1200){
+            t_sonar.stop();
+            t_sonar.reset();
+            t_sonar.start(); 
+            distance = sensor.distance(); 
+            stm.printf("dystans  %d  \n",distance);  
+            }
+        if(bufor[9] == NULL){
+            bufor[i] = stm.getc();           
+            if(i==0){
+                if(bufor[i]=='@') //zaczynamy zapelniac bufor jak dostaniemy @
+                   ++i;    
+            }
+            else if(bufor[i]=='@'){ // i != 0 //interpretujemy jakby potencjalny poczatek ramki
+                cleanBuffer(bufor);
+                bufor[0]='@';
+                i=1; //bo zerowy znak '@' juz zczytal
+                }
+            else i++;
+        continue;
+        }         
+        if(isCorrectPacket(bufor)){   
+            t.stop();
+            t.reset();
+            t.start();                    
+        }
+        if(bufor[9]=='$'){
+        move_wheels();
+        }
+        else if(bufor[9]=='&'){
+        move_camera();
+        }
+        i=0;
+        cleanBuffer(bufor);
+    }    
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Jun 21 07:45:32 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/6c34061e7c34
\ No newline at end of file