Update SeguidorLineaArduino.ino

Ultimas actualizaciones

SeguidorLineaArduino/SeguidorLineaArduino.ino

@@ -25,20 +25,24 @@ int ir =2;//encender y apagar la barra
 #define BIN2  4
 #define PWMB  3
 
+#define LED_DER 12
+#define LED_IZQ 2
+
+
+//------------------------------------------------------------
+//----------------------------PID-----------------------------
+//------------------------------------------------------------
+float kp=.5;
+float ki=0;
+float kd=0;
+int velocidad=100;
+
+
 
 //------------------------------------------------------------
 //----------------------INICIALIZACION------------------------
 //------------------------------------------------------------
 void setup() {
-  Serial.begin(115200);
-  //Inicializamos los pines de los sensores
-  for(int x=0;x<numSensores;x++){
-    pinMode(sensores[x],INPUT);
-  }
-  pinMode(LED_BUILTIN,OUTPUT);
-  //pin para controlar el encendido de la barra
-  pinMode(ir,OUTPUT);
-
   //pines para controlar motores
   pinMode(PWMA,OUTPUT);
   pinMode(PWMB,OUTPUT);
@@ -49,8 +53,22 @@ void setup() {
   //establecer direccion de los motores
   digitalWrite(AIN1,HIGH);
   digitalWrite(AIN2,LOW);
-  digitalWrite(BIN1,HIGH);
-  digitalWrite(BIN2,LOW);
+  digitalWrite(BIN1,LOW);
+  digitalWrite(BIN2,HIGH);
+  
+  Serial.begin(115200);
+  //Inicializamos los pines de los sensores
+  for(int x=0;x<numSensores;x++){
+    pinMode(sensores[x],INPUT);
+  }
+  
+  pinMode(LED_BUILTIN,OUTPUT);
+  pinMode(LED_DER,OUTPUT);
+  pinMode(LED_IZQ,OUTPUT);
+  //pin para controlar el encendido de la barra
+  pinMode(ir,OUTPUT);
+
+  
   
   digitalWrite(ir,HIGH);//Encendemos la barra
   calibracion();
@@ -62,21 +80,48 @@ void setup() {
 //------------------------------------------------------------
 void loop() {
    int pos=getPosicion();
-
+/*
     //PINTAR LOS VALORES
   for(int x=0;x<numSensores;x++){  
     Serial.print(valores[x]);
     Serial.print("\t");
   }
   Serial.println(pos);
-
-  acelerar(50,50);
-  delay(100);
+*/
+  int error=map(pos,500,4000,-255,255);
+  Serial.print(pos);
+  Serial.print("->");
+  Serial.println(error);
+  luces(false,false);
+  float pid=kp*error;
+  acelerar(70+pid,70-pid);
+  
 }
 
 void acelerar(int motIzq, int motDer){
-  analogWrite(PWMA,motIzq);
-  analogWrite(PWMA,motDer);
+    if(motIzq<0){
+      analogWrite(PWMB,0);
+    }else{
+      if(motIzq>255){
+        analogWrite(PWMB,255);
+      }else{
+        analogWrite(PWMB,motIzq);
+      }
+    }
+    if(motDer<0){
+      analogWrite(PWMA,0);
+    }else{
+      if(motDer>255){
+        analogWrite(PWMA,255);
+      }else{
+        analogWrite(PWMA,motDer);
+      }
+    }
+}
+
+void luces(boolean izq,boolean der){
+  digitalWrite(LED_IZQ,izq);
+  digitalWrite(LED_DER,der);
 }
 
 
@@ -127,7 +172,7 @@ int getPosicion(){
 void leerDigital(){
    //LEER LOS VALORES DE LOS SENSORES
     for(int x=0;x<numSensores;x++){
-      valores[x]=analogRead(sensores[x])>(umbral[x]+70)?(x*500)+500:0;
+      valores[x]=analogRead(sensores[x])>(umbral[x])?(x*500)+500:0;
     }
 }
 
@@ -141,16 +186,24 @@ void leerDigital(){
  * TODO:reacomodar esta función
  */
 void calibracion(){
+  int min[]={1023,1023,1023,1023,1023,1023,1023,1023};
+  int max[]={0,0,0,0,0,0,0,0};
+  
   digitalWrite(LED_BUILTIN,HIGH);
   for(int y=0;y<400;y++){
   for(int x=0;x<numSensores;x++){
     int v=analogRead(sensores[x]);
-    if(umbral[x]>v){
-      umbral[x]=v;
+    if(min[x]>v){
+      min[x]=v;
+    }
+    if(max[x]<v){
+      max[x]=v;
     }
   }
   delay(10);
   }
-  
+  for(int x=0;x<numSensores;x++){
+    umbral[x]=(min[x]+max[x])/2;
+  }
   digitalWrite(LED_BUILTIN,LOW);
 }