Olá senhores, estou trabalhando a meses na construção de um drone que poderá ser um Bicoptero ou Tricoptero. De qualquer forma dar estabilidade a este tipo de aeromodelo é um pouco complicado e piora quando se quer criar um sistema do zero tanto no hardware como no Software.

Bem, solicito a atenção cuidadosa de todos os interessados, por que sei q no futuro ter o conhecimento de como programar algo assim é importante, pois pode-se a aplicar a qualquer projeto que necessite de estabilidade, como estabilidade para cameras, robôs bipedes etc..

 

Bem, estou tentando construir algo semelhante aqueles aeroplanos do filme avatar:

 

https://www.youtube.com/watch?v=iAyzPHYFTT8

 

No link acima, o criador em 0:25 a 1:20 usa um pendulo para verificar o equilibrio de todo o sistema.

Mas acredito que ele acrescentou alguma placa controladora ou IMU ja pronta, que não é meu caso.

 

Bem estou usando um Acelerometro e um Gyroscopio e a minha dúvida no momento  é:

É o caso! Usar os dois componentes juntos para obter um valor unico para obter a estabilidade de todo o sistema, ou não necessariamente?

Eu acredito que apenas um giro e a programação para ele eu consiga obter a estabilidade do bicoptero, então solicito opinião de vocês e experiência, pois caso acredite que apenas o gyro é necessário irei abandonar esse codigo e seguir por outro caminho.

 

Estou baseando meu projeto em robos do tipo Segway, pelo menos o código, no projeto do Otavio de SP que postou um tutorial no link:

http://www.instructables.com/id/Angle-measurement-using-gyro-accele...

Esse tipo de robo se baseia no equilibrio por isso tomei este projeto como modelo.

O robo dele possui apenas um eixo de rodas e é controlado pelo joystick do Nitendo wii Nunchuck Wii adapter for Arduino

 

Eu modifiquei o codigo dele para as minhas necessidades que é controlar a velocidade de dois motores brusheless no intuito de acelerar e desacelerar e manter o sistema estavel.

 

Segue o código:

//----------------------------------------------------------------------------------------

// Este código é uma miscelanea de diversos códigos
// que encontrei na internet. Mas agradeço principalmente
// ao Otávio - SP por disponibilizar um tutorial
// bastante elucidativo sobre o tema (Acelerometro + Gyroscópio)
// em seu robo do tipo Segway
// http://www.instructables.com/id/Angle-measurement-using-gyro-accele...
//---------------------------------------------------------------
// Este código tem a finalidade de controlar de maneira
// estavel o vôo de um Drone
//----------------------------------------------------------------

#include <ServoTimer2.h> 

// A Biblioteca ServoTimer permite
// Declarar variáveis para até oito Motores
ServoTimer2 Motor01;   
ServoTimer2 Motor02;
ServoTimer2 Motor03;

#define X  12
#define Y  11
#define Z  10


 int ligar =1;
   //---------------------
 int val, val1, val2;
  //---------------------
//----------------------------------------------------------------


//variaveis genericas
int ledPin = 13;
char c;
int i=0,inicio=0;
boolean first_exec=true;
//double T=0;

//variaveis de controle WI-FI
int zbut,cbut;

//variaveis do aceleroemtro
int offsetX=132,offsetY=134,offsetZ=190,offsetW=319;  //offset values are obtained holding the chuck on a fix zero position
//valores de deslocamento são obtidos segurando o mandril em uma posição zero correção
static double scaleX=-50,scaleY=50,scaleZ=50,scaleW=0.512;  //point axxis upward then downward, subtracting the values, and divide by 2
//Axxis ponto para cima, em seguida, para baixo, subtraindo os valores e dividir por 2
double accx,accy,accz;
byte countx,county,countz;
double pitch=0;

//variaveis do gyroscopio
double gyro=0,w=0,teta=0,filter_teta=0;

//variaveis do PID
double dt=1;
double error=0, last_error=0;
double last_time=0,time=0;
double output=0;


#define HPF 0.98
#define LPF (1-HPF)

/* No Setup, sao inicializados as portas, entrada ou saida,
 configurado a velocidade da porta serial e o protocolo de comunicacao
 com o controle do wii, nunchuck.
 */

void setup()
{
  //Inicializa a porta Serial
  Serial.begin(19200);
  analogReference(DEFAULT);
  Serial.flush();
  pinMode(ledPin,OUTPUT);
  // Anexando os motores ao código 
  Motor01.attach(X);    
  Motor02.attach(Y);
  Motor03.attach(Z);

}


/* O codigo dentro do loop eh executado continuamente, aqui onde serao feitas as leituras dos sensores
 o processamento deste sinal, e a execucao do controle. O tempo entre cada loop eh de aproximademente
 0.05 segundos, ou seja 200Hz de frequencia.
 */
void loop()
{

  /*Esse comando gera um delay de 2s na primeira execucao do programa para calibracao das variaveis*/
  if(first_exec==true && inicio>200)
  {
    zera();
    set_offset();
    zera();
    first_exec=false;
  }
  inicio++;
  /*Nesta parte do codigo, os valores das contantes podem ser alterados por software*/
  if(Serial.available()) c=Serial.read();
  switch(c)
  {  
  case 'k':
    zera();
    set_offset();
    break;
  }
  c='l';

  while( millis() - last_time < 4 )
  {
    dt=4;
  }
  last_time = millis();

  /* Executa a leitura do nunchuck (acelerometro e botoes) e a
   leitura do gyrscopio.   
   */
  countx  = analogRead(0);
  county  = analogRead(1);
  countz  = analogRead(2);
  gyro = analogRead(3);

  /* Transforma os valores lidos pelo controle em aceleracao (g)
   pitch eh o angulo em graus calculado pela aceleracao, ou seja, inclinacao
   do acelerometro.
   */
  accx = ((double)((countx-offsetX)/scaleX));
  accy = ((double)((county-offsetY)/scaleY));
  accz = ((double)((countz-offsetZ)/scaleZ+1));
  pitch = (double)(atan2(accy,sqrt(sq(accz)+sq(accx)))*180/3.14);

  /* Transforma os valores lidos pela porta ADC em velocidade angular */
  w    = ((double)(gyro-offsetW)/scaleW);

  teta = teta + w*dt/1000;

  /* Calculo do angulo teta estimado, aqui eh usado a tecnica descrita na secao de filtros,
   onde HPF eh a constante do filtro passa alta, LPF a cosntante do filtro passa baixa,
   w eh a velocidade angular, dt eh o tempo entre cada loop.
   */
  filter_teta = HPF*(filter_teta + w*dt/1000 ) + LPF*(pitch);
 
  //Aqui novos offsets sao setados, ou seja, a nova posicao zero do robo
  //set_offset();
 

  /* Os erros podem ser zeradas para evitar que o controlador se torne instavel  */
  // if(zbut==1) {
  //  zera();
 // }
 // else{
  //  digitalWrite(ledPin,LOW);
 // }
//--------------------------------------------------------
        if (ligar != 0)
            {
               for (val= 1500; val < 1598; val +=1)
               {
                  Motor01.write(val);//Motor Esquerdo
                  Motor02.write(val);//Motor Direito
                 // Serial.print(val);
                 // Serial.print("");
                 // Serial.println();
                  delay(100);
               }
          
            }
          ligar = 0;
//-------------------------------------------------------
 //--------------------------------
  // Execução Motor Esquerdo
  //---------------------------------
  // yRaw eixo Y do acelerômetro
  // -112, menor valor de descida da asa esquerda
  // 213, Maior subida da asa esquerda
  // 1997 Velocidade aumenta quando asa desce
  // 1993 Velocidade diminui quando asa sobe
  // É feita uma proporção dos valores
  //------------------------------------------------
  // val1 val2 - variavel que recebe valor de proporção resultante
  // entre os valores do acelerometro e velocidade do motor
  //-----------------------------------------------------
  // Melhor valor até agora
  //  val1 = map (yRaw, -20,100,1596,1592);//Esquerdo
  //   val2 = map (yRaw, 40,213,1594,1598);//Direito
  // Esquerdo
  
           val1 = map (filter_teta, -90,90,1597,1594);
  // A linha abaixo manda o pulso para o motor esquerdo
  if (val1 > 1601)
    {
      val1 = 1600;
    }
           Motor01.write(val1);
  //--------------------------------
  // Execução Motor Direito
  //--------------------------------
  // yRaw eixo Y do acelerômetro
  // 40, menor valor de descida da asa Direita
  // 213, Maior subida da asa Direita
  // 1996 Velocidade aumenta quando asa desce
  // 1993 Velocidade diminui quando asa sobe
  // É feita uma proporção dos valores
//-------------------------------------------------
          val2 = map (filter_teta, -90,90,1594,1600);
  // A linha abaixo manda o pulso para o motor direito
        if (val2 > 1601)
    {
      val2 = 1600;
    }
 
 
          Motor02.write(val2);
 //---------------------------------------------------------

   Serial.print(w); // Angulo Gyro
   Serial.print(",");
   Serial.print(pitch);// Angulo Acelerometro
   Serial.print(",");
   Serial.print(filter_teta);// Angulo com filtro
   Serial.print(",");
//----Informações de Dados - Motores -----------------------
   Serial.print (val1);//Velocidade Motor Esq
   Serial.print (",");
   Serial.print (val2);//Velocidade Motor Dir
   Serial.print (",");
   Serial.println();
 
 
}


void zera(){
  digitalWrite(ledPin,HIGH);
  error=0;
  filter_teta=0;
  output=0;
  last_error=0;
  teta=0;
  w=0;
  Serial.println("Variaveis zeradas");
  delay(500);
}

void set_offset(){
  delay(500);
  offsetX=countx;
  offsetY=county;
  offsetZ=countz;
  offsetW=analogRead(3);
  Serial.println("Novos offsets setados");
}

 

 

 

 

 

 

Exibições: 14153

Responder esta

Respostas a este tópico

Opa Rafael.

Conseguiu algum avanço com o filtro kalman? To tentando estabilizar um giroscópio, porem ele fica incrementando um erro, mesmo ele estando parado, o sistema diz que esta ocorrendo um leve giro, giro com velocidades diferentes em todos os  3 eixos. Pelo que pesquisei vou ter que partir para o filtro kalman, mais ele é bem complexo e chato de entender. Implementou alguma coisa?

Olá Jucelei

 Vejo que este tópico é antigo para os nossos padroes daqui, mas gostaria de saber o q vc fez neste sentido, adquiri um gyro de 270 g/s (MEMS), automotivo,  recentemente e logo pretendo começar os testes,

gostaria de saber o q já fez  para melhorar o drift. Já tentou um filtro complementar ?

Abços

Joe.

Até testei o filtro complementar, ele funciona sim. Só tem problema para estatizar o eixo Z (yaw), já que o acelerômetro não  detecta o giro em torno desse eixo.

Mais a idéia seria:

Eixo X e Y, giroscópio+acelerômetro.

Eixo Z ,giroscópio+GPS(angulo do gps -alta velocidade) || magnetômetro (angulo baixa velocidade).

Ainda voltarei a mexer com isso, o próximo passo seria adquirir a bussola digital, já que recentemente adquiri um GPS e um SIM900 para complementar.

:)

Olá Jucelei

Consegui ler o yaw com um acelerometro analógico de tres eixos, utilizando-o como clinometro, medindo a aceleração estática do eixo gravitacional, não resolveu o meu problema por excesso de ruído e falta de linearidade acima de 10º, experimentei acelerometros de baixo custo, inclusive usando Kalman, não consegui resolução melhor do que 1º.

Aqui neste tópico tem um AN de como calcula-lo,se seu caso for o medir angulo do movimento curvo de um aeromodelo acredito que de certo.

http://labdegaragem.com/group/matematica-com-arduino

IMU para uso tático utilizam 3 gyros,  clinometros de 3 eixos e magnetometros de 3 eixos tb.

E muitos cálculos trigonométricos envolvendo transformações, rotação e translação de coordenadas, ótimo para quem gosta de resolver matrizes

Matemática ai pro nosso colega Almir Filho.

Abços

Estou pensando nestas questões para produzir um controle inercial.

pretendo usar:

-2 inclinometros;

-1 magnetometro (norte);

-1 giroscopio;

-gps (opcional para velocidade superior a 350m/s);

(controle de "veiculo" anti aereo).

uma dica, up o código no github acho que fica mais tranquilo do pessoal ajudar uma vez que o código ficaria mais legível.

RSS

© 2024   Criado por Marcelo Rodrigues.   Ativado por

Badges  |  Relatar um incidente  |  Termos de serviço