Diff: main.cpp

Revision:

0:8c5a0e6a171b

Child:

2:a68b0082faac

diff -r 000000000000 -r 8c5a0e6a171b main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Sep 17 15:08:31 2020 +0000
@@ -0,0 +1,312 @@
+//========================= BIBLIOTECAS =========================//
+
+#include "mbed.h"
+#include "motors.h"
+
+//========================= DEFINICOES =========================//
+
+#define threshold 250 // parametro que diferencia a leitura do branco pro preto // 250 na ultima tomada de tempo
+
+//DigitalIn button(p18);
+//DigitalOut led(p20);
+
+//InterruptIn A (p21);
+//DigitalIn B (p22);
+//InterruptIn C (p29);
+//DigitalIn D (p30);
+
+//Ticker END;
+//Ticker END1;
+
+DigitalOut multiplexador1(p5); // portas do multiplexador que selecionam o sensor de linha
+DigitalOut multiplexador2(p6);
+DigitalOut multiplexador3(p8);
+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}; // A combinacao dos itens de mesmo indice numerico determina a 
+int tabelaVerdade3[16] = {0,0,0,0,1,1,1,1,0,0,0,0,1,1,1,1}; // porta de saida do multiplexador ,dentro da funcao.
+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
+//#define Ki_Roda 1
+
+//============== VARIAVEIS MAIS IMPORTANTES ================//
+//Separadas do resto para facilitar a alteração
+    
+float ParametroCorrecao = 1.15; //1.02              //1.15  
+float Kp_Sensor = 0.675;        //0.65              //0.675
+float Kd_Sensor = 24.55;        //24.45             //24.55
+float velocidadeBase = 0.18;         //0.18              //0.18
+                                //8.02V na bateria  //7.93Volts
+//float Ki_Sensor = -0.0000005;//testando KI 
+                                                                                                                //
+//==========================================================//
+//#define VMAX_rodaEsquerda 100
+//#define VMAX_Encoder 100
+
+//#define TESTE_MOTOR
+//#define TESTE_SENSOR
+
+Timeout END;//Variavel END que e utilizada para interromper o programa do papacuras
+//========================= VARIAVEIS GLOBAIS =========================
+
+int Position; //De acordo com a leitura dos sensores, sera determinada uma posicao
+int Error=0;
+int Correction; //Variavel que armazena a correcao do PID
+int TotalError = 0;
+int SET_POINT = 45; //Centro dos sensores, nosso setpoint
+//int Integral=0;
+int lastSensor = 4; // Ultima sensor lido
+//float Turbo = velocidadeBase;
+
+//short LimiteFim;
+//short ContaFim = 0;
+//bool  LastRead = false;
+//bool  Preto[15];
+//int Threshold = 600;
+
+int LastError = 0;//Ultimo erro gerado pela leitura dos sensores, utilizado pelo KD.
+//int Integral_Esquerda = 0, Integral_Direita = 0;
+//int MAX_INTEGRAL = 25;
+//int MIN_INTEGRAL = -25;
+float Correcao_Sensor;//Variavel que armazena o resultado do PID
+
+//int Contador_PID_Esquerda = 1, Contador_PID_Direita = 1;
+//int Esquerda_LastError = 0, Direita_LastError = 0;
+float Esquerda_Esperada, Esquerda_Real, Esquerda_Corrigida;
+float Direita_Esperada, Direita_Real, Direita_Corrigida;
+//int Esquerda_realSpeed, Direita_realSpeed;
+//int Direita_Count = 0, Direita_LastCount = 0;
+//int Esquerda_Count = 0, Esquerda_LastCount = 0;
+//int Esquerda_lastExpectedSpeed, Direita_lastExpectedSpeed;
+
+
+/*=================================================
+-------------- CODIGO DOS SENSORES ----------------
+=================================================*/
+
+//------------------ FUNCAO -----------------------
+
+void Setup_Sensores (void){ //Define o modo de cada sensor
+    multiplexadorInOut.mode(PullNone);
+}
+
+int sensorCheck(int sensor){// int sensor é qual sensor se deseja ler,entre 0 a 15 "0 a 7 sao sensores frontais"
+    // 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_us(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){// esperando a leitura do sensor.
+    //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();//Tempo de leitura sera igual a leitura o tempo de leitura dos sensores
+            //pc.printf ("Tempo de leitura :%f \n\r ",tempoDeLeitura); // debug
+            tempoDoSensor.reset();
+            multiplexadorInOut.output();
+            multiplexadorInOut = 0;
+            return 0; // retorna quando for preto
+            }
+        
+    }
+    tempoDoSensor.stop();
+    float tempoDeLeitura = tempoDoSensor.read_us();//Tempo de leitura sera igual a leitura o tempo de leitura dos sensores
+    //pc.printf ("Tempo de leitura :%f \n\r ",tempoDeLeitura); // debug
+    tempoDoSensor.reset();
+    multiplexadorInOut.output();
+    multiplexadorInOut = 0;
+    return 1;// retorna quando for branco
+    }
+
+void Leitura_Sensores (void){ //Faz a leitura dos sensores e retorna a posição
+    int leitura[8]={0,0,0,0,1,0,1,0}; // leitura representa os sensores lidos,onde 1 é branco e 0 é preto
+    for (int i=0;i<8;i++){leitura[i] = sensorCheck(i);} // leitura dos sensores
+    //analizando a leitura e setando a posição
+    //if (leitura[3]== 1 && leitura[4] == 1) {Position = 45;} //Se ler os dois do meio, está no SetPoint
+    if (lastSensor == 3 && leitura [0]==0 && leitura [1]==0 && leitura [2]==0 && leitura [3]==0 && leitura[5]==0 && leitura [7] == 0 ){Position = 50;lastSensor = 4;}
+    if (leitura[2]== 1 && leitura[5] == 1) {Position = 45;lastSensor = 3;}// lastSensor=3 pois esta no meio, medida tomada para cruzamentos.
+    else if (leitura[1]== 1 && leitura[5] == 1) {Position = 45;lastSensor = 3;}
+    else if (leitura[1]==1) {Position = 20;lastSensor = 1;}
+    else if (leitura[0]==1) {Position = 10;lastSensor = 0;}
+    else if (leitura[7]==1) {Position = 80;lastSensor = 7;}
+    else if (leitura[2]==1) {Position = 30;lastSensor = 2;}
+    else if (leitura[5]==1) {Position = 60;lastSensor = 5;}
+    else if (leitura[3]==1) {Position = 40;lastSensor = 3;}
+    else if (leitura[6]==1) {Position = 70;lastSensor = 6;}
+    //else {Final_Stop();}
+    //pc.printf("\n\rPosicao: %d", Position); //Print de teste
+}
+
+/*=================================================
+--------------- CODIGO DOS MOTORES ----------------
+=================================================*/
+
+//------------------ FUNCOES ----------------------
+
+void Setup_Motores(void);
+void ControleMotores (float VelocidadeEsquerda, float VelocidadeDireita );
+
+/*=================================================
+---------------  PID DOS SENSORES -----------------
+=================================================*/
+
+float PID_Sensores(int Position){ //Função que aplica o PID e calcula a correção
+    Error = Position - SET_POINT;//Achando o erro do PID atraves da leitura dos sensores.
+  //  Integral=Integral + Error; // acumulador de erro da integral
+    //bt.printf("\n\rErro: %d", Error);
+    /*
+    if (Integral>MAX_INTEGRAL){ //limitador da integral
+        Integral=MAX_INTEGRAL;}
+    else if (Integral<MIN_INTEGRAL){
+        Integral=MIN_INTEGRAL;}
+    */
+    if (Error < 25 && Error > -25){//Sem o parametro de correcao
+        Correcao_Sensor = Kp_Sensor * (float)Error + Kd_Sensor * ((float)Error - (float)LastError);// + Integral*Ki_Sensor ;
+    }
+    else{//Com o parametro de correcao
+        Correcao_Sensor = ParametroCorrecao * Kp_Sensor * (float)Error + ParametroCorrecao * Kd_Sensor * ((float)Error - (float)LastError);// + Integral*Ki_Sensor;
+    }
+    
+    //bt.printf("\n\rCorrecao: %.2f | Erro: %d | LastError: %d | KP: %.1f | KD: %.1f", Correcao_Sensor, Error, LastError, (Kp_Sensor * (float)Error), (Kd_Sensor * ((float)Error - (float)LastError)));
+    //Comando para printar e verificar os dados de saida via bluetooth
+    
+    LastError = Error;
+    
+    return Correcao_Sensor;
+}
+
+/*=================================================
+---------------- PID DOS MOTORES ------------------
+=================================================*/
+
+/*
+
+void rightEncoderEvent (){
+        if (B.read() == 0){
+            Direita_Count++;
+        }
+        
+        else{
+            Direita_Count--;
+        }
+}
+
+void leftEncoderEvent(){
+    if (C.read() == 0) { Esquerda_Count++; }
+    else { Esquerda_Count--; }
+}
+
+void Speed () {
+
+    Esquerda_realSpeed = (Esquerda_Count - Esquerda_LastCount); //A realSpeed é o numero de voltas/giros por leitura. Comparar o maximo e minimo do Afro com a leitura do encoder.
+    Esquerda_LastCount = Esquerda_Count;
+    
+    Direita_realSpeed = (Direita_Count - Direita_LastCount);
+    Direita_LastCount = Direita_Count;
+    //bt.printf("\n\rCount: %d ## Speed: %d",rightCount,realSpeed);
+        
+}
+
+int Converte_Velocidade (int realSpeed){
+    int realrodaEsquerda;
+    
+    realrodaEsquerda = (VMAX_rodaEsquerda*realSpeed)/VMAX_Encoder;
+    
+    return realrodaEsquerda;    
+}
+
+int PID_Roda (int Lado_realrodaEsquerda, int Lado_VelocidadeEsperada, int Lado_LastError, int Contador_LadoMotor, int Integral_Lado_Roda, int Lado_lastExpectedSpeed){
+    int proporcional, derivativo, Correcao, Velocidade_Corrigida;
+    
+    proporcional = Lado_realrodaEsquerda - Lado_VelocidadeEsperada;
+    //bt.printf("\n\r%d",proporcional);
+    derivativo = proporcional - Lado_LastError;
+    //bt.printf("\n\rDerivativo: %d",derivativo);
+    
+    Correcao = Kp_Roda*proporcional + Kd_Roda*derivativo + Ki_Roda*Integral_Lado_Roda;
+    //bt.printf("\n\r%d",Correcao);
+    
+    Lado_LastError = proporcional;
+    Integral_Lado_Roda += proporcional;
+
+    if (Lado_lastExpectedSpeed == Lado_VelocidadeEsperada) { return Velocidade_Corrigida; }
+    
+    if (proporcional == 0){ Lado_LastError = 0; }
+    else{
+        if (Integral_Lado_Roda >= MAX_INTEGRAL) { Integral_Lado_Roda = MAX_INTEGRAL; }
+        else { Integral_Lado_Roda = MIN_INTEGRAL; }        
+    
+    }
+    Lado_lastExpectedSpeed = Lado_VelocidadeEsperada;
+    
+    Velocidade_Corrigida = (Lado_realrodaEsquerda - Correcao);
+    
+    return Velocidade_Corrigida;
+}
+
+*/
+
+/*=================================================
+---------------  FUNCAO PRINCIPAL -----------------
+=================================================*/
+
+int main(void){
+    
+    //A.rise(&rightEncoderEvent);
+    //END.attach(&Speed, 1.0);
+    //C.rise(&leftEncoderEvent);
+    
+    //END.attach(&Speed, 1.0);
+    //END1.attach(&Speed, 1.0); //TESTAR VER SE ISSO FUNCIONA
+    
+    Setup_Sensores(); //Faz o setup dos sensores
+    Setup_Motores(); //Faz o setup dos motores
+    
+    //while (button.read()) { led = 1; wait(0.2); led=0; wait(0.2); }
+    
+    wait(2); //Delay para estabilizar
+    
+    PulsoInicial(); // Pulso inicial para os motore
+    
+    END.attach(&ParadaFinal, 24.5); //Chama a funcao ParadaFinal em x segundos
+    
+    while(1){
+        //Para_Demonio();
+        Leitura_Sensores(); //Lê os sensores
+        
+        Correction = PID_Sensores(Position); //Calcula a correção
+        
+        Esquerda_Esperada = -1*(velocidadeBase + (Correction * 0.01)); //Aplica a correção
+        Direita_Esperada = velocidadeBase - (Correction * 0.01);
+        
+        //bt.printf("\n\rEsquerda: %.2f | Direita: %.2f", Esquerda_Esperada, Direita_Esperada);
+        //Print das velocidades que estão chegando nas rodas após o PID
+        
+        //Volta_Pra_Linha(); //Volta para a linha caso tenha perdido
+    
+        ControleMotores(Esquerda_Esperada, Direita_Esperada); //Drive
+            
+    }
+}
\ No newline at end of file