Paso 4: Paso 4: el código
Subo el código en adjuntar archivo
#include
#include
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include
pantalla LCD
Lcd de LiquidCrystal_I2C (0x27, 2, 1, 0, 4, 5, 6, 7, 3, positivo);
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#define LOG_INPUT 0
#define MANUAL_TUNING 0
#define LOG_PID_CONSTANTS 1 //MANUAL_TUNING debe ser 1
#define MOVE_BACK_FORTH 0
#define MIN_ABS_SPEED 30
MPU
MPU6050 mpu;
Vars de control estado MPU
bool dmpReady = false; sistema true si init DMP fue exitosa
mpuIntStatus de uint8_t; contiene el byte de estado de interrupción real de la MPU
devStatus de uint8_t; volver estado después de cada operación de dispositivo (0 = éxito,! 0 = error)
packetSize uint16_t; espera que el tamaño del paquete de DMP (por defecto es de 42 bytes)
fifoCount de uint16_t; cuenta de bytes todos actualmente en FIFO
fifoBuffer de uint8_t [64]; Buffer de almacenamiento FIFO
orientación del movimiento vars
Cuaternión q; [w, x, y, z] contenedor cuaternión
VectorFloat de gravedad; [x, y, z] vector de gravedad
flotador ypr [3]; [yaw, pitch, roll] vector contenedor y la gravedad del desvío/pitch/roll
PID
#if MANUAL_TUNING
doble kp, ki, kd;
doble prevKp, prevKi, prevKd;
#endif
doble originalSetpoint = 174.29;
doble setpoint = originalSetpoint;
doble movingAngleOffset = 0,3;
doble entrada, salida;
int moveState = 0; 0 = saldo; 1 = espalda; 2 = adelante
int LED = 13;
int LED2 = 10;
#if MANUAL_TUNING
PID pid (& entrada, salida y punto de referencia, 0, 0, 0, directos);
#else
PID pid (& entrada, salida y punto de referencia, 230, 300, 2.9, directos);
#endif
CONTROLADOR DE MOTOR
int ENA = 3;
int 1 = 4;
int IN2 = 8;
int IN3 = 5;
int IN4 = 7;
int ENB = 6;
LMotorController motorController (ENA, IN1, IN2, ENB, IN3, IN4, 0,6, 1);
Contadores de tiempo
time1Hz largo = 0;
time5Hz largo = 0;
volátiles bool mpuInterrupt = false; indica si el pin de interrupción MPU ha ido alta
void dmpDataReady()
{
mpuInterrupt = true;
}
void setup()
{
Únete a bus I2C (I2Cdev biblioteca no esta automáticamente)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.Begin();
TWBR = 24; reloj de I2C 400KHz (200kHz si el CPU es de 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::Setup (200, true);
#endif
pinMode(LED,OUTPUT);
digitalWrite(LED,HIGH);
pinMode(LED2,OUTPUT);
digitalWrite(LED2,HIGH);
inicializar la comunicación serial
(115200 elegido porque se requiere para la salida de la Demo de tetera, pero tiene
realmente depende de ti dependiendo de tu proyecto)
Serial.Begin(115200);
LCD.Begin(20,4); inicializar el lcd de 20 caracteres 4 líneas y encender luz de fondo
---Rápidas 3 parpadeos de luz---
para (int i = 0; i < 4; i ++)
{
LCD.Backlight();
Delay(250);
lcd.noBacklight();
Delay(250);
}
LCD.Backlight(); acabado con luz de fondo
---Escribe caracteres en la pantalla---
Nota: Posición del Cursor: CHAR, línea) desde 0
lcd.setCursor(3,0); Desde caracter 4 en línea 0
LCD.Print ("Hola, mundo!");
Delay(1000);
lcd.setCursor(2,1);
LCD.Print ("mi nombre KHALID");
Delay(1000);
lcd.setCursor(0,2);
LCD.Print ("Self Balancing ROBOT");
lcd.setCursor(0,3);
Delay(700);
LCD.Print("YouTube:CAYMANGUY123");
Delay(2000);
mientras (!. Serie); esperar para la enumeración de Leonardo, otros siguen inmediatamente
inicializar el dispositivo
Serial.println (F ("inicializar I2C dispositivos..."));
lcd.setCursor(0,0); Desde caracter 4 en línea 0
LCD.Print F ("inicializar I2C");
Delay(1000);
MPU.Initialize();
verificar conexión
Serial.println (F ("prueba dispositivo conexiones..."));
lcd.setCursor(0,1); Desde caracter 4 en línea 0
LCD.Print F ("dispositivos de prueba");
Delay(1000);
¿Serial.println(MPU.testconnection()? F("MPU6050 Connection successful"): F ("MPU6050 fallada la conexión"));
lcd.setCursor(0,3);
¿LCD.println(MPU.testconnection()? F("MPU6050 Connection"): F ("MPU6050 fallada la conexión"));
cargar y configurar la DMP
Serial.println (F ("inicializar DMP..."));
devStatus = mpu.dmpInitialize();
fuente de su propio giro desplaza aquí, escala mínima sensibilidad
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(98);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); 1688 de fábrica para mi chip de prueba
Asegúrese de que funcionaba (devuelve 0 si es así)
Si (devStatus == 0)
{
Encienda el DMP, ahora que está listo
Serial.println (F ("habilitar DMP..."));
mpu.setDMPEnabled(true);
habilitar la detección de interrupción de Arduino
Serial.println (F ("habilitar interrupción detección (interrupción externa 0 de Arduino)..."));
attachInterrupt (0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
establecer nuestra bandera DMP listo para que la función loop() principal sepa que está bien usarlo
Serial.println (F ("DMP listo! Esperando primera interrupción..."));
dmpReady = true;
Haz esperado tamaño de paquete DMP para posterior comparación
packetSize = mpu.dmpGetFIFOPacketSize();
configuración PID
PID. SetMode(AUTOMATIC);
PID. SetSampleTime(10);
PID. SetOutputLimits (-255, 255);
}
otra cosa
{
¡ ERROR!
1 = carga inicial de la memoria no se pudo
2 = actualizaciones de configuración DMP no pudiera
(si se va a romper, generalmente el código será 1)
Serial.Print (F ("error de inicialización de DMP (código"));
Serial.Print(devStatus);
Serial.println(F(")"));
}
}
void loop()
{
Si no de programación, no intente hacer nada
Si (! dmpReady) volver;
espera interrupción MPU o paquetes adicionales disponibles
mientras (! mpuInterrupt & & fifoCount < packetSize)
{
no hay datos de mpu - realizar cálculos de PID y la salida a motores
pid.Compute();
motorController.move (salida, MIN_ABS_SPEED);
unsigned currentMillis largo = millis();
Si (currentMillis - time1Hz > = 1000)
{
loopAt1Hz();
time1Hz = currentMillis;
}
Si (currentMillis - time5Hz > = 5000)
{
loopAt5Hz();
time5Hz = currentMillis;
}
}
Bandera de interrupción de RESET y conseguir byte INT_STATUS
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
Haz recuento actual de FIFO
fifoCount = mpu.getFIFOCount();
verificación de desbordamiento (esto nunca ocurre a menos que nuestro código es demasiado ineficiente)
Si ((mpuIntStatus & 0x10) || fifoCount == 1024)
{
reiniciar para que podamos seguir limpiamente
mpu.resetFIFO();
Serial.println (F ("desbordamiento de la FIFO!"));
en caso contrario, Compruebe la interrupción listo datos de DMP (esto ocurre con frecuencia)
}
else if (mpuIntStatus & 0 x 02)
{
espere para la longitud correcta de los datos disponibles, debe ser muy breve espera
mientras que (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
leer un paquete de FIFO
mpu.getFIFOBytes (fifoBuffer, packetSize);
pista aquí cuenta de FIFO en caso de que se dispone de > 1 paquete
(esto nos permite más inmediatamente sin esperar una interrupción)
fifoCount = packetSize;
mpu.dmpGetQuaternion (& q, fifoBuffer);
mpu.dmpGetGravity (gravedad & q);
mpu.dmpGetYawPitchRoll (ypr & q y gravedad);
#if LOG_INPUT
Serial.Print("ypr\t");
Serial.Print (ypr [0] * 180/constantio);
Serial.Print("\t");
Serial.Print (ypr [1] * 180/constantio);
Serial.Print("\t");
Serial.println (ypr [2] * 180/constantio);
#endif
entrada = ypr [1] * 180/constantio + 180;
}
}
void loopAt1Hz()
{
#if MANUAL_TUNING
setPIDTuningValues();
#endif
}
void loopAt5Hz()
{
#if MOVE_BACK_FORTH
moveBackForth();
#endif
}
mueva hacia adelante y hacia atrás
void moveBackForth()
{
moveState ++;
Si moveState (moveState > 2) = 0;
Si (moveState == 0)
valor = originalSetpoint;
else if (moveState == 1)
punto de ajuste = originalSetpoint - movingAngleOffset;
otra cosa
punto de ajuste = originalSetpoint + movingAngleOffset;
}
PID Sintonía (3 potenciómetros)
#if MANUAL_TUNING
void setPIDTuningValues()
{
readPIDTuningValues();
Si (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 = mapa (potKp, 0, 1023, 0, 25000) / 100.0; 0 - 250
Ki = mapa (potKi, 0, 1023, 0, 100000) / 100.0; 0 - 1000
KD = mapa (potKd, 0, 1023, 0, 500) / 100.0; 0 - 5
}
#endif
