Balansera Instructable Robot (12 / 14 steg)

Steg 12: Ladda upp koden

När du har gjort roboten slutligen dess tid att ladda upp koden och ge roboten en provkörning. Jag har lämnat den ifyllda koden bara ladda upp det och experiment med PID konstant att uppnå rätt balans.

KOD:

#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"
#include "MPU6050.h" / / inte nödvändigt om du använder MotionApps include-filen

Arduino tråd bibliotek krävs om I2Cdev I2CDEV_ARDUINO_WIRE genomförande
används i I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif

MPU6050 mpu;
#define OUTPUT_READABLE_YAWPITCHROLL

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
VectorInt16 aa; [x, y, z] accel sensor mätningar
VectorInt16 aaReal; [x, y, z] gravity-fri accel sensor mätningar
VectorInt16 aaWorld; [x, y, z] världen-frame accel sensor mätningar
VectorFloat allvar; [x, y, z] gravity vektor
flyta euler [3]. [psi, theta, phi] Euler vinkel behållare
float ypr [3]. [yaw, pitch, rulla] gir/pitch/rulle behållare och allvar vektor

statisk int pot1Pin = A0;
statisk int pot2Pin = A1;
statisk int pot3Pin = A2;
flyta Kp = 0;
flyta Ki = 0;
flyta Kd = 0;
flyta targetAngle = 8,5;
flyta currAngle = 0;
flyta pTerm = 0;
flyta iTerm = 0;
flyta dTerm = 0;
flyta integrated_error = 0;
flyta last_error = 0;
float K = 1;
// ================================================================
=== AVBROTT UPPTÄCKT RUTIN ===
// ================================================================

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

// ================================================================
=== INITIAL SETUP ===
// ================================================================

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 (400, true);
#endif

Serial.BEGIN(115200);
tag (!. Seriell);
Serial.println F ("initierar I2C enheter...").
MPU.Initialize();
Serial.println F ("Testing enhetsanslutningar...").
Serial.println(MPU.testConnection()? F("MPU6050 Connection successful"): F ("MPU6050 anslutning misslyckades"));
Serial.println F ("initierar DMP...").
devStatus = mpu.dmpInitialize();
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); 1688 fabriksinställningarna för mitt test chip
om (devStatus == 0) {
Serial.println F ("Aktivera DMP...").
mpu.setDMPEnabled(true);
Serial.println (F ("Aktivera avbryta upptäckt (Arduino externa avbrott 0)..."));
attachInterrupt (0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
Serial.println (F "(DMP redo! Väntar på första avbrott..."));
dmpReady = sant;
packetSize = mpu.dmpGetFIFOPacketSize();
} annat {
Serial.Print (F ("DMP initialisering misslyckades (kod"));
Serial.Print(devStatus);
Serial.println(F(")"));
}
pinMode (3, OUTPUT);
pinMode (4, OUTPUT);
pinMode (pot3Pin, ingång);
pinMode (pot1Pin, ingång);
pinMode (pot2Pin, ingång);
pinMode (A3, OUTPUT);
pinMode(12,OUTPUT);
pinMode(13,OUTPUT);
}

// ================================================================
=== MAIN PROGRAM LOOP ===
// ================================================================

void loop() {
om (! dmpReady) återvända;
medan (! mpuInterrupt & & fifoCount < packetSize) {
}
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount();
om ((mpuIntStatus & 0x10) || fifoCount == 1024) {
mpu.resetFIFO();
Serial.println F ("FIFO overflow!").
} else om (mpuIntStatus & 0x02) {
medan (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes (fifoBuffer, packetSize);
fifoCount-= packetSize;

#ifdef OUTPUT_READABLE_YAWPITCHROLL
mpu.dmpGetQuaternion (& q, fifoBuffer);
mpu.dmpGetGravity (& gravitation, & q);
mpu.dmpGetYawPitchRoll (ypr, & q & gravitation);
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
KP = map(analogRead(pot1Pin), 0, 1024, 0, 255);
KI = map(analogRead(pot2Pin), 0, 1024, 0, 20);
KD = map(analogRead(pot3Pin), 0, 1024, 0, 20);
Serial.Print ("Kp:");
Serial.Print(KP);
Serial.Print ("Ki:");
Serial.Print(KI);
Serial.Print ("Kd:");
Serial.println(KD);
currAngle = (ypr [1] * 180/M_PI);
Drive_Motor(updateSpeed());
Serial.Print ("hastighet:");
Serial.println(updateSpeed());
}
}

float updateSpeed() {
flyta fel = targetAngle - currAngle;
pTerm = Kp * fel;
integrated_error += fel;
iTerm = Ki * begränsa (integrated_error, -50, 50);
dTerm = Kd * (fel - last_error);
last_error = fel;
återvända - begränsa (K * (pTerm + iTerm + dTerm),-255, 255);
}

Drive_Motor(float torque) {float
Serial.Print ("vridmoment:");
Serial.println(torque);
om (vridmoment > = 0) {/ / drivmotorer fram
digitalWrite (3, låg).
digitalWrite (4, hög).
digitalWrite 12, låg.
digitalWrite 13, hög.
} annat {/ / drivmotorer bakåt
digitalWrite (3, hög).
digitalWrite (4, låg).
digitalWrite 12, hög.
digitalWrite 13, låg.
vridmoment = abs(torque);
}
analogWrite(A3,torque);
}

Om anslutningarna följs exakt får du motorerna förvandlas i minst en riktning, om den inte gå tillbaka till föregående steg och kontrollera anslutningarna igen. När allt går bra kommer du få resultat som visas i testet videon ovan. Sedan kommer jag ge några instruktioner på hur till få anständigt resultat från PID konstanterna.

Tuning PID konstanter:

1. För det första öka proportionellt värdet genom att långsamt vrida reglaget proportionell potentiometer. Testa balanseringen av roboten för varje tur potentiometerns.
2. Sedan när roboten svänger från sida till sida som en pendel öka sedan du Integral och derivat konstant potentiometrar.
3. Detta är den svåraste delen eftersom endast vid vissa värden kan effektivt balansera uppnås i roboten. Mitt förslag är att variera värdena och bestämma de idealiska värdena för din robot. Observera att dessa värden variera från robot till robot.

____________________________________________

Observera att detta är min första framgångsrika balanserande robot. Så jag har fått roboten balansering men det säkert finns utrymme för förbättringar. I framtiden vill jag göra en mer exakt PID kontrollerade balanserande robot med ytterligare funktioner kanske i framtiden.