Update SeguidorLineaArduino.ino

SeguidorLineaArduino/SeguidorLineaArduino.ino

@@ -36,6 +36,9 @@ float kp=.5;
 float ki=0;
 float kd=0;
 int velocidad=100;
+int ultimaPosicion=0;
+int errorAnterior=0;
+int P,I,D;
 
 
 
@@ -80,6 +83,12 @@ void setup() {
 //------------------------------------------------------------
 void loop() {
    int pos=getPosicion();
+   int pos=getPosicion();
+   if(pos!=0){
+      ultimaPosicion=pos;
+   }else{
+    pos=ultimaPosicion;
+   }
 /*
     //PINTAR LOS VALORES
   for(int x=0;x<numSensores;x++){  
@@ -102,8 +111,13 @@ void loop() {
 * TODO: se debe agregar el código para incluir los valores de
 *  D e I, pues actualmante solo tutiliza control P
 */
-  float pid=kp*error;
-  acelerar(70+pid,70-pid);
+  P=error;
+  D=error-errorAnterior;
+  I=I+error;
+  errorAnterior=error;
+  float pid=(kp*P)+(ki*I)+(kd*D);
+  
+  acelerar(velocidad-pid,velocidad+pid); 
   
 }