From 583f901a767b62226a41f4c99a9137a250fdc876 Mon Sep 17 00:00:00 2001
From: Miguel Delgado <MdelgadoL>
Date: Wed, 24 Nov 2021 12:52:36 -0600
Subject: [PATCH] Update SeguidorLineaArduino.ino

---
 SeguidorLineaArduino/SeguidorLineaArduino.ino | 18 ++++++++++++++++--
 1 file changed, 16 insertions(+), 2 deletions(-)

diff --git a/SeguidorLineaArduino/SeguidorLineaArduino.ino b/SeguidorLineaArduino/SeguidorLineaArduino.ino
index 5fc6afa..8e93fc8 100644
--- a/SeguidorLineaArduino/SeguidorLineaArduino.ino
+++ b/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); 
   
 }