Diff: frenar.cpp

Revision:

0:bd4170087cf1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/frenar.cpp	Mon May 04 08:39:07 2015 +0000
@@ -0,0 +1,93 @@
+#include "mbed.h"
+#include "main.h"
+
+Serial pc2(USBTX, USBRX);
+int sentido= 0;
+int fase = 0;
+bool iniFreno = false;
+
+void frenar(double velocidadActual, double& potenciaMotores, double& potenciaMotoresFrameAnterior, PwmOut pinDAd, PwmOut pinDAt, PwmOut pinIAd, PwmOut pinIAt){
+  double sumador;
+  
+  //Primero, comprobamos si nos estamos moviendo
+ 
+  //  Serial.println(velocidadActual);
+//  Serial.print("Sentido");
+ // Serial.println(sentido);
+  if(velocidadActual>0.2){
+  double velocidadAnterior;
+      if(nominado == 0){
+        velocidadAnterior = historialVelocidades[2];
+      } else if(nominado == 1){
+        velocidadAnterior = historialVelocidades[1];
+       }else {
+        velocidadAnterior = historialVelocidades[0];
+       }
+
+       
+       if(sentido == 0){
+         potenciaMotores = -0.6f;
+         sentido = 1;
+         }
+         else if (sentido == 1){
+           if(velocidadActual <= (velocidadAnterior+0.3)){
+             potenciaMotores = potenciaMotores-0.05;
+             }else{
+               sentido = -1;
+               potenciaMotores = -potenciaMotores/3;
+               }
+           }
+           else{
+             if(velocidadActual <= (velocidadAnterior+0.3)){
+             potenciaMotores = potenciaMotores+0.05;
+             }else{
+               sentido = 1;
+               potenciaMotores = -potenciaMotores/3;
+               }
+             }
+         
+      if(potenciaMotores > 1.0){
+        potenciaMotores = 1.0;
+        }
+      if(potenciaMotores < -1.0){
+        potenciaMotores = -1.0;
+        }
+         
+         
+  //     Serial.print("POTENCIA " );
+  //     Serial.println(potenciaMotores);
+      
+
+    }else{
+      potenciaMotores=0;
+  //    Serial.println("ESTÁ FRENADO");
+      sentido = 0;
+      fase = 0;
+      }
+           if(potenciaMotores<0){
+        
+               pinDAt.write(-potenciaMotores);
+               pinDAd.write(0.0);
+               pinIAt.write(-potenciaMotores);
+               pinIAd.write(0.0);
+       /* analogWrite(pinRuedaDerechaAtras, -potenciaMotores);
+        analogWrite(pinRuedaDerechaAdelante, 0);
+        analogWrite(pinRuedaIzquierdaAtras, -potenciaMotores);
+        analogWrite(pinRuedaIzquierdaAdelante, 0);*/
+        
+      }else{
+        /*analogWrite(pinRuedaDerechaAdelante, potenciaMotores);
+        analogWrite(pinRuedaDerechaAtras, 0);
+        analogWrite(pinRuedaIzquierdaAdelante, potenciaMotores);
+        analogWrite(pinRuedaIzquierdaAtras, 0);*/
+        
+               pinDAd.write(potenciaMotores);
+               pinDAt.write(0.0);
+               pinIAd.write(potenciaMotores);
+               pinIAt.write(0.0);
+        
+      }
+    //  tiempoUltimoControlFrenado = millis();
+  
+  }
+