Go Down

Topic: Balancing Robot with MPU6050 didn't works (Read 299 times) previous topic - next topic

0xinfinite

Aug 02, 2019, 01:34 pm Last Edit: Aug 02, 2019, 01:44 pm by 0xinfinite
Hello, I'm making Balancing robot with Arduino UNO. I'm using a gyro sensor to MPU6050 and 2 DC motors. They connected with 2A L298N motor driver. Also, using 6 AA batteries.



When I upload a sketch and connect power, It cannot function. Error got 2 types. One is motor spin only one direction. when it happens, serial monitor doesn't print any single lines. Another one is gyro angle value print to randomly, so motor just vibration.

I changed all parts new one. Even When I changed Arduino UNO, It's still not functional. The funny thing is, When I am making a first Balancing robot, It works so well. This is my second balancing robot and It didn't work. What am I missing?

here is my sketch and I'll attach current used libraries.

Code: [Select]

#include "I2Cdev.h"
#include <PID_v1.h>
#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// 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

//setting PID parameter
double setpoint= 171;
double Kp = 21;
double Kd = 0.8;
double Ki = 140;

double input, output;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);



volatile bool mpuInterrupt = false;    
void dmpDataReady()
{
   mpuInterrupt = true;
}

void setup() {
 Serial.begin(115200);


   Serial.println(F("Initializing I2C devices..."));
   mpu.initialize();


   Serial.println(F("Testing device connections..."));
   Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));


   devStatus = mpu.dmpInitialize();

  

   mpu.setXGyroOffset(220);
   mpu.setYGyroOffset(76);
   mpu.setZGyroOffset(-85);
   mpu.setZAccelOffset(1688);


   if (devStatus == 0)
   {

       Serial.println(F("Enabling DMP..."));
       mpu.setDMPEnabled(true);


       Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
       attachInterrupt(0, dmpDataReady, RISING);
       mpuIntStatus = mpu.getIntStatus();


       Serial.println(F("DMP ready! Waiting for first interrupt..."));
       dmpReady = true;


       packetSize = mpu.dmpGetFIFOPacketSize();
      

       pid.SetMode(AUTOMATIC);
       pid.SetSampleTime(10);
       pid.SetOutputLimits(-255, 255);  
   }
   else
   {
       Serial.print(F("DMP Initialization failed (code "));
       Serial.print(devStatus);
       Serial.println(F(")"));
   }


   pinMode (6, OUTPUT);
   pinMode (9, OUTPUT);
   pinMode (10, OUTPUT);
   pinMode (11, OUTPUT);


   analogWrite(6,LOW);
   analogWrite(9,LOW);
   analogWrite(10,LOW);
   analogWrite(11,LOW);
}

String receivedValue;
bool motorPrint = true;

void loop() {



   if (!dmpReady) return;


   while (!mpuInterrupt && fifoCount < packetSize)
   {

       pid.Compute();  
      

       if(motorPrint==true)
       {
         Serial.print(input); Serial.print(" =>"); Serial.println(output);
       }
            
       if (input>150 && input<200){
        
       if (output>0)
       Forward();
       else if (output<0)
       Reverse();
       }
       else
       Stop();
      
   }

   // 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); //get value for q
       mpu.dmpGetGravity(&gravity, &q); //get value for gravity
       mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); //get value for ypr

       input = ypr[1] * 180/M_PI + 180;

  }

  delay(1);
}

void Forward()
{
   analogWrite(6,output);
   analogWrite(9,0);
   analogWrite(10,output);
   analogWrite(11,0);
   //Serial.print("F");
}

void Reverse()
{
   analogWrite(6,0);
   analogWrite(9,output*-1);
   analogWrite(10,0);
   analogWrite(11,output*-1);
 //  Serial.print("R");
}

void Stop()
{
   analogWrite(6,0);
   analogWrite(9,0);
   analogWrite(10,0);
   analogWrite(11,0);
 //  Serial.print("S");
}



vinceherman

Proper use of code tags on your first post.  YAY!  Karma++

I would troubleshoot this piece by piece.
Can you write a sketch that makes each motor run forward for some time, stop for some time and then run backward for some time?  That will confirm that you have things wired up properly and that you can control the motors from software.

Can you write a sketch that does nothing but read the gyro angle and prints it to the serial monitor?  Then move the vehicle and note the changes in values.  This will confirm that you have the gyro wired up properly and that you can read valid values from it.

zhomeslice

I see no one posted. Sorry I didn't spot this till now. I hope you haven't given up.
Her is my code for My balancing bot. 
The Default PID Library fails to handle time correctly when you are needing derivative calculations to be accurate. Integral is worthless for the initial setup add it last and only if needed.
Also, you should be able to get a sequence for controlling your motors by following my code.
As for gathering The FIFO data, I have refined that as well. My version waits for an interrupt (@ every 10ms), gathers the data then triggers a PID loop then controls the motors. 
This is a simple video to see what happens when you are attempting to balance your bot.
How to Balance Robot PID tutorial in under 2 minutes!

Z

HC

RoyIn7505

 I met the same issue with you. The reason why serial port cant work normal is that spining motors make noise to the controller. And the noise makes I2c boken. I saved the problem by adding an isolate between motors and the UNO. Mabye it can help you. Sorry for my poor English.

Go Up