Diff: main.cpp

Revision:

3:d78e7f6477d8

Parent:

2:325a730cd438

--- a/main.cpp	Wed Jun 15 12:33:51 2016 +0000
+++ b/main.cpp	Fri Jun 17 08:58:05 2016 +0000
@@ -1,6 +1,7 @@
 #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);
@@ -10,6 +11,7 @@
 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];
@@ -20,7 +22,8 @@
 int p=0;
 int po=0;
 int pi=0;
-Timer t;
+Timer t, t_sonar;
+long distance=0;
  
 //*************** Funkcja czyszczaca bufor *****************
 void cleanBuffer(char *buffor)
@@ -76,8 +79,8 @@
     //stm.printf("pakiet poprawny\n");]
     return true;
 }
- 
-void move_wheels()
+
+void calculate_velocity()
 {
         //*******PRZYPISANIE DO TABLICY LEWEJ LICZBY*********//
             for(int j=0; j<=2; j++){
@@ -104,10 +107,15 @@
         //******KOREKCJA ZNAKU***********//
             if(bufor[1]=='-') l=-l;
             if(bufor[5]=='-') p=-p;
-        //**********ODPALANIE SILNIKOW******// 
+}
+
+ 
+void move_wheels()
+{
             eng_left.move(l);
             eng_right.move(p);
 }
+
 void move_camera()
 {
         //*******PRZYPISANIE DO KAMERY POZIOM*********//   
@@ -143,11 +151,21 @@
     int i = 0;
     cleanBuffer(bufor);
     stm.baud(115200);
+    t_sonar.start(); 
     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){
@@ -167,8 +185,20 @@
             t.reset();
             t.start();                    
         }
-        if(bufor[9]=='$'){
-        move_wheels();
+        if(bufor[9]=='$'){            
+            calculate_velocity();
+            if(distance>25){ 
+                move_wheels();
+                }                
+            else if (distance<=25){
+                if(l<=0 && p<=0 || l*p<=0){                
+                    move_wheels();
+                }
+                else{
+                    eng_left.move(0);
+                    eng_right.move(0);
+                }
+            }
         }
         else if(bufor[9]=='&'){
         move_camera();