collapse

* Posts Recentes

Amplificador - Rockboard HA 1 In-Ear por almamater
[27 de Março de 2024, 19:13]


O que é isto ? por KammutierSpule
[26 de Março de 2024, 19:35]


Bateria - Portátil por almamater
[25 de Março de 2024, 22:14]


Emulador NES em ESP32 por dropes
[13 de Março de 2024, 21:19]


Escolher Osciloscópio por jm_araujo
[06 de Fevereiro de 2024, 23:07]


TP4056 - Dúvida por dropes
[31 de Janeiro de 2024, 14:13]


Leitura de dados por Porta Serie por jm_araujo
[22 de Janeiro de 2024, 14:00]


Distancia Cabo por jm_araujo
[08 de Janeiro de 2024, 16:30]


Meu novo robô por josecarlos
[06 de Janeiro de 2024, 16:46]


Laser Engraver - Alguém tem? por almamater
[16 de Dezembro de 2023, 14:23]

Autor Tópico: FlyeRobot - Equilíbrio Automático  (Lida 27129 vezes)

0 Membros e 1 Visitante estão a ver este tópico.

Offline Nunito

  • Mini Robot
  • *
  • Mensagens: 923
Re: FlyeRobot - Equilíbrio Automático
« Responder #30 em: 20 de Outubro de 2014, 10:41 »
Bom dia.
Como poderei eu colocar os motores a acelerar mais ou menos conforme a inclinação do MPU6050.
Alguém para me dar umas dicas, estou com alguma dificuldade em perceber.

Cumprimentos, Nuno

Offline jm_araujo

  • Mini Robot
  • *
  • Mensagens: 2.947
  • NERD!
Re: FlyeRobot - Equilíbrio Automático
« Responder #31 em: 20 de Outubro de 2014, 10:58 »
Com um driver com com controlo Step/Dir conseguias usando só um timer.

Com o LM298 eu faria da seguinte maneira:
  • Primeiro não me preocupava para já em desligar o motor entre passos, primeiro põe-no a andar como queres, podes ir controlando se aquece demais à mão e quando tiveres a funcionar tratas disso (se necessário)
  • Uma função que dá um passo no motor. Aceita como único parâmetro a direção da rotação. Guarda o estado atual das saídas do LM298 e salta para a seguinte (ou anterior) por forma a dar o passo
  • Um interrupt controlado por um timer que chama a tal função. A direção pode ser uma var global alterada pela função que interpreta os dados do MPU6050. Se só tiveres timers "rapidos", podes implementar um divisor extra (um acumulador) que só chama a cada N interrupções
  • Na função que trata os dados do MPU conforme o valor lido altera a frequência do timer ligado ao interrupt conforme a velocidade pretendida e a direção.

Acho que chega para começar.

Offline Nunito

  • Mini Robot
  • *
  • Mensagens: 923
Re: FlyeRobot - Equilíbrio Automático
« Responder #32 em: 20 de Outubro de 2014, 13:34 »
Com um driver com com controlo Step/Dir conseguias usando só um timer.

Com o LM298 eu faria da seguinte maneira:
  • Primeiro não me preocupava para já em desligar o motor entre passos, primeiro põe-no a andar como queres, podes ir controlando se aquece demais à mão e quando tiveres a funcionar tratas disso (se necessário)
  • Uma função que dá um passo no motor. Aceita como único parâmetro a direção da rotação. Guarda o estado atual das saídas do LM298 e salta para a seguinte (ou anterior) por forma a dar o passo
  • Um interrupt controlado por um timer que chama a tal função. A direção pode ser uma var global alterada pela função que interpreta os dados do MPU6050. Se só tiveres timers "rapidos", podes implementar um divisor extra (um acumulador) que só chama a cada N interrupções
  • Na função que trata os dados do MPU conforme o valor lido altera a frequência do timer ligado ao interrupt conforme a velocidade pretendida e a direção.

Acho que chega para começar.
Obrigado pelas dicas, eu agora deixei os motores de passo de lado e estou a efectuar os testes com dois motores DC com engrenagens.
Acho que assim consigo entender melhor a evolução da programação.

Offline jm_araujo

  • Mini Robot
  • *
  • Mensagens: 2.947
  • NERD!
Re: FlyeRobot - Equilíbrio Automático
« Responder #33 em: 20 de Outubro de 2014, 14:19 »
Motores DC é mais simples.
Controlas diretamente por PWM. No Arduino tens o analogWrite que trata de tudo.

Offline Nunito

  • Mini Robot
  • *
  • Mensagens: 923
Re: FlyeRobot - Equilíbrio Automático
« Responder #34 em: 22 de Outubro de 2014, 18:27 »
Boa tarde.

Estou em fases de testes com a programação, aparecem-me muitas vezes um erro " FIFO Overflow"
Alguém me sabe dizer o porquê disto acontecer, a minha ideia é que deverá ser o arduino Nano v3 que estou a utilizar, que não consegue responder.
Está a funcionar bem ao inclinar o MPU6050, mas por vezes dá este erro e o L298 começa a passar-se, recebe impulsos nos Inputs todos.
Estou a carregar um video para mostrar.
« Última modificação: 22 de Outubro de 2014, 19:33 por Nunito »

Offline metRo_

  • Administrator
  • Mini Robot
  • *****
  • Mensagens: 3.753
Re: FlyeRobot - Equilíbrio Automático
« Responder #35 em: 22 de Outubro de 2014, 19:02 »
Leste a datasheet do sensor?

Offline Nunito

  • Mini Robot
  • *
  • Mensagens: 923
Re: FlyeRobot - Equilíbrio Automático
« Responder #36 em: 22 de Outubro de 2014, 19:32 »
Leste a datasheet do sensor?
Vi que é o nome de uma interrupção e um registo de 1024 bytes.
Sinceramente nunca estudei acerca de interrupções, daí as minhas dificuldades.

Offline metRo_

  • Administrator
  • Mini Robot
  • *****
  • Mensagens: 3.753
Re: FlyeRobot - Equilíbrio Automático
« Responder #37 em: 22 de Outubro de 2014, 20:29 »
Do que me lembro muitos desses sensores gravam os valores numa FIFO onde tu depois tens que ir ler. Deves ter isso configurado para manter os valores até serem lidos. Neste caso o ideal é configurares isso para ele escrever por cima quando tiver cheio assim evitas esse erro e tens o valor da posição mais actual. Isto supondo o que erro que estás a ter é vindo do sensor!

Offline Nunito

  • Mini Robot
  • *
  • Mensagens: 923
Re: FlyeRobot - Equilíbrio Automático
« Responder #38 em: 22 de Outubro de 2014, 21:04 »
Citar
    if (!dmpReady) return;
    while (!mpuInterrupt && fifoCount < packetSize)
    {
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    fifoCount = mpu.getFIFOCount(); Verifica FIFO overflow

    if ((mpuIntStatus & 0x10) || fifoCount == 1024)
    {
        mpu.resetFIFO();
        Serial.println(F("FIFO overflow!));
    }
    else if (mpuIntStatus & 0x02)
    {
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        fifoCount -= packetSize;
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);


Offline senso

  • Global Moderator
  • Mini Robot
  • *****
  • Mensagens: 9.733
  • Helpdesk do sitio
Re: FlyeRobot - Equilíbrio Automático
« Responder #39 em: 22 de Outubro de 2014, 23:04 »
Não esquecer que o Arduino Nano só tem 2Kb de RAM, se os pinos se passam provavelmente para além de problemas no sensor deves estar a rebentar pela stack fora devido a uso excessivo de ram.
Avr fanboy

Offline Nunito

  • Mini Robot
  • *
  • Mensagens: 923
Re: FlyeRobot - Equilíbrio Automático
« Responder #40 em: 22 de Outubro de 2014, 23:15 »
Não esquecer que o Arduino Nano só tem 2Kb de RAM, se os pinos se passam provavelmente para além de problemas no sensor deves estar a rebentar pela stack fora devido a uso excessivo de ram.

Pois, vou testar com o UNO.
Obrigado pela dica.
« Última modificação: 22 de Outubro de 2014, 23:18 por Nunito »

Offline senso

  • Global Moderator
  • Mini Robot
  • *****
  • Mensagens: 9.733
  • Helpdesk do sitio
Re: FlyeRobot - Equilíbrio Automático
« Responder #41 em: 22 de Outubro de 2014, 23:26 »
É exactamente o mesmo chip, vá o uno é um atmega32u4 que tem mais 512 bytes de RAM, mas são usados para o USB, por isso é muito relativo essa ram extra.

Não podes postar o código todo?
E já agora compila o código carregado no shift antes de pressionar no botão de compilar, dá-te o uso da flash e ram(estática, chamadas a funções e variaveis/arrays não globais não são contabilizados).

Sem esquecer que só usar serial são logo 128bytes que vão ao ar, se bem me recordo isso usa buffers de send e receive de 64 bytes para cada lado, biblioteca i2c vai usar mais, depois as bibliotecas para arduino é variaveis com fartura para tudo e mais alguma coisa, com muitas cópias locais da mesma coisa.
Avr fanboy

Offline Nunito

  • Mini Robot
  • *
  • Mensagens: 923
Re: FlyeRobot - Equilíbrio Automático
« Responder #42 em: 22 de Outubro de 2014, 23:31 »
É exactamente o mesmo chip, vá o uno é um atmega32u4 que tem mais 512 bytes de RAM, mas são usados para o USB, por isso é muito relativo essa ram extra.

Não podes postar o código todo?
E já agora compila o código carregado no shift antes de pressionar no botão de compilar, dá-te o uso da flash e ram(estática, chamadas a funções e variaveis/arrays não globais não são contabilizados).

Sem esquecer que só usar serial são logo 128bytes que vão ao ar, se bem me recordo isso usa buffers de send e receive de 64 bytes para cada lado, biblioteca i2c vai usar mais, depois as bibliotecas para arduino é variaveis com fartura para tudo e mais alguma coisa, com muitas cópias locais da mesma coisa.
Pois só dei conta quando testei agora com o UNO, o UNO tem o mesmo chip  :) , dá os mesmos erros.
Não queria estar a colcoar o código todo para não confundir a malta aqui no tópico.
Essa do shift aprendi agora  :)
Não me deixa complar com o shift apertado, diz que não encontra nenhuma porta USB.

Citar
#include <PID_v1.h>
#include <LMotorController.h>
#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif


#define LOG_INPUT 0
#define MANUAL_TUNING 0
#define LOG_PID_CONSTANTS 0 //MANUAL_TUNING must be 1
#define MOVE_BACK_FORTH 0

#define MIN_ABS_SPEED 30

//MPU
MPU6050 mpu;

//#define LED_PIN 13

// MPU control/status vars
bool dmpReady = false// set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q; // [w, x, y, z]         quaternion container
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector


//PID


#if MANUAL_TUNING
  double kp , ki, kd;
  double prevKp, prevKi, prevKd;
#endif
double originalSetpoint = 174.29;
double setpoint = originalSetpoint;
double movingAngleOffset = 0.3;
double input, output;
int moveState=0; //0 = balance; 1 = back; 2 = forth

#if MANUAL_TUNING
  PID pid(&input, &output, &setpoint, 0, 0, 0, DIRECT);
#else
  PID pid(&input, &output, &setpoint, 70, 240, 2, DIRECT); // originais 70, 240, 1.9
#endif


//MOTOR CONTROLLER


int ENA = 6;
int ENB = 7;
int IN1 = 8;
int IN2 = 9;
int IN3 = 10;
int IN4 = 11;


LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, 0.6, 1);


//timers


long time1Hz = 0;
long time5Hz = 0;


volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady()
{
    mpuInterrupt = true;
}


void setup()
{
    // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        TWBR = 12; // 400kHz I2C clock (200kHz if CPU is 8MHz) original 24
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(200, true); // original 400
    #endif

    // initialize serial communication
    // (115200 chosen because it is required for Teapot Demo output, but it's
    // really up to you depending on your project)
    Serial.begin(57600);
    while (!Serial); // wait for Leonardo enumeration, others continue immediately

    // initialize device
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();

    // verify connection
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    // load and configure the DMP
    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

    // make sure it worked (returns 0 if so)
    if (devStatus == 0)
    {
        // turn on the DMP, now that it's ready
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
       
        //setup PID
       
        pid.SetMode(AUTOMATIC);
        pid.SetSampleTime(10);
        pid.SetOutputLimits(-255, 255); 
    }
    else
    {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }
        // configure LED for output
        // pinMode(LED_PIN, OUTPUT);
}


void loop()
{
    // if programming failed, don't try to do anything
    if (!dmpReady) return;

    // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize)
    {
        //no mpu data - performing PID calculations and output to motors
       
        pid.Compute();
        motorController.move(output, MIN_ABS_SPEED);
       
        unsigned long currentMillis = millis();

        if (currentMillis - time1Hz >= 1000)
        {
            loopAt1Hz();
            time1Hz = currentMillis;
        }
       
        if (currentMillis - time5Hz >= 5000)
        {
            loopAt5Hz();
            time5Hz = currentMillis;
        }
    }

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();

    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024)
    {
        // reset so we can continue cleanly
        mpu.resetFIFO();
        Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    }
    else if (mpuIntStatus & 0x02)
    {
        // wait for correct available data length, should be a VERY short wait
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
       
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;


        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
        #if LOG_INPUT
            Serial.print("ypr\t");
            Serial.print(ypr[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(ypr[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(ypr[2] * 180/M_PI);
        #endif
        input = ypr[1] * 180/M_PI + 180;
   }
}


void loopAt1Hz()
{
#if MANUAL_TUNING
    setPIDTuningValues();
#endif
}


void loopAt5Hz()
{
    #if MOVE_BACK_FORTH
        moveBackForth();
    #endif
}


//move back and forth


void moveBackForth()
{
    moveState++;
    if (moveState > 2) moveState = 0;
   
    if (moveState == 0)
      setpoint = originalSetpoint;
    else if (moveState == 1)
      setpoint = originalSetpoint - movingAngleOffset;
    else
      setpoint = originalSetpoint + movingAngleOffset;
}


//PID Tuning (3 potentiometers)

#if MANUAL_TUNING
void setPIDTuningValues()
{
    readPIDTuningValues();
   
    if (kp != prevKp || ki != prevKi || kd != prevKd)
    {
#if LOG_PID_CONSTANTS
        Serial.print(kp);Serial.print(", ");Serial.print(ki);Serial.print(", ");Serial.println(kd);
#endif

        pid.SetTunings(kp, ki, kd);
        prevKp = kp; prevKi = ki; prevKd = kd;
    }
}


void readPIDTuningValues()
{
    int potKp = analogRead(A0);
    int potKi = analogRead(A1);
    int potKd = analogRead(A2);
       
    kp = map(potKp, 0, 1023, 0, 25000) / 100.0; //0 - 250
    ki = map(potKi, 0, 1023, 0, 100000) / 100.0; //0 - 1000
    kd = map(potKd, 0, 1023, 0, 500) / 100.0; //0 - 5
}
#endif

« Última modificação: 22 de Outubro de 2014, 23:33 por Nunito »

Offline senso

  • Global Moderator
  • Mini Robot
  • *****
  • Mensagens: 9.733
  • Helpdesk do sitio
Re: FlyeRobot - Equilíbrio Automático
« Responder #43 em: 23 de Outubro de 2014, 00:02 »
Tens de ter a placa ligada porque depois de compilar ele tenta fazer o upload.
Avr fanboy

Offline Nunito

  • Mini Robot
  • *
  • Mensagens: 923
Re: FlyeRobot - Equilíbrio Automático
« Responder #44 em: 23 de Outubro de 2014, 12:43 »
Tens de ter a placa ligada porque depois de compilar ele tenta fazer o upload.
Não me está a deixar compilar na mesma.