Header dos motores do seguidor de linha
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