Självbalanserande Robot med LCD- (4 / 5 steg)

Steg 4: Steg 4: koden

Jag ladda upp koden på Bifoga fil

#include

#include
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include
LCD
LiquidCrystal_I2C lcd (0x27, 2, 1, 0, 4, 5, 6, 7, 3, positiv);

#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 måste vara 1
#define MOVE_BACK_FORTH 0
#define MIN_ABS_SPEED 30
MPU

MPU6050 mpu;
MPU kontroll/status vars
bool dmpReady = false; Ange sant om DMP init var framgångsrik
uint8_t mpuIntStatus; innehar faktiska interrupt status byte från MPU
uint8_t devStatus; returstatus efter varje enhet operation (0 = framgång,! 0 = fel)
uint16_t packetSize; förväntat DMP paketstorlek (standard är 42 bytes)
uint16_t fifoCount; räkning av alla byte i FIFO
uint8_t fifoBuffer [64]; FIFO lagring buffert
läggning/rörelse vars
Quaternion q; [w, x, y, z] quaternion behållare
VectorFloat allvar; [x, y, z] gravity vektor
float ypr [3]. [yaw, pitch, rulla] gir/pitch/rulle behållare och allvar vektor

PID

#if MANUAL_TUNING
dubbel kp, ki, kd;
dubbla prevKp, prevKi, prevKd;
#endif
Double originalSetpoint = 174.29;
dubbel setpoint = originalSetpoint;
Double movingAngleOffset = 0,3;
dubbel ingång, utgång;
int moveState = 0; 0 = balans; 1 = tillbaka; 2 = fram
int LED = 13.
int LED2 = 10;
#if MANUAL_TUNING
PID pid (& input, output & setpoint, 0, 0, 0, direkt);
#else
PID pid (& input, output & setpoint, 230, 300, 2.9, direkt);
#endif

MOTOR CONTROLLER

int ENA = 3;
int IN1 = 4;
int IN2 = 8.
int IN3 = 5;
int IN4 = 7.
int ENB = 6;

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

timers

lång time1Hz = 0;
lång time5Hz = 0;

flyktiga bool mpuInterrupt = false; Anger om MPU avbryta pin har gått hög
void dmpDataReady()
{
mpuInterrupt = sant;
}

void setup()
{
gå med I2C bussen (I2Cdev bibliotek inte gör detta automatiskt)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.BEGIN();
TWBR = 24. 400kHz I2C klocka (200kHz om CPU är 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::Setup (200, true);
#endif
pinMode(LED,OUTPUT);
digitalWrite(LED,HIGH);
pinMode(LED2,OUTPUT);
digitalWrite(LED2,HIGH);
initiera seriell kommunikation
(115200 valt eftersom det krävs för tekanna Demo utgång, men det har
verkligen upp till dig beroende på ditt projekt)
Serial.BEGIN(115200);

LCD.BEGIN(20,4); initiera lcd för 20 tecken 4 rader och slå på bakgrundsbelysningen
---Quick 3 blinkar av bakgrundsbelysning---
för (int jag = 0; jag < 4; i ++)
{
LCD.backlight();
Delay(250);
lcd.noBacklight();
Delay(250);
}
LCD.backlight(); avsluta med bakgrundsbelysning på

---Skriv tecken på displayen---
Obs: Markörens Position: röding, LINE) börjar på 0
lcd.setCursor(3,0); Börja med tecknet 4 på rad 0
LCD.Print ("Hello, world!");
Delay(1000);
lcd.setCursor(2,1);
LCD.Print ("mitt namn KHALID").
Delay(1000);
lcd.setCursor(0,2);
LCD.Print ("Self Balancing ROBOT");
lcd.setCursor(0,3);
Delay(700);
LCD.Print("YouTube:CAYMANGUY123");
Delay(2000);

tag (!. Seriell); vänta på Leonardo uppräkning, andra fortsätta omedelbart
initiera enheten
Serial.println F ("initierar I2C enheter...").
lcd.setCursor(0,0); Börja med tecknet 4 på rad 0
LCD.Print F ("initierar I2C").
Delay(1000);
MPU.Initialize();
Kontrollera anslutning
Serial.println F ("Testing enhetsanslutningar...").
lcd.setCursor(0,1); Börja med tecknet 4 på rad 0
LCD.Print F ("Testing enheter").
Delay(1000);
Serial.println(MPU.testConnection()? F("MPU6050 Connection successful"): F ("MPU6050 anslutning misslyckades"));
lcd.setCursor(0,3);
LCD.println(MPU.testConnection()? F("MPU6050 Connection"): F ("MPU6050 anslutning misslyckades"));
Ladda och konfigurera DMP
Serial.println F ("initierar DMP...").

devStatus = mpu.dmpInitialize();
lämna din egen gyro förskjutningar här, skalas för min känslighet
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(98);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); 1688 fabriksinställningarna för mitt test chip
se till att det fungerade (returnerar 0 om så är fallet)
om (devStatus == 0)
{
slå på DMP, nu när den är klar
Serial.println F ("Aktivera DMP...").

mpu.setDMPEnabled(true);
Aktivera Arduino avbrott upptäckt
Serial.println (F ("Aktivera avbryta upptäckt (Arduino externa avbrott 0)..."));
attachInterrupt (0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
ställa vår DMP redo flagga så funktionen huvudsakliga loop() vet att det är okej att använda det
Serial.println (F "(DMP redo! Väntar på första avbrott..."));
dmpReady = sant;
få förväntade DMP paketstorlek för senare jämförelse
packetSize = mpu.dmpGetFIFOPacketSize();

setup PID

PID. SetMode(AUTOMATIC);
PID. SetSampleTime(10);
PID. SetOutputLimits (-255, 255);
}
annat
{
FEL!
1 = första minne ladda misslyckades
2 = DMP konfiguration uppdateringar misslyckades
(om det kommer att bryta, oftast koden kommer att vara 1)
Serial.Print (F ("DMP initialisering misslyckades (kod"));
Serial.Print(devStatus);
Serial.println(F(")"));
}
}

void loop()
{
om programmering misslyckades, försök inte att göra något
om (! dmpReady) återvända;
vänta på MPU avbrott eller extra paket togs tillgängliga
medan (! mpuInterrupt & & fifoCount < packetSize)
{
inga mpu data - utför PID beräkningar och utgång till motorer

pid.Compute();
motorController.move (produktion, MIN_ABS_SPEED);

osignerade långa currentMillis = millis();
om (currentMillis - time1Hz > = 1000)
{
loopAt1Hz();
time1Hz = currentMillis;
}

om (currentMillis - time5Hz > = 5000)
{
loopAt5Hz();
time5Hz = currentMillis;
}
}
återställa avbrott flagga och få INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
få aktuella FIFO räkna
fifoCount = mpu.getFIFOCount();
check för overflow (detta skulle aldrig hända om inte vår kod är alltför ineffektiva)
om ((mpuIntStatus & 0x10) || fifoCount == 1024)
{
Återställ så vi kan fortsätta renlig
mpu.resetFIFO();
Serial.println F ("FIFO overflow!").

annars kolla för DMP data redo avbrott (detta bör ske ofta)
}
annars om (mpuIntStatus & 0x02)
{
vänta på rätt tillgänglig datalängd, bör vara en mycket kort vänta
medan (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
läsa ett paket från FIFO
mpu.getFIFOBytes (fifoBuffer, packetSize);

spåra FIFO räkningen här om det finns > 1 packet
(detta låter oss omedelbart Läs mer utan att invänta ett avbrott)
fifoCount-= packetSize;
mpu.dmpGetQuaternion (& q, fifoBuffer);
mpu.dmpGetGravity (& gravitation, & q);
mpu.dmpGetYawPitchRoll (ypr, & q & gravitation);
#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
}

flytta fram och tillbaka

void moveBackForth()
{
moveState ++;
om (moveState > 2) moveState = 0;

om (moveState == 0)
SetPoint = originalSetpoint;
annat if (moveState == 1)
SetPoint = originalSetpoint - movingAngleOffset;
annat
SetPoint = originalSetpoint + movingAngleOffset;

}

PID Tuning (3 potentiometrar)
#if MANUAL_TUNING
void setPIDTuningValues()
{
readPIDTuningValues();

om (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 = karta (potKp, 0, 1023, 0, 25000) / 100,0; 0 - 250
KI = karta (potKi, 0, 1023, 0, 100000) / 100,0; 0 - 1000
KD = karta (potKd, 0, 1023, 0, 500) / 100,0; 0 - 5
}
#endif