Gracias por la respuesta.
Sí, efectivamente. Cuándo comento que no uso interrupciones, me refiero a los encoders de cuadratura. Me explico:
Te adjunto en este Post el código del robot autobalanceado sin la parte del código específica para los encoders. Es decir, cableando adecuadamente el robot con unos motores DC cualesquiera y sintonizando correctamente las constantes del PID (kp, ki, kd) el robot se mantendría erguido con el código que aquí he colgado.
En otras palabras, este código que aquí adjunto solo lee los datos del sensor de ángulo MPU 6050 y los vuelca en el PID. Ahora bien, si quisiéramos leer los datos del encoder hall de cuadratura mediante interrupciones, produciríamos conflictos con el MPU, dejando éste de tomar datos. El código se volvería ineficiente mostrando la advertencia de "FIFO overflow!".
En definitiva quiero fusionar los dos códigos que he colgado de manera que al final el SetPoint de este PID: "PID myPID(&Input, &U, &Setpoint, kp, ki, kd, DIRECT);" sea constantemente modificado por un segundo PID por ejemplo "PID myPID(&rpm, &nuevoSetPoint, 0, kp2, ki2, kd2, DIRECT);". La manera de hacerlo sería modificando el ángulo del primer PID de esta forma: Setpoint = 0º - nuevoSetPoint.
¿Porqué un robot autobalanceado nunca puede estar completamente parado con un sólo PID? Esto se debe a muchas razones, una es que es imposible establecer exactamente el ángulo de equilibrio perfecto. Luego los motores con reducción tienen un cierto juego, luego está evidentemente las pequeñísimas imperfecciones del suelo o fuerzas externas que pudieran existir (el aire...), etc.
Por eso es necesario tener un segundo PID que modifique el ángulo objetivo o SetPoint del primero. El SetPoint del segundo PID sería velocidad de los motores = 0.
En definitiva y si lo que pienso es correcto, si consiguiera sacar las rpm del if, creo que podría conseguirlo.
Gracias nuevamente!
#include <PID_v1.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
bool blinkState = false;
// 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
// packet structure for InvenSense teapot demo
//uint8_t teapotPacket[14] = { '
, 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
//Motor RH. Variables para los motores DC y módulo controlador L298N
int IN2 = 5;
int IN1 = 4;
int ENA = 3;
//motor LH. Variables para los motores DC y módulo controlador L298N
int IN3 = 11;
int IN4 = 10;
int ENB = 9;
// Variables PID
double U, Setpoint = 6.0, Input; // nota: U = Output, es decir lo que controla el motor. Para mi, 6º es el equilibrio.
double kp = 140, ki = 15, kd = 90; //valores sintonizados manualmente
//Specify the links and initial tuning parameters
PID myPID(&Input, &U, &Setpoint, kp, ki, kd, DIRECT);
void setup()
{
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
//Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
Serial.begin(115200);
mpu.initialize();
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(-855);
mpu.setYGyroOffset(-251);
mpu.setZGyroOffset(-259);
mpu.setZAccelOffset(1005); // 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
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
myPID.SetOutputLimits(-255, 255); (mínimo 255 y máximo 255)
myPID.SetSampleTime(5);
myPID.SetMode(AUTOMATIC);
} 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(")"));
}
// Motores
pinMode(IN1, OUTPUT); //Motor RH. Salida para hacia el módulo controlador L298N
pinMode(IN2, OUTPUT); //Motor RH. Salida para hacia el módulo controlador L298N
pinMode(ENA, OUTPUT); //Motor RH. Salida para hacia el módulo controlador L298N
pinMode(IN3, OUTPUT); //Motor LH. Salida para hacia el módulo controlador L298N
pinMode(IN4, OUTPUT); //Motor LH. Salida para hacia el módulo controlador L298N
pinMode(ENB, OUTPUT); //Motor LH. Salida para hacia el módulo controlador L298N
//Ajuste de las variables con potenciómetro
pinMode(A0, INPUT);
pinMode(A1, INPUT);
pinMode(A2, INPUT);
}
void loop()
{
if (!dmpReady) return;
while (!mpuInterrupt && fifoCount < packetSize) {
//Arrancamos la libería del PID
myPID.Compute();
}
// 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;
//display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Input = ypr[2] * 180 / M_PI; (este es el ángulo que lee el MPU 6050)
}
if (U == 0) {
digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);
analogWrite(ENA, 0);
digitalWrite(IN3, LOW);
digitalWrite(IN4, LOW);
analogWrite(ENB, 0);
}
if (U > 0) {
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
analogWrite(ENA, U);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
analogWrite(ENB, U);
}
if (U < 0) {
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
analogWrite(ENA, -U);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
analogWrite(ENB, -U);
}
}
o utilizar un segundo PID que modifique el SetPoint del primero, buscando siempre dos objetivos el ángulo 0 (o el deseado) y velocidad 0. Quizás hay otra solución, pero la desconozco.