RioBotz / Mbed 2 deprecated LineFollowerV2-Final

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

PIDControl/PID.cpp@4:beaad00c1396, 2019-02-28 (annotated)

Committer:
Raygrou
Date:
Thu Feb 28 19:04:25 2019 +0000
Revision:
4:beaad00c1396
Parent:
3:a0c3e850f70d
Child:
5:667681629022
.

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 //################################################################################
amaral99 0:44f2bd8f9b56 14 //################################################################################
amaral99 0:44f2bd8f9b56 15 //**************************** DEFINICOES - INICIO *****************************
amaral99 0:44f2bd8f9b56 16
Raygrou 3:a0c3e850f70d 17 #define Kp 0.7 //Definicao da constante proporcional do PID
Raygrou 3:a0c3e850f70d 18 #define Kd 4.85 //Definicao da constante derivativa do PID
Raygrou 3:a0c3e850f70d 19 #define Ki 0.0 //Definicao da constante integrativa do PID
Raygrou 3:a0c3e850f70d 20
Raygrou 2:cbd63463e7a1 21 #define MIN_SPEED 0.0 //Definicao da velocidade minima em %
Raygrou 2:cbd63463e7a1 22 #define BASE_SPEED 30.0 //Definicao da velocidade base (no SetPoint) em %
Raygrou 3:a0c3e850f70d 23 #define MAX_SPEED 50.0 //Definicao da velocidade maxima em %
amaral99 0:44f2bd8f9b56 24
Raygrou 3:a0c3e850f70d 25 #define GANHO_CURVA 62.5 //Definicao do ganho em curva (quando o LF perder a linha)
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 0:44f2bd8f9b56 31 int LastError = 0; //Definicao da variavel que armazenara o ultimo erro
amaral99 0:44f2bd8f9b56 32 int LastSensor = 0; //Definicao da variavel que armazenara a ultima posicao
Raygrou 4:beaad00c1396 33 double Integral = 0; //Definicao da variavel que armazera a soma dos erros
Raygrou 4:beaad00c1396 34
Raygrou 4:beaad00c1396 35 //#define DEBUG
Raygrou 4:beaad00c1396 36 Serial pc (USBTX, USBRX);
amaral99 0:44f2bd8f9b56 37
amaral99 0:44f2bd8f9b56 38 //***************************** DEFINICOES - FINAL *****************************
amaral99 0:44f2bd8f9b56 39 //################################################################################
amaral99 0:44f2bd8f9b56 40 //################################################################################
amaral99 0:44f2bd8f9b56 41 //******************************* FUNCOES - INICIO ******************************
amaral99 0:44f2bd8f9b56 42
amaral99 0:44f2bd8f9b56 43 //Funcao que realiza o controle do Robo
Raygrou 2:cbd63463e7a1 44 void PID_Control(int Error, float *LeftSpeed, float *RightSpeed) {
amaral99 0:44f2bd8f9b56 45
amaral99 0:44f2bd8f9b56 46 float Control; //Inicializacao da variavel de controle
amaral99 0:44f2bd8f9b56 47
amaral99 0:44f2bd8f9b56 48 //Calculo do controle a partir do Erro
Raygrou 2:cbd63463e7a1 49 Error = Error - SET_POINT;
Raygrou 2:cbd63463e7a1 50 Control = (Error * Kp) + ((Error - LastError) * Kd) + (Integral * Ki);
amaral99 0:44f2bd8f9b56 51
amaral99 0:44f2bd8f9b56 52 LastError = Error; //Atualizacao do ultimo Erro
amaral99 0:44f2bd8f9b56 53 Integral += Error; //Atualizacao da samoa dos Erros
amaral99 0:44f2bd8f9b56 54
amaral99 0:44f2bd8f9b56 55 //Caso o Erro seja zero (ja esta em linha reta)
amaral99 0:44f2bd8f9b56 56 //Se nao limita-se o valor da variavel
Raygrou 2:cbd63463e7a1 57 if (Error == 0) { Integral = 0; }
amaral99 0:44f2bd8f9b56 58 else { if (Integral > MAX_INTEGRAL) { Integral = MAX_INTEGRAL; }
amaral99 0:44f2bd8f9b56 59 if (Integral < MIN_INTEGRAL) { Integral = MIN_INTEGRAL; } }
Raygrou 3:a0c3e850f70d 60
Raygrou 3:a0c3e850f70d 61 if (Get_Preto()) {
Raygrou 4:beaad00c1396 62 if(LastSensor > 0){
Raygrou 4:beaad00c1396 63 (*LeftSpeed) = -(GANHO_CURVA);
Raygrou 4:beaad00c1396 64 (*RightSpeed) = GANHO_CURVA;
Raygrou 4:beaad00c1396 65 Drive((*LeftSpeed), (*RightSpeed));
Raygrou 4:beaad00c1396 66 #ifdef DEBUG
Raygrou 4:beaad00c1396 67 pc.printf ("LeftSpeed: %f", (*LeftSpeed));
Raygrou 4:beaad00c1396 68 pc.printf ("\tRightSpeed: %f\n\r", (*RightSpeed));
Raygrou 4:beaad00c1396 69 pc.printf("CHEGUEI AQUI > SET_POINT\n\r");
Raygrou 4:beaad00c1396 70 pc.printf("LastSensor: %i", LastSensor);
Raygrou 4:beaad00c1396 71 wait(0.5);
Raygrou 4:beaad00c1396 72 #endif
Raygrou 4:beaad00c1396 73 }
Raygrou 4:beaad00c1396 74 if(LastSensor < 0){
Raygrou 4:beaad00c1396 75 (*LeftSpeed) = GANHO_CURVA;
Raygrou 4:beaad00c1396 76 (*RightSpeed) = -(GANHO_CURVA);
Raygrou 4:beaad00c1396 77 Drive((*LeftSpeed), (*RightSpeed));
Raygrou 4:beaad00c1396 78 #ifdef DEBUG
Raygrou 4:beaad00c1396 79 pc.printf ("LeftSpeed: %f", (*LeftSpeed));
Raygrou 4:beaad00c1396 80 pc.printf ("\tRightSpeed: %f\n\r", (*RightSpeed));
Raygrou 4:beaad00c1396 81 pc.printf("CHEGUEI AQUI < SET POINT\n\r");
Raygrou 4:beaad00c1396 82 pc.printf("LastSensor: %i", LastSensor);
Raygrou 4:beaad00c1396 83 wait(0.5);
Raygrou 4:beaad00c1396 84 #endif
Raygrou 4:beaad00c1396 85 } }
Raygrou 3:a0c3e850f70d 86 else{
amaral99 0:44f2bd8f9b56 87
Raygrou 4:beaad00c1396 88 LastSensor = Error;
Raygrou 4:beaad00c1396 89
Raygrou 2:cbd63463e7a1 90 (*LeftSpeed) = BASE_SPEED - Control; //Atualizacao da velocidade esquerda
Raygrou 2:cbd63463e7a1 91 (*RightSpeed) = BASE_SPEED + Control; //Atualizacao da velocidade direita
amaral99 0:44f2bd8f9b56 92
Raygrou 4:beaad00c1396 93 #ifdef DEBUG
Raygrou 4:beaad00c1396 94 pc.printf ("LeftSpeed: %f", (*LeftSpeed));
Raygrou 4:beaad00c1396 95 pc.printf ("\tRightSpeed: %f\n\r", (*RightSpeed));
Raygrou 4:beaad00c1396 96 pc.printf("LastSensor: %d", LastSensor);
Raygrou 4:beaad00c1396 97 wait(0.5);
Raygrou 4:beaad00c1396 98 #endif
Raygrou 4:beaad00c1396 99
amaral99 0:44f2bd8f9b56 100 //Limitacao das velocidades do motor direito e esquerdo
Raygrou 2:cbd63463e7a1 101 if ((*LeftSpeed) > MAX_SPEED) { (*LeftSpeed) = MAX_SPEED; }
Raygrou 2:cbd63463e7a1 102 if ((*LeftSpeed) < MIN_SPEED) { (*LeftSpeed) = MIN_SPEED; }
Raygrou 2:cbd63463e7a1 103 if ((*RightSpeed) > MAX_SPEED) { (*RightSpeed) = MAX_SPEED; }
Raygrou 2:cbd63463e7a1 104 if ((*RightSpeed) < MIN_SPEED) { (*RightSpeed) = MIN_SPEED; }
Raygrou 3:a0c3e850f70d 105 }
amaral99 0:44f2bd8f9b56 106 }
amaral99 0:44f2bd8f9b56 107
amaral99 0:44f2bd8f9b56 108 //******************************** FUNCOES - FINAL ******************************