Revision 1:444af82e35cb, committed 2020-02-22

Comitter:

bispomat

Date:

Sat Feb 22 18:41:39 2020 +0000

Parent:

0:b803244146f7

Commit message:

Codigo completo do Papacuras com leitura analogica por tempo

Changed in this revision

--- a/main.cpp	Wed Oct 16 22:35:17 2019 +0000
+++ b/main.cpp	Sat Feb 22 18:41:39 2020 +0000
@@ -4,6 +4,7 @@
 
 //========================= DEFINICOES =========================//
 
+#define threshold 10600 // parametro que diferencia a leitura do branco pro preto
 PwmOut pwm(p25);
 DigitalOut dir (p26);
 PwmOut pwm1(p24);
@@ -12,6 +13,7 @@
 //DigitalOut led(p20);
 
 Serial bt(p28, p27);
+Serial pc(USBTX, USBRX);
 
 //InterruptIn A (p21);
 //DigitalIn B (p22);
@@ -21,14 +23,19 @@
 //Ticker END;
 //Ticker END1;
 
-DigitalIn s1(p5);  
-DigitalIn s2(p6);
-DigitalIn s3(p7);
-DigitalIn s4(p8);
-DigitalIn s5(p9);
-DigitalIn s6(p10);
-DigitalIn s7(p12);
-DigitalIn s8(p13);
+DigitalOut multiplexador1(p5); // portas do multiplexador que selecionam o sensor de linha
+DigitalOut multiplexador2(p6);
+DigitalOut multiplexador3(p8); // PRECISA AJUSTAR OS PINOS
+DigitalOut multiplexador4(p7);
+//DigitalOut multiplexador5(p9);// Enable do multiplexador
+DigitalInOut multiplexadorInOut(p12); // In/Out do multiplexador
+
+int tabelaVerdade1[16] = {0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1}; // Arrays para selecionar o sensor de linha na função sensorCheck
+int tabelaVerdade2[16] = {0,0,1,1,0,0,1,1,0,0,1,1,0,0,1,1};
+int tabelaVerdade3[16] = {0,0,0,0,1,1,1,1,0,0,0,0,1,1,1,1};
+int tabelaVerdade4[16] = {0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1};
+
+Timer tempoDoSensor; // timer para leitura do sensor de linha
 
 //#define Kp_Roda 1
 //#define Kd_Roda 1
@@ -40,7 +47,7 @@
 float ParametroCorrecao = 5.3;
 float Kp_Sensor = 0.145; //0.14;   //0.2
 float Kd_Sensor = 1.64; //1.75;   //1.15
-float BaseSpeed = 0.17;
+float BaseSpeed = 0.12;
 
 //==========================================================//
 //#define VMAX_PWM 100
@@ -88,30 +95,66 @@
 
 //------------------ FUNCAO -----------------------
 
-void Setup_Sensores (void){ //Define o modo de cada sensor como PullUp
-    s1.mode(PullUp); 
-    s2.mode(PullUp);
-    s3.mode(PullUp);
-    s4.mode(PullUp);
-    s5.mode(PullUp);
-    s6.mode(PullUp);
-    s7.mode(PullUp);
-    s8.mode(PullUp);
+void Setup_Sensores (void){ //Define o modo de cada sensor
+    multiplexadorInOut.mode(PullNone);
 }
 
+bool sensorCheck(int sensor){// int sensor é qual sensor se deseja ler,entre 0 a 15
+    // função para ler os motores,retorna true se for branco e false se for preto
+   
+    multiplexador1 = tabelaVerdade1[sensor]; // selecionando o sensor
+    multiplexador2 = tabelaVerdade2[sensor];
+    multiplexador3 = tabelaVerdade3[sensor];
+    multiplexador4 = tabelaVerdade4[sensor];
+    //pc.printf("Status do Sensor entrando no check: %d ", multiplexadorInOut.read());
+    multiplexadorInOut.output(); // colocando o InOut como Out
+    multiplexadorInOut = 1; // levando ele a high por pelo menos 10 microssegundos
+    
+    tempoDoSensor.start(); // iniciando o contador
+    
+    wait_ms(10); // tempo para o output ir para high
+
+    multiplexadorInOut.input(); // colocando o InOut como In
+    //pc.printf("Status do Sensor supostamente em HIGH: %d ", multiplexadorInOut.read());
+    
+    while(multiplexadorInOut == 1){// enrolando enquanto o sensor esta lendo
+    //pc.printf("To no loop ->");
+    //pc.printf("Status do Sensor: %d ", multiplexadorInOut.read());
+        
+        if (tempoDoSensor.read_us() >= threshold){// termina o while se o tempo for maior que o threshold,agilizando o processo de leitura      
+            tempoDoSensor.stop();
+            //pc.printf("To no if ->");
+            float tempoDeLeitura = tempoDoSensor.read_us();
+            //pc.printf ("Tempo de leitura :%f \n\r ",tempoDeLeitura); // debug
+            tempoDoSensor.reset();
+            multiplexadorInOut.output();
+            multiplexadorInOut = 0;
+            return false; // retorna quando for preto
+            }
+        
+    }
+    tempoDoSensor.stop();
+    float tempoDeLeitura = tempoDoSensor.read_us();
+    //pc.printf ("Tempo de leitura :%f \n\r ",tempoDeLeitura); // debug
+    tempoDoSensor.reset();
+    multiplexadorInOut.output();
+    multiplexadorInOut = 0;
+    return true;// retorna quando for branco
+    }
+
 void Leitura_Sensores (void){ //Faz a leitura dos sensores e retorna a posição
     
-    if (s4.read()==0 && s5.read()==0) {Position = 45;} //Se ler os dois do meio, está no SetPoint
-    else if (s7.read()==0) {Position = 70;}    //Ele só vai ler os sensores das extremidades
-    else if (s8.read()==0) {Position = 80;}    //se tiver certeza que não está no meio (ajuda o LF a passar pelos cruzamentos)
-    if (s1.read()==0) {Position = 10;}
-    if (s2.read()==0) {Position = 20;}
-    if (s3.read()==0) {Position = 30;}
-    if (s4.read()==0) {Position = 40;}
-    if (s5.read()==0) {Position = 50;}
-    if (s6.read()==0) {Position = 60;}
+    if (sensorCheck(3)== 1 && sensorCheck(4) == 1) {Position = 45;} //Se ler os dois do meio, está no SetPoint
+    else if (sensorCheck(0) == 1) {Position = 10;}    //Ele só vai ler os sensores das extremidades
+    else if (sensorCheck(7) == 1) {Position = 80;}    //se tiver certeza que não está no meio (ajuda o LF a passar pelos cruzamentos)
+    if (sensorCheck(1)==1) {Position = 20;}
+    if (sensorCheck(2)==1) {Position = 30;}
+    if (sensorCheck(3)==1) {Position = 40;}
+    if (sensorCheck(4)==1) {Position = 50;}
+    if (sensorCheck(5)==1) {Position = 60;}
+    if (sensorCheck(6)==1) {Position = 70;}
     
-    //bt.printf("\n\rPosicao: %d", Position); //Print de teste
+    //pc.printf("\n\rPosicao: %d", Position); //Print de teste
 }
 
 /*=================================================
@@ -267,7 +310,7 @@
     pwm.write(0.00); //Pulso inicial para cada motor
     pwm1.write(0.00);
     
-    END.attach(&Final_Stop, 20); //Faz o LF parar depois de 20 segundos
+    //END.attach(&Final_Stop, 20); //Faz o LF parar depois de 20 segundos
     
     while(1){
         //Para_Demonio();