Update ProfessionalLineFollower.ino

updated

ProfessionalLineFollower/ProfessionalLineFollower.ino

@@ -4,6 +4,7 @@
 #include <Adafruit_GFX.h>
 #include <Adafruit_SSD1306.h>
 
+
 //PB11 SDA2
 //PB11 SCL2
 #define SCREEN_WIDTH 128 // OLED display width, in pixels
@@ -13,7 +14,7 @@
 TwoWire WIRE2 (2,I2C_FAST_MODE);
 #define Wire WIRE2
 Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
-String valor;
+String cad;
 
 #define A0 PA0
 #define A1 PA1    //POT 1 DER
@@ -26,6 +27,7 @@ String valor;
 #define A8 PB1    //POT 1 IZQ
 #define A9 PB0    //POT 2 IZQ
 
+
 #define button PB12
 
 #define ledGreenLeft    PA8
@@ -35,10 +37,10 @@ String valor;
 #define ledRedRight     PB6
 #define ledBlueRight    PB7
 
-int pots[]={A1,A2,A8,A9};     //Pines de los potenciometros
-int maxPot[]={100,100,100,100};  //Valores maximos a mapear los potenciometros
-int potNumber=1;              //cantidad de potenciometros
 
+int valores[16];      //Arreglo para obtener los datos de los sensores
+int numSensores=16;
+int umbral[16];        //Arreglo con el umbral de cada sensor
 
 boolean start=false;
 
@@ -71,49 +73,118 @@ void setup() {
   pinMode(ledBlueRight,OUTPUT);
   pinMode(ledRedRight,OUTPUT);
   pinMode(ledGreenRight,OUTPUT);
-
+  while(!Serial){}
+calibracion();
 }
 
 void loop() {
-  valor="";
-  for(int x=0;x<16;x++){
+/*
+  leerRaw();
+  for(int x=0;x<numSensores;x++){
+    Serial.print("(");
+    Serial.print(umbral[x]);
+    Serial.print(")");
+    Serial.print(valores[x]);
+    Serial.print("\t");
+  }
+  Serial.println();
+*/
+  int pos=leerDigital();
+  String cad="";
+  for(int x=0;x<numSensores;x++){
+    Serial.print(valores[x]);
+    Serial.print("  ");
+    cad+=valores[x]!=0?"0":"_";
+  }
+  Serial.print("--->  ");
+  pos=map(pos,0,1600, -255,255);
+  Serial.println(pos);
+
+
+  pantalla(cad);
+  delay(100);
+
+}
+
+void getPosicion(){
+  leerDigital();
+  for(int x=0;x<numSensores;x++){
+    valores[x]=valores[x]*(100*x);
+  }
+}
+
+void leerRaw(){
+  for(int x=0;x<numSensores;x++){
     digitalWrite(A3,bitRead(x,1));
     digitalWrite(A4,bitRead(x,0));
     digitalWrite(A6,bitRead(x,2));
     digitalWrite(A7,bitRead(x,3));
-    int v=analogRead(A5);
-    delay(1);
-    if(v>4000){
-      valor+="0";
+    valores[x]=analogRead(A5);
+  }
+}
+
+int leerDigital(){
+  int suma=0;
+  int contador=0;
+  for(int x=0;x<numSensores;x++){
+    digitalWrite(A3,bitRead(x,1));
+    digitalWrite(A4,bitRead(x,0));
+    digitalWrite(A6,bitRead(x,2));
+    digitalWrite(A7,bitRead(x,3));
+    if(analogRead(A5)>umbral[x]){
+     valores[x]=(x+1)*100;
+     suma+=valores[x];
+     contador++;
     }else{
-      valor+="-";
+      valores[x]=0;
+    }
+  }
+  return suma/contador;
+}
+
+
+void calibracion(){
+  int min[]={5000,5000,5000,5000,5000,5000,5000,5000,5000,5000,5000,5000,5000,5000,5000,5000};
+  int max[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
+  digitalWrite(ledGreenLeft,HIGH);
+  digitalWrite(ledGreenRight,HIGH);
+  for(int y=0;y<800;y++){
+    for(int x=0;x<numSensores;x++){
+      digitalWrite(A3,bitRead(x,1));
+      digitalWrite(A4,bitRead(x,0));
+      digitalWrite(A6,bitRead(x,2));
+      digitalWrite(A7,bitRead(x,3));
+      int v=analogRead(A5);
+      if(min[x]>v){
+        min[x]=v;
+      }
+      if(max[x]<v){
+        max[x]=v;
+      }
     }
-   // Serial.print(v);
-   // Serial.print("\t");
+    delay(10);
   }
-  Serial.println(valor);
-  pantalla(valor);
+  for(int x=0;x<numSensores;x++){
+    umbral[x]=(min[x]+max[x])/2;
+  }
+  digitalWrite(ledGreenLeft,LOW);
+  digitalWrite(ledGreenRight,LOW);
+}
 
 
+
+void debug(){
+  
 }
 
 void pantalla(String valor){
     display.clearDisplay();
     display.setCursor(0, 0);
-    for(int x=0;x<potNumber;x++){
-      display.setCursor(0, 0);
-      display.print("POT");
-      display.print(x);
-      display.print(" = ");
-      int p1=analogRead(pots[x]);
-      p1=map(p1,0,5000,1,maxPot[x]);
-      display.println(p1);
-    }
-  /*  display.print("POT1 = ");
+    display.print("POT1 = ");
     int p1=analogRead(A1);
     p1=map(p1,0,5000,1,255);
     display.println(p1);
-    */display.setCursor(60, 0);
+    display.setCursor(60, 0);
     display.print("POT2 = ");
     int p2=analogRead(A2);
     p2=map(p2,0,5000,1,255);