prof.rodrigoluiz

Ter Out 26, 2021 9:39 am
Robô Seguidor deParede com controlador PID

Olá.

Estou deixando aqui o meu código de robô seguidor de parede.

Além do hardware que vem na caixa da plataforma Falcon, usei um sensor de distância HC-SR04 acoplado na parte de cima do chassi da Falcon montado sobre um motor servo que pretendo usar para as próximas etapas deste projeto. No código que estou deixando aqui o servo não é utilizado.

Para fixar o servo e o sensor de distância adicional, o ideal seria usar um suporte Blackskull - que dá pra comprar na loja da Robocore. Como eu não tinha esse suporte, improvisei com umas peças de LEGO e elásticos. Queria postar uma imagem aqui, mas me parece que não tenho permissão para adicionar imagens e nem para anexar arquivos.

O sensor frontal é usado para detectar obstáculos e fazer o robô girar em 90 graus caso ele detecte um obstáculo a uma certa distância determinada no código. 

Para testar a ideia de criar um seguidor de parede, eu fiz uma versão sem PID, mas o robô oscila muito e se perde muito facilmente em curvas - mesmo em curvas não muito fechadas. Por isso decidi implementar o PID. Mesmo esta versão que estou deixando aqui tende a se perder na curva caso ela seja uma curva de 180 graus - quando o robô tenta contornar a extremidade de uma parede, por exemplo. Não passei muito tempo calibrando as constantes para o cálculo do PID, mas o resultado obtido com este código é satisfatório o suficiente na minha opinião.

O código é basicamente uma adaptação do controlador PID que implementei para um robô seguidor de linha e que postei aqui no fórum https://www.robocore.net/...opic&t=7130. Como não tenho muita experiência com programação, imagino que o código possa ser melhorado bastante. Agradeço quaisquer dicas, sugestões ou críticas que possam melhorar este projeto.



Código:/*******************************************************************************
Robô seguidor de parede

Este código faz com que a plataforma Falcon siga uma parede que esteja à esquerda do robô e desvie para a direita caso encontre um objeto bloqueando o caminho.

*******************************************************************************/

#include <FalconRobot.h>                                                                //inclui biblioteca da plataforma Falcon

#define wallDistance 15                                                                 //parâmetro para distância entre o robô e a parede
#define SPEED 50                                                                        // Set to any number from 0 - 100.

FalconRobotMotors motors(5, 7, 6, 8);                                                   //cria o objeto "motors" e associa-os aos pinos 5, 6, 7 e 8
int leftSpeed;                                                                          // variável para atribuir a velocidade do motor esquerdo
int rightSpeed;                                                                         // variável para atribuir a velocidade do motor direito

int initial_motor_speed = SPEED;                                                        //velocidade inicial do motor

int left_motor_speed;                                                                   //variável para atribuir a velocidade do motor esquerdo após a correção PID
int right_motor_speed;                                                                  //variável para atribuir a velocidade do motor esquerdo após a correção PID


int DISTANCE;                                                                           //distância lida pelo sensor posicionado no topo da plataforma Falcon
int FRONT;                                                                              //distância lida pelo sensor posicionado no frente da plataforma Falcon 

int object;                                                                              //variável para detecção de um objeto bloqueando o caminho do robô


FalconRobotDistanceSensor frontSensor(2, 3); // (echo, trig)                             cria o sensor posicionado na frente do robô
FalconRobotDistanceSensor distance(12, 13);  // (echo, trig)                             cria o sensor posicionado no topo do robô

// Constantes PID: Valores de correção Porporcional, Integral e Diferencial. 
float Kp = 40;
float Ki = 20;
float Kd = 32;

float error = 0, P = 0, I = 0, D = 0, PID_value = 0;              // variáveis necessárias para o cálculo do fator de correção PID
float previous_error = 0, previous_I = 0;                         // variáveis necessárias para o cálculo do fator de correção PID


void setup() {
  Serial.begin(9600);
  Serial.println("Welcome to the Wall Follower Experiment");
  delay(2000);
}

void loop() {

  calculate_pid();                                               //chama a função para o cálculo da correção PID
  motor_control();                                               //chama a função de controle dos motores
  read_front_sensor();                                            //chama a função de detecção de objeto


  if (object == 1)                                               //se algum objeto for detectado...
  {
    stop_bot();                                                  //o robô para e...
    delay(100);
    turnRight();                                                 //gira para a direita
    object = 0;

  }

  if (object == 0) {                                             //se nenhum objeto for detectado...


    if (error == 2.0)                                            //...e o robô estiver longe da parede...
    {
      read_top_sensor();
      leftSpeed = left_motor_speed;                              
      rightSpeed = right_motor_speed;
      sharpLeftTurn();                                           //...o robô vira bruscamente para a esquerda.
    }
    if (error == 1.0)                                            //...e o robô não estiver tão longe assim da parede...
    {
      read_top_sensor();                             
      leftSpeed = left_motor_speed;
      rightSpeed = right_motor_speed;
      Left();                                                    //...o robô vira para a esquerda. 
      // forward();
    }

    if (error == 0.0)                                            //...e o robô estiver à distânca ideal da parede...
    {
      read_top_sensor();
      leftSpeed = left_motor_speed;
      rightSpeed = right_motor_speed;
      forward();                                                 //... o robô move-se para a frente.
    }
    if (error == -1.0)                                           //se o robô não estiver muito próximo da parede...
    {
      read_top_sensor();
      leftSpeed = left_motor_speed;
      rightSpeed = right_motor_speed;
      Right();                                                    //...o robô vira para a direita...
      // forward();
    }
    if (error == -2.0)                                            //se o robô estiver próximo da parede...
    {
      read_top_sensor();
      leftSpeed = left_motor_speed;
      rightSpeed = right_motor_speed;
      sharpRightTurn();                                            //o robô vira bruscamente para a direita.
    }
  }
  delay(0);
}

void read_front_sensor() {                                           //funcao para detectar se há um objeto bloqueando o caminho do robô

  FRONT = frontSensor.read();
  if (FRONT > 0 && FRONT < 20)                                     //se houver um objeto posicionado a menos de 20 cm do sensor frontal...
  {
    object = 1;                                                    //...muda o valor de "object" para 1
  }

  else if (FRONT > 20)                                             //...caso contrário...

  {
    object = 0;                                                    //..."object" vale 0, ou seja, o caminho está livre.
  }

}

void read_top_sensor() {
  //esta função transforma a distância entre a parede e o sensor em um valor inteiro de -2 a 2 que será usado no cálculo da correção PID. 


  
  //   Observe o diagrama a seguir para entender o que cada valor de "error" significa. "wD" significa "wallDistance", variável criada no preâmbulo deste código
  //
  //
  //            1*wD    2*1*wD    3*wD    4*Wd      5*wD
  //  P ||       |        |        |        |        |
  //  A ||   *   |        |        |        |        |error =  -2               robô muito próximo da parede
  //  R ||       |   *    |        |        |        |error =  -1               robô não muito próximo da parede
  //  E ||       |        |   *    |        |        |error =   0               robô na distância ideal
  //  D ||       |        |        |   *    |        |error =   1               robô não muito longe da parede
  //  E ||       |        |        |        |   *    |error =   2               rôbo muito longe da parede
  //    ||       |        |        |        |
  //


  DISTANCE = distance.read(); //leitura do sensor no topo do robô


  if ((DISTANCE < 0 * wallDistance) && (DISTANCE <= 1 * wallDistance))           //robô precisa girar bruscamente para a direita
  {
    error = -2.0;
  }

  else if ((DISTANCE < 2 * wallDistance) && (DISTANCE <= 3 * wallDistance))      //robô precisa girar para a direita
  {
    error = -1.0;
  }
  else if ((DISTANCE < 3 * wallDistance) && (DISTANCE <= 4 * wallDistance))       //robô move-se para frente
    error =  0.0;

  else if ((DISTANCE < 4 * wallDistance) && (DISTANCE <= 5 * wallDistance))       //robô precisa girar para a esquerda
  {
    error = 1.0;
  }
  else if ((DISTANCE < 5 * wallDistance) && (DISTANCE <= 6 * wallDistance))        //robô precisa girar bruscamente para a esquerda
  {
    error = 2.0;
  }

}

void calculate_pid()                                                //função para calcular os valores de P, I e D após o "read_top_sensor()" retornar um valor inteiro entre -2 e 2
{
  P = error;                                                        //valor da função "read_top_sensor()" em relação à parede
  I = I + previous_I;                                               //valor do erro acumulado
  D = error - previous_error;                                       //variável para calcular a variação do erro ao longo do tempo

  PID_value = (Kp * P) + (Ki * I) + (Kd * D);                       //fórmula de cálculo do fator de correção

  previous_I = I;                                                   //atualização da variável "I"
  previous_error = error;                                           //atualização da variável "error"
}

void motor_control()                                                //função para controlar os motores do robô
{
  // Calculating the effective motor speed:
  int left_motor_speed = initial_motor_speed - PID_value;           //velocidade da roda esquerda após a correção feita pelo PID
  int right_motor_speed = initial_motor_speed + PID_value;          //velocidade da roda direita após a correção feita pelo PID

  // The motor speed should not exceed 100
  left_motor_speed = constrain(left_motor_speed, 0, 100);           //limita a velocidade da roda esquerda a um valor entre 0 e 100
  right_motor_speed = constrain(right_motor_speed, 0, 100);         //limita a velocidade da roda direita a um valor entre 0 e 100

  leftSpeed = left_motor_speed;                                     //atribuir o valor da velocidade da roda esquerda após a correção à variável "leftSpeed"
  rightSpeed = right_motor_speed;                                   //atribuir o valor da velocidade da roda direita após a correção à variável "righttSpeed"

  forward();                                                        //move o robô para frente
}

void forward()                                                      //função para mover o robô para a frente
{
  motors.leftDrive(leftSpeed, FORWARD);
  motors.rightDrive(rightSpeed, FORWARD);
}
void reverse()                                                     //função para mover o robô para trás
{
  motors.leftDrive(leftSpeed, BACKWARD);
  motors.rightDrive(rightSpeed, BACKWARD);
}
void Right()                                                       //função para girar o robô para a direita
{
  motors.leftDrive(leftSpeed, FORWARD);
  motors.rightDrive(rightSpeed, FORWARD);
}
void Left()                                                        //função para girar o robô para a esquerda
{
  motors.leftDrive(leftSpeed, FORWARD);
  motors.rightDrive(rightSpeed, FORWARD);
}
void sharpRightTurn()                                               //função para girar o robô bruscamente para a direita
{
  motors.leftDrive(leftSpeed, FORWARD);
  motors.rightDrive(rightSpeed, BACKWARD);
}

void sharpLeftTurn()                                                 //função para girar o robô bruscamente para a esquerda
{
  motors.leftDrive(leftSpeed, BACKWARD);
  motors.rightDrive(rightSpeed, FORWARD);
}

void turnRight() {                                                   //função para girar o robô para a direita caso um obstáculo seja detectado
  motors.leftDrive(50, FORWARD);  // spin CCW
  motors.rightDrive(50, BACKWARD); // spin CCW
  delay(250);
  motors.stop();
  delay(250);  // short delay to let robot fully stop
}

void stop_bot()
{
  motors.stop();       // Stops both motors
}