Étape 4: Étape 4: le code
J’ai télécharger le code sur la joindre fichier
#include
#include
#include « I2Cdev.h »
#include « MPU6050_6Axis_MotionApps20.h »
#include
écran LCD
LiquidCrystal_I2C lcd (0 x 27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE) ;
#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 doit être 1
#define MOVE_BACK_FORTH 0
#define MIN_ABS_SPEED 30
MPU
Mpu MPU6050 ;
MPU/statut vars
bool dmpReady = false ; la valeur true si DMP init a réussi
uint8_t mpuIntStatus ; détient l’octet d’État réelle interruption de MPU
uint8_t devStatus ; retourne l’état après chaque opération de dispositif (0 = réussite,! 0 = erreur)
uint16_t packetSize ; attendre la taille de paquet DMP (valeur par défaut est 42 octets)
uint16_t fifoCount ; nombre d’octets tous actuellement de FIFO
uint8_t fifoBuffer [64] ; Tampon de stockage FIFO
orientation/motion vars
Quaternion q ; [w, x, y, z] conteneur de quaternion
VectorFloat gravité ; [x, y, z] vecteur de la gravité
flotteur ypr [3] ; [mouvement de lacet, tangage, rouler] lacet/tangage/roulis conteneur et gravité le vecteur
NID
#if MANUAL_TUNING
double kp, ki, kd ;
double prevKp, prevKi, prevKd ;
#endif
double originalSetpoint = 174.29 ;
double point de consigne = originalSetpoint ;
double movingAngleOffset = 0,3 ;
double entrée, sortie ;
int moveState = 0 ; 0 = solde ; 1 = retour ; 2 = suite
int LED = 13 ;
int 2 = 10 ;
#if MANUAL_TUNING
PID pid (& entrée, sortie, & point de consigne, 0, 0, 0, DIRECT) ;
#else
PID pid (& point de consigne, entrée & sortie 230, 300, 2.9, DIRECT) ;
#endif
CONTRÔLEUR DE MOTEUR
int ENA = 3 ;
int IN1 = 4 ;
int IN2 = 8 ;
int IN3 = 5 ;
int IN4 = 7 ;
int ENB = 6 ;
LMotorController motorController (IN1, IN2, ENB, IN3, ENA, IN4, 0,6, 1) ;
minuteries
long time1Hz = 0 ;
long time5Hz = 0 ;
volatils bool mpuInterrupt = false ; indique si broche interruption MPU est passée en haut
Sub dmpDataReady()
{
mpuInterrupt = true ;
}
void setup()
{
Joignez-vous à bus I2C (I2Cdev bibliothèque ne fait pas automatiquement)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.Begin() ;
TWBR = 24 ; horloge de 400kHz I2C (200kHz si le CPU est 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) ;
initialiser la communication série
(115 200 bauds choisi parce qu’il est nécessaire pour la sortie de la démo de théière, mais il a
vraiment à vous en fonction de votre projet)
Serial.Begin(115200) ;
LCD.Begin(20,4) ; initialiser l’écran lcd pour les lignes de 20 caractères 4 et activer le rétroéclairage
---Rapides 3 clignotements de rétro-éclairage---
pour (int i = 0; i < 4; i ++)
{
LCD.backlight() ;
Delay(250) ;
lcd.noBacklight() ;
Delay(250) ;
}
LCD.backlight() ; finission avec rétro-éclairage
---Écrire des caractères à l’écran---
Remarque : Position du curseur : CHAR, ligne) commencent à 0
lcd.setCursor(3,0) ; Commencer à caractère 4 ligne 0
LCD.Print ("Hello, world!") ;
Delay(1000) ;
lcd.setCursor(2,1) ;
LCD.Print ("mon nom KHALID") ;
Delay(1000) ;
lcd.setCursor(0,2) ;
LCD.Print ("Self Balancing ROBOT") ;
lcd.setCursor(0,3) ;
Delay(700) ;
LCD.Print("YouTube:CAYMANGUY123") ;
Delay(2000) ;
while (!. Série) ; attendre pour le dénombrement de Leonardo, d’autres continuent immédiatement
initialiser le périphérique
Serial.println (F ("initialisation I2C dispositifs...")) ;
lcd.setCursor(0,0) ; Commencer à caractère 4 ligne 0
LCD.Print (F ("I2C initialisation")) ;
Delay(1000) ;
MPU.Initialize() ;
vérifier la connexion
Serial.println (F ("Testing dispositif connexions...")) ;
lcd.setCursor(0,1) ; Commencer à caractère 4 ligne 0
LCD.Print (F ("dispositifs d’essai")) ;
Delay(1000) ;
Serial.println(MPU.testConnection() ? F("MPU6050 Connection successful"): F (« MPU6050 de connexion a échoué")) ;
lcd.setCursor(0,3) ;
LCD.println(MPU.testConnection() ? F("MPU6050 Connection"): F (« MPU6050 de connexion a échoué")) ;
charger et configurer DPM
Serial.println (F ("initialisation DMP...")) ;
devStatus = mpu.dmpInitialize() ;
fournir vos propres compensations gyro ici, mis à l’échelle de sensibilité min
mpu.setXGyroOffset(220) ;
mpu.setYGyroOffset(98) ;
mpu.setZGyroOffset(-85) ;
mpu.setZAccelOffset(1788) ; 1688 par défaut pour ma puce de test
Veillez à ce que cela a fonctionné (renvoie 0 si oui)
Si (devStatus == 0)
{
Allumez le DMP, maintenant qu’il est prêt
Serial.println (F (« activation DMP...")) ;
mpu.setDMPEnabled(true) ;
activer la détection d’interruption Arduino
Serial.println (F ("Enabling interrupt détection (Arduino interruption externe 0)...")) ;
attachInterrupt (0, dmpDataReady, RISING) ;
mpuIntStatus = mpu.getIntStatus() ;
donc la fonction principale de loop() sait qu’il est acceptable d’utiliser la valeur notre drapeau DMP prêt
Serial.println (F (DMP ready ! En attente de première interruption...")) ;
dmpReady = true ;
obtenir la taille du paquet DMP attendue pour comparaison ultérieure
packetSize = mpu.dmpGetFIFOPacketSize() ;
réglage PID
nid. SetMode(AUTOMATIC) ;
nid. SetSampleTime(10) ;
nid. SetOutputLimits (-255, 255) ;
}
d’autre
{
ERREUR !
1 = charge de mémoire initiale a échoué
2 = DMP configuration mises à jour a échoué
(si ça va casser, habituellement le code sera 1)
Serial.Print (F (« échec de l’initialisation de DMP (code ")) ;
Serial.Print(devStatus) ;
Serial.println(F(")")) ;
}
}
void loop()
{
Si la programmation n’a pas, n’essayez pas de faire n’importe quoi
Si (! dmpReady) return ;
attendre pour interruption MPU ou extra paquet (s) disponibles
tandis que (! mpuInterrupt & & fifoCount < packetSize)
{
aucune donnée de mpu - effectuer des calculs de PID et de sortie aux moteurs
pid.Compute() ;
motorController.move (sortie, MIN_ABS_SPEED) ;
unsigned long currentMillis = millis() ;
Si (currentMillis - time1Hz > = 1000)
{
loopAt1Hz() ;
time1Hz = currentMillis ;
}
Si (currentMillis - time5Hz > = 5000)
{
loopAt5Hz() ;
time5Hz = currentMillis ;
}
}
réinitialiser l’indicateur d’interruption et obtenir les octets INT_STATUS
mpuInterrupt = false ;
mpuIntStatus = mpu.getIntStatus() ;
obtenir le décompte actuel de FIFO
fifoCount = mpu.getFIFOCount() ;
vérification de dépassement de capacité (cela ne devrait jamais arriver sauf si notre code est trop inefficace)
Si ((mpuIntStatus & 0x10) || fifoCount = 1024)
{
réinitialiser afin que nous puissions continuer proprement
mpu.resetFIFO() ;
Serial.println (F (« dépassement de la FIFO!")) ;
dans le cas contraire, recherchez les interrupt prêt de DMP données (cela se produit fréquemment)
}
ElseIf (mpuIntStatus & 0 x 02)
{
Attendez la longueur correcte de données disponibles, devrait être très courte attente
tandis que (fifoCount < packetSize) fifoCount = mpu.getFIFOCount() ;
lire un paquet de FIFO
mpu.getFIFOBytes (fifoBuffer, packetSize) ;
suivre le comte FIFO ici dans le cas où il y a > 1 paquet disponible
(Ceci nous permet immédiatement lire la suite sans attendre une interruption)
fifoCount = packetSize ;
mpu.dmpGetQuaternion (& q, fifoBuffer) ;
mpu.dmpGetGravity (gravité, & q) ;
mpu.dmpGetYawPitchRoll (ypr, & q et gravité) ;
#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
entrée = ypr [1] * 180/M_PI + 180 ;
}
}
Sub loopAt1Hz()
{
#if MANUAL_TUNING
setPIDTuningValues() ;
#endif
}
Sub loopAt5Hz()
{
#if MOVE_BACK_FORTH
moveBackForth() ;
#endif
}
mouvement de va-et-vient
Sub moveBackForth()
{
moveState ++ ;
Si (moveState > 2) moveState = 0 ;
Si (moveState == 0)
consigne = originalSetpoint ;
ElseIf (moveState == 1)
consigne = originalSetpoint - movingAngleOffset ;
d’autre
consigne = originalSetpoint + movingAngleOffset ;
}
PID Tuning (3 potentiomètres)
#if MANUAL_TUNING
Sub 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
nid. SetTunings (kp, ki, kd) ;
prevKp = kp ; prevKi = ki ; prevKd = kd ;
}
}
Sub readPIDTuningValues()
{
int potKp = analogRead(A0) ;
int potKi = analogRead(A1) ;
int potKd = analogRead(A2) ;
KP = carte (potKp, 0, 1023, 0, 25000) / 100,0 ; 0 - 250
Ki = carte (potKi, 0, 1023, 0, 100000) / 100,0 ; 0 - 1000
KD = carte (potKd, 0, 1023, 0, 500) / 100,0 ; 0 - 5
}
#endif
