RioBotz / Mbed 2 deprecated LineFollowerV2-Final

Dependencies:   mbed

PIDControl/PID.cpp@11:8d71a1e1e947, 2019-08-09 (annotated)

Committer:
amaral99
Date:
Fri Aug 09 17:11:40 2019 +0000
Revision:
11:8d71a1e1e947
Parent:
10:e412756d0f66 
.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
amaral99 0:44f2bd8f9b56 1 //================================================================================
amaral99 0:44f2bd8f9b56 2 //=========================== PID CONTROL - PID.CPP ===========================
amaral99 0:44f2bd8f9b56 3 //================================================================================
amaral99 0:44f2bd8f9b56 4
amaral99 0:44f2bd8f9b56 5 //***************************** INCLUDES - INICIO ******************************
amaral99 0:44f2bd8f9b56 6
amaral99 0:44f2bd8f9b56 7 #include "mbed.h"
amaral99 0:44f2bd8f9b56 8 #include "PID.h"
Raygrou 3:a0c3e850f70d 9 #include "Sensor.h"
Raygrou 3:a0c3e850f70d 10 #include "Motor.h"
amaral99 0:44f2bd8f9b56 11
amaral99 0:44f2bd8f9b56 12 //***************************** INCLUDES - FINAL ******************************
amaral99 0:44f2bd8f9b56 13 //################################################################################
Raygrou 7:800abbab7051 14 //################################################################################
amaral99 0:44f2bd8f9b56 15 //**************************** DEFINICOES - INICIO *****************************
amaral99 0:44f2bd8f9b56 16
amaral99 11:8d71a1e1e947 17 #define Kp 0.05 //Definicao da constante proporcional do PID - Melhor valor 0.5
amaral99 11:8d71a1e1e947 18 #define Kd 08.812 //Definicao da constante derivativa do PID - Melhor valor 8.812
amaral99 11:8d71a1e1e947 19 #define Ki -0.03 //Definicao da constante integrativa do PID - Melhor valor -0.03
amaral99 11:8d71a1e1e947 20
Raygrou 2:cbd63463e7a1 21 #define MIN_SPEED 0.0 //Definicao da velocidade minima em %
amaral99 11:8d71a1e1e947 22 #define BASE_SPEED 20.0 //Definicao da velocidade base (no SetPoint) em %
amaral99 11:8d71a1e1e947 23 #define MAX_SPEED 80.0 //Definicao da velocidade maxima em %
amaral99 0:44f2bd8f9b56 24
amaral99 11:8d71a1e1e947 25 #define GANHO_CURVA 10 //Definicao do ganho em curva (quando o LF perder a linha) Valor original: 32
amaral99 0:44f2bd8f9b56 26 #define SET_POINT 45 //Definicao da leitura no SetPoint
amaral99 0:44f2bd8f9b56 27
amaral99 0:44f2bd8f9b56 28 #define MIN_INTEGRAL -50 //Definicao do valor minimo da integral do PID
amaral99 0:44f2bd8f9b56 29 #define MAX_INTEGRAL +50 //Definicao do valor maximo da integral do PID
amaral99 0:44f2bd8f9b56 30
amaral99 11:8d71a1e1e947 31 Serial PCPID(USBTX,USBRX);
amaral99 11:8d71a1e1e947 32
Raygrou 8:2b0c6a2aef4a 33 int LastError = 0; //Definicao da variavel que armazenara o ultimo erro
Raygrou 8:2b0c6a2aef4a 34 int LastSensor = 0; //Definicao da variavel que armazenara a ultima posicao
Raygrou 8:2b0c6a2aef4a 35 int Integral = 0; //Definicao da variavel que armazera a soma dos erros
amaral99 0:44f2bd8f9b56 36
Raygrou 10:e412756d0f66 37 DigitalIn sM(p10); //Definicao da porta do sensor traseiro do meio
Raygrou 9:f7f8dd99b3d4 38
amaral99 0:44f2bd8f9b56 39 //***************************** DEFINICOES - FINAL *****************************
amaral99 0:44f2bd8f9b56 40 //################################################################################
amaral99 0:44f2bd8f9b56 41 //################################################################################
amaral99 0:44f2bd8f9b56 42 //******************************* FUNCOES - INICIO ******************************
amaral99 0:44f2bd8f9b56 43
Raygrou 10:e412756d0f66 44 //Define o mbed como um resistor de PullUp
Raygrou 10:e412756d0f66 45 void Meio_Setup(void){ sM.mode(PullUp); }
Raygrou 10:e412756d0f66 46
amaral99 0:44f2bd8f9b56 47 //Funcao que realiza o controle do Robo
Raygrou 2:cbd63463e7a1 48 void PID_Control(int Error, float *LeftSpeed, float *RightSpeed) {
amaral99 0:44f2bd8f9b56 49
amaral99 0:44f2bd8f9b56 50 float Control; //Inicializacao da variavel de controle
amaral99 0:44f2bd8f9b56 51
Raygrou 2:cbd63463e7a1 52 Error = Error - SET_POINT;
amaral99 11:8d71a1e1e947 53 PCPID.printf("Error: %d\n\r",Error);
amaral99 0:44f2bd8f9b56 54
amaral99 11:8d71a1e1e947 55 //Calculo do controle a partir do Erro\ Error = Error;
amaral99 11:8d71a1e1e947 56 if(Error > 25 || Error < -25){ Control = (Error * 5 * Kp) + ((Error - LastError)* 5 * Kd) + (Integral* 5 * Ki); }
amaral99 11:8d71a1e1e947 57 else { Control = (Error * Kp) + ((Error - LastError) * Kd) + (Integral * Ki); }
amaral99 11:8d71a1e1e947 58 PCPID.printf("Control: %d\n\r",Control);
amaral99 0:44f2bd8f9b56 59 LastError = Error; //Atualizacao do ultimo Erro
amaral99 0:44f2bd8f9b56 60 Integral += Error; //Atualizacao da samoa dos Erros
amaral99 0:44f2bd8f9b56 61
amaral99 0:44f2bd8f9b56 62 //Caso o Erro seja zero (ja esta em linha reta)
amaral99 0:44f2bd8f9b56 63 //Se nao limita-se o valor da variavel
Raygrou 2:cbd63463e7a1 64 if (Error == 0) { Integral = 0; }
amaral99 0:44f2bd8f9b56 65 else { if (Integral > MAX_INTEGRAL) { Integral = MAX_INTEGRAL; }
amaral99 0:44f2bd8f9b56 66 if (Integral < MIN_INTEGRAL) { Integral = MIN_INTEGRAL; } }
Raygrou 10:e412756d0f66 67
Raygrou 10:e412756d0f66 68 //Caso todos os sensores frontais leiam preto
Raygrou 10:e412756d0f66 69 //o LF gira para o lado do ultimo sensor que leu a linha
Raygrou 3:a0c3e850f70d 70 if (Get_Preto()) {
Raygrou 10:e412756d0f66 71 if(LastSensor > 0){ (*LeftSpeed) = -50; (*RightSpeed) = GANHO_CURVA; } // Valor original: 40
Raygrou 10:e412756d0f66 72 if(LastSensor < 0){ (*LeftSpeed) = GANHO_CURVA; (*RightSpeed) = -50; }
Raygrou 8:2b0c6a2aef4a 73 }
Raygrou 8:2b0c6a2aef4a 74
Raygrou 10:e412756d0f66 75 //Se pelo menos um sensor estiver lendo entra no else
Raygrou 8:2b0c6a2aef4a 76 else {
Raygrou 4:beaad00c1396 77 LastSensor = Error;
Raygrou 9:f7f8dd99b3d4 78
Raygrou 10:e412756d0f66 79 if(Error == 0 && sM.read() == 0){ //Caso o sensor fontal do meio esteja lendo a linha
Raygrou 10:e412756d0f66 80 (*LeftSpeed) = 45; //e o traseiro central também esteja, ativa o Turbo
Raygrou 10:e412756d0f66 81 (*RightSpeed) = 45;
Raygrou 9:f7f8dd99b3d4 82 }
Raygrou 10:e412756d0f66 83 //Caso não entre em Turbo, atualiza a velocidade
Raygrou 9:f7f8dd99b3d4 84 else{
Raygrou 9:f7f8dd99b3d4 85 (*LeftSpeed) = BASE_SPEED - Control; //Atualizacao da velocidade esquerda
Raygrou 9:f7f8dd99b3d4 86 (*RightSpeed) = BASE_SPEED + Control; //Atualizacao da velocidade direita
Raygrou 9:f7f8dd99b3d4 87 }
amaral99 0:44f2bd8f9b56 88
amaral99 0:44f2bd8f9b56 89 //Limitacao das velocidades do motor direito e esquerdo
Raygrou 2:cbd63463e7a1 90 if ((*LeftSpeed) > MAX_SPEED) { (*LeftSpeed) = MAX_SPEED; }
Raygrou 2:cbd63463e7a1 91 if ((*LeftSpeed) < MIN_SPEED) { (*LeftSpeed) = MIN_SPEED; }
Raygrou 2:cbd63463e7a1 92 if ((*RightSpeed) > MAX_SPEED) { (*RightSpeed) = MAX_SPEED; }
Raygrou 2:cbd63463e7a1 93 if ((*RightSpeed) < MIN_SPEED) { (*RightSpeed) = MIN_SPEED; }
Raygrou 3:a0c3e850f70d 94 }
amaral99 0:44f2bd8f9b56 95 }
amaral99 0:44f2bd8f9b56 96
amaral99 0:44f2bd8f9b56 97 //******************************** FUNCOES - FINAL ******************************