In diesem Projekt wird ein Beschleunigungssensor verwendet, um einen Arduino auf zwei Rädern auszubalancieren.
Es wird im Video Schritt für Schritt gezeigt, wie dieser 2Rad-Arduino das laufen lernte.
Neben den schon vorgestellten Bauteilen für das Bluetooth gesteuerte Fahrzeug, kommt nun der Beschleunigungssensor MPU6050 zusätzlich zum Einsatz.
Er enthält sowohl einen Beschleunigungsmesser als auch einen Gyroskop und kann daher die Beschleunigung und die Drehung eines Objekts in drei Dimensionen messen.
Der MPU6050 verwendet einen mikroelektromechanischen System(MEMS)-Chip, um die Bewegung des Sensors zu messen. Der Beschleunigungsmesser misst die Veränderungen der Bewegungsgeschwindigkeit, während das Gyroskop die Änderungen der Orientierung des Sensors im Raum erfasst. Die gemessenen Daten werden dann durch einen eingebetteten Prozessor im Sensor verarbeitet, um die genauen Werte für die Beschleunigung und Drehung in allen drei Dimensionen zu berechnen.
Dank integriertem “Digital Motion Processor” (DMP) können komplexe Berechnungen auch innerhalb des Chips ausgeführt werden, um den Mikrocontroller zu entlasten.
Der MPU6050 verfügt auch über einen integrierten Temperatursensor, der die Temperatur des Sensors misst und zur Kompensation von Messfehlern verwendet werden kann.
Der Sensor kann über eine I2C-Schnittstelle an Mikrocontroller oder andere digitale Geräte angeschlossen werden und ist für viele Anwendungen geeignet, wie beispielsweise in der Robotik, Drohnensteuerung, Video- und Bildstabilisierung und in der Spiele- oder VR-Technologie.
Im regungslosen Zustand misst der Beschleunigungssensor des MPU6050 die Erdbeschleunigung auch im Ruhezustand (in z-Richtung, wenn das Modul flach liegt) . Dies bedeutet, dass der Sensor die Schwerkraft der Erde erkennt und daher die z-Achse nicht Null ist. Stattdessen würde es ungefähr 1g anzeigen, was der Erdbeschleunigung entspricht.
Datenblatt:
MPU6000/6050
Hardware:
Arduino Nano
Getriebemotor (3-6V) inkl. Rad
MotoDriver2
Modul GY-521 mit dem MPU-6050 Chip
Hardware (optional):
Arduino Nano Expansion Shield
Bibliotheken:
PID von https://github.com/br3ttb/
LMotorController von https://github.com/lukagabric/
I2Cdev und MPU6050 von https://github.com/jrowberg/
Verdrahtung:
Quellcode:
Den Test Quellcode für Motoren findest du hier: Bluetooth gesteuerte Fahrzeug.
Für den Beschleunigungssensor-Test den Demo-Sketch „basic_readings“ aus der Library „Adafruit MPU6050“ verwenden.
Finaler Sketch:
#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 MIN_ABS_SPEED 30
MPU6050 mpu;
// MPU control/status vars
bool dmpReady = false; // true setzen, wenn DMP-Init erfolgreich war
uint8_t mpuIntStatus; // enthält das aktuelle Interrupt-Statusbyte von der MPU
uint8_t devStatus; // Rückgabestatus nach jedem Gerätevorgang (0 = success, !0 = error)
uint16_t packetSize; // voraussichtliche DMP-Paketgröße (standard ist 42 bytes)
uint16_t fifoCount; // Anzahl aller Bytes, im FIFO
uint8_t fifoBuffer[64]; // FIFO Speicherpuffer
// DMP = Digital Motion Processor
// 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
double originalSetpoint = 180; //kippt zu weit nach vorne/hinten
double setpoint = originalSetpoint;
double movingAngleOffset = 0.1;
double input, output;
//Passen diese Werte an dein eigenes Fahrzeug an
double Kp = 40; //Korrektur Grad - zu niedrig es fällt um / zu hoch es ist wild am zappeln
double Kd = 2.2; // Ist Kp ok, dann kann hier die Schwingungen verringert werden bis Fahrzeug fast stabil ist
double Ki = 270; // Zeit für Stabilisierung
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);
double motorSpeedFactorLeft = 0.6;
double motorSpeedFactorRight = 0.5;
//MOTOR CONTROLLER
int ENA = 5;
int IN1 = 6;
int IN2 = 7;
int IN3 = 9;
int IN4 = 8;
int ENB = 10;
LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, motorSpeedFactorLeft, motorSpeedFactorRight);
volatile bool mpuInterrupt = false; // zeigt an, ob der MPU-Interrupt-Pin auf High gegangen ist
void dmpDataReady()
{
mpuInterrupt = true;
}
void setup()
{
// join I2C bus (I2Cdev library macht das nicht automatisch)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24; // 400kHz I2C clock (da der Nano 16 MHz hat. Wenn zB 8MHz dann auf 200kHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
mpu.initialize();
devStatus = mpu.dmpInitialize();
// Gebe hier deinen eigenen Kreisel-Offsets ein, skaliert für die Mindestempfindlichkeit
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default
// Wenn 0 dann hat es funktioniert
if (devStatus == 0)
{
// Schaltet DMP ein, jetzt, wo es bereit ist.
mpu.setDMPEnabled(true);
// Arduino-Interrupt-Erkennung aktivieren
attachInterrupt(0, dmpDataReady, RISING); //wenn steigend
mpuIntStatus = mpu.getIntStatus();
// DMP-Ready-Flag für Funktion loop()
dmpReady = true;
// die erwartete DMP-Paketgröße für einen späteren Vergleich zu ermitteln
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(")"));
}
}
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);
}
// 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);
input = ypr[1] * 180/M_PI + 180;
}
}
Bauteile zum ausdrucken auf dem 3D-Drucker:
https://www.thingiverse.com/thing:6012890
Die 3D-Druckvorlagen haben 4 vorbereitete Löcher, um dort M3 Gewindeeinsätze bei 200 Grad mit dem Lötkolben einzupressen. Daran wurde das Arduino Nano Expansion Shield befestigt.
Hardware (optional):
Gewindeeinsatz für 3D-Druck
M3 Schrauben Satz
Video: