Revision 5:92151b9067e4, committed 2016-01-19

Comitter:

jcabello7

Date:

Tue Jan 19 14:43:51 2016 +0000

Parent:

4:de2112ca0735

Commit message:

- No deixa avan?ar al robot si hi ha un obstacle en la direcci? d'avan?ament.; - Canviat el port s?rie per un que si que funciona.

Changed in this revision

--- a/Hc05.cpp	Wed Dec 23 15:23:01 2015 +0000
+++ b/Hc05.cpp	Tue Jan 19 14:43:51 2016 +0000
@@ -2,8 +2,9 @@
 #include "mbed.h"
 #include "math.h"
 
-#define pi 3.14159265
-#define longString 128
+#define pi 3.14159265 //Valor de pi
+#define longString 128 //Longitud màxima dels strings
+#define minUltrasons 200 //Valor mínim dels ultrasons
 
 
         Hc05::Hc05(PinName tx, PinName rx) : RawSerial(tx, rx){
@@ -119,13 +120,16 @@
         };
         
         //Calculs motors
-        void Hc05::calculaMotors(){
-            if(mode==1){ //Falta afegir una condició perquè no deixi avançar en determinada direcció si detecta els ultrasons if((mode==1) && ((ultraso1 > 10) && (ang<-45) && (ang>45)) && ((ultraso2 > 10) && (ang<90) && (ang>180)) && ((ultraso3 > 10) && (ang<-180) && (ang>-90))))
-                x = cos(ang*pi/180);
-                y = sin(ang*pi/180);
-                m1 = (-y)*vel;
-                m3 = (x*0.8660254038+y*0.5)*vel;
-                m2 = (x*(-0.8660254038)+y*0.5)*vel;
+        void Hc05::calculaMotors(float S1, float S2, float S3){
+            if(mode==1) { // Comprova que no hi hagin obstacles en la direcció en que volem avançar
+                if ( (!((ang>=-45) && (ang<=45) && (S1<=minUltrasons))) && (!((ang>=90) && (ang<=180) && (S2<=minUltrasons))) && (!((ang>=-90) && (ang<=-180) && (S3<=minUltrasons))) ) {
+                    x = cos(ang*pi/180);
+                    y = sin(ang*pi/180);
+                    m1 = (-y)*vel;
+                    m3 = (x*0.8660254038+y*0.5)*vel;
+                    m2 = (x*(-0.8660254038)+y*0.5)*vel;
+                }
+                else {m1 = m2 = m3 = x = y = 0;}
             }
             else if ((mode==2) && (ang!=0)){
                 x = y = 0;

--- a/Hc05.h	Wed Dec 23 15:23:01 2015 +0000
+++ b/Hc05.h	Tue Jan 19 14:43:51 2016 +0000
@@ -30,7 +30,7 @@
         void comprovaConnexio();
         int getTimer();
         //Calculs
-        void calculaMotors();
+        void calculaMotors(float S1, float S2, float S3);
         float getM1();
         float getM2();
         float getM3();