Stap 4: Stap 4: de code
Ik uploaden de code op het bijlage bestand
#include
#include
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include
LCD
LiquidCrystal_I2C lcd (0x27, 2, 1, 0, 4, 5, 6, 7, 3, positieve);
#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 moet 1 worden
#define MOVE_BACK_FORTH 0
#define MIN_ABS_SPEED 30
MPU
MPU6050 mpu;
MPU controle/status vars
BOOL dmpReady = false; Deze eigenschap is true als DMP init succesvol was ingesteld
uint8_t mpuIntStatus; werkelijke interrupt status byte van MPU houdt
uint8_t devStatus; status na operatie van elk apparaat retourneren (0 = succes! 0 = fout)
uint16_t packetomvang; verwacht DMP pakketgrootte (standaard is 42 bytes)
uint16_t fifoCount; telling van alle bytes in FIFO
uint8_t fifoBuffer [64]; FIFO-opslag buffer
oriëntatie/beweging vars
Quaternion q; [w, x, y, z] quaternion container
VectorFloat Ernst; [x, y, z] zwaartekracht vector
float ypr, [3]; [yaw, pitch, roll] yaw/pitch/roll container en zwaartekracht vector
PID
#if MANUAL_TUNING
dubbele kp, ki, kd;
dubbele prevKp, prevKi, prevKd;
#endif
dubbele originalSetpoint = 174.29;
dubbele setpoint = originalSetpoint;
dubbele movingAngleOffset = 0,3;
dubbele input, output;
int moveState = 0; 0 = balans; 1 = terug; 2 = weer
int LED = 13;
int LED2 = 10;
#if MANUAL_TUNING
PID pid (& input, output, & setpoint, 0, 0, 0, directe);
#else
PID pid (& input, output, & setpoint, 230, 300, 2.9, directe);
#endif
MOTORCONTROLLER
int ENA = 3;
int IN1 = 4;
int IN2 = 8;
int 3 = 5;
int IN4 = 7;
int ENB = 6;
LMotorController motorController (ENA IN1, IN2, ENB, 3, IN4, 0.6, 1);
timers
lange time1Hz = 0;
lange time5Hz = 0;
vluchtige bool mpuInterrupt = false; geeft aan of MPU interrupt pin hoge is gegaan
VOID dmpDataReady()
{
mpuInterrupt = true;
}
VOID Setup
{
deelnemen aan I2C bus (I2Cdev bibliotheek niet dit automatisch doet)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24; 400kHz I2C klok (200kHz als CPU 8MHz is)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::Setup (200, true);
#endif
pinMode(LED,OUTPUT);
digitalWrite(LED,HIGH);
pinMode(LED2,OUTPUT);
digitalWrite(LED2,HIGH);
initialiseren van seriële communicatie
(115200 gekozen omdat het is vereist voor de uitvoer van de theepot Demo, maar het heeft
echt aan u, afhankelijk van uw project)
Serial.begin(115200);
LCD.begin(20,4); initialiseren van het LCD-scherm voor 20 tekens 4 lijnen en achtergrondverlichting inschakelen
---Snelle 3 knippert van achtergrondverlichting---
for (int i = 0; ik < 4; i ++)
{
LCD.backlight();
delay(250);
lcd.noBacklight();
delay(250);
}
LCD.backlight(); eindigen met achtergrondverlichting op
---Schrijven tekens op het scherm---
Opmerking: De positie van de Cursor: CHAR, lijn) beginnen bij 0
lcd.setCursor(3,0); Beginnen bij teken 4 op regel 0
LCD.Print ("Hello, world!");
delay(1000);
lcd.setCursor(2,1);
LCD.Print ("mijn naam KHALID");
delay(1000);
lcd.setCursor(0,2);
LCD.Print ("Self Balancing ROBOT");
lcd.setCursor(0,3);
delay(700);
LCD.Print("YouTube:CAYMANGUY123");
delay(2000);
terwijl (!. Serieel); wachten voor Leonardo opsomming, anderen blijven onmiddellijk
initialiseren van apparaat
Serial.println (F ("initialiseren I2C apparaten..."));
lcd.setCursor(0,0); Beginnen bij teken 4 op regel 0
LCD.Print (F ("initialiseren I2C"));
delay(1000);
MPU.initialize();
verbinding controleren
Serial.println (F ("testen Apparaatverbindingen..."));
lcd.setCursor(0,1); Beginnen bij teken 4 op regel 0
LCD.Print (F ("testen apparaten"));
delay(1000);
Serial.println(MPU.testConnection()? F("MPU6050 Connection successful"): F ("MPU6050 connection failed"));
lcd.setCursor(0,3);
LCD.println(MPU.testConnection()? F("MPU6050 Connection"): F ("MPU6050 connection failed"));
laden en configureer de DMP
Serial.println (F ("initialiseren DMP..."));
devStatus = mpu.dmpInitialize();
levering eigen gyro compenseert hier, geschaald voor min gevoeligheid
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(98);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); 1688-fabrieksinstellingen voor mijn test-chip
Zorg ervoor dat het werkte (geeft als resultaat 0 als dat zo is)
Als (devStatus == 0)
{
inschakelen van de DMP, nu dat het klaar is
Serial.println (F ("waardoor DMP..."));
mpu.setDMPEnabled(true);
Arduino interrupt-detectie inschakelen
Serial.println (F ("inschakelen interrupt detectie (Arduino externe interrupt 0)..."));
attachInterrupt (0, dmpDataReady, stijgt);
mpuIntStatus = mpu.getIntStatus();
onze DMP klaar vlag instellen zodat de belangrijkste loop functie weet dat het is o.k. om het te gebruiken
Serial.println (F ("DMP klaar! Waiting for eerste interrupt..."));
dmpReady = true;
krijgen van de verwachte DMP pakketgrootte voor later vergelijking
packetomvang = mpu.dmpGetFIFOPacketSize();
Setup PID
PID. SetMode(AUTOMATIC);
PID. SetSampleTime(10);
PID. SetOutputLimits (-255, 255);
}
anders
{
FOUT!
1 = eerste geheugen laden mislukt
2 = DMP configuratie updates mislukt
(als het gaat om te breken, meestal de code zullen 1)
Serial.Print (F ("DMP-initialisatie is mislukt (code"));
Serial.Print(devStatus);
Serial.println(F(")"));
}
}
void loop
{
Als programmeren mislukt, probeer niet om iets te doen
Als (! dmpReady) terugkeer;
wachten op MPU interrupt of extra pakket(ten) beschikbaar
terwijl (! mpuInterrupt & & fifoCount < packetomvang)
{
geen gegevens mpu - uitvoeren van PID berekeningen en de uitvoer naar motoren
pid.Compute();
motorController.move (vermogen, MIN_ABS_SPEED);
unsigned long currentMillis = millis();
Als (currentMillis - time1Hz > = 1000)
{
loopAt1Hz();
time1Hz = currentMillis;
}
Als (currentMillis - time5Hz > = 5000)
{
loopAt5Hz();
time5Hz = currentMillis;
}
}
resetten van de interrupt-vlag en krijgen INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
huidige FIFO tellen
fifoCount = mpu.getFIFOCount();
selectievakje voor overloop (dit mag nooit gebeuren tenzij onze code te inefficiënt is)
Als ((mpuIntStatus & 0x10) || fifoCount == 1024)
{
resetten zodat we netjes blijven kunnen
mpu.resetFIFO();
Serial.println (F ('FIFO overflow!"));
anders, Controleer voor DMP gegevens klaar interrupt (dit zou moeten gebeuren vaak)
}
anders als (mpuIntStatus & 0x02)
{
wachten op de lengte van de juiste beschikbare gegevens, moet een zeer korte wachttijd
terwijl (fifoCount < packetomvang) fifoCount = mpu.getFIFOCount();
een pakje van de FIFO leest
mpu.getFIFOBytes (fifoBuffer, packetomvang);
bijhouden van FIFO graaf hier in het geval er > 1 pakket beschikbaar
(dit laat ons onmiddellijk Lees meer zonder te wachten tot een interrupt)
fifoCount-= packetomvang;
mpu.dmpGetQuaternion (& q, fifoBuffer);
mpu.dmpGetGravity (zwaartekracht, & q);
mpu.dmpGetYawPitchRoll (ypr, q, & zwaartekracht);
#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
invoer = ypr [1] * 180/M_PI + 180;
}
}
VOID loopAt1Hz()
{
#if MANUAL_TUNING
setPIDTuningValues();
#endif
}
VOID loopAt5Hz()
{
#if MOVE_BACK_FORTH
moveBackForth();
#endif
}
heen en weer
VOID moveBackForth()
{
moveState ++;
Als (moveState > 2) moveState = 0;
Als (moveState == 0)
Setpoint = originalSetpoint;
else if (moveState == 1)
Setpoint = originalSetpoint - movingAngleOffset;
anders
Setpoint = originalSetpoint + movingAngleOffset;
}
PID Tuning (3 potentiometers)
#if MANUAL_TUNING
VOID setPIDTuningValues()
{
readPIDTuningValues();
Als (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, kd, ki);
prevKp = kp; prevKi = ki; prevKd = kd;
}
}
VOID readPIDTuningValues()
{
int potKp = analogRead(A0);
int potKi = analogRead(A1);
int potKd = analogRead(A2);
KP = kaart (potKp, 0, 1023, 0, 25000) / 100.0; 0 - 250
Ki = kaart (potKi, 0, 1023, 0, 100000) / 100.0; 0 - 1000
KD = kaart (potKd, 0, 1023, 0, 500) / 100.0; 0 - 5
}
#endif
