Something stupid here ... help appreciated [SOLVED]

[dbdbdb]

OK, call me stupid :-[ I've been beating this to death all day but I can't see my mistake - all advice welcome !

Although my BT and my self-balancing robot code work separately, putting them together fails dismally - the robot balances reasonably well indefinitely, but as soon as I send it a command over BT it falls over, though the motors are still locked. It's as if it gets stuck in the BT loop, or never gets any more MPU6050 interrupts, not sure ...
I'm not understanding something here - please help

Thanks,

David

#include "I2Cdev.h"
#include <PID_v1.h>
#include "MPU6050_6Axis_MotionApps20.h"
#include <SoftwareSerial.h>

const int HC06_YELLOW=11;
const int HC06_GREEN=12;

SoftwareSerial BT(HC06_YELLOW, HC06_GREEN);

MPU6050 mpu;

bool dmpReady = false;  
uint8_t mpuIntStatus;
uint8_t devStatus;
uint16_t packetSize;  
uint16_t fifoCount; 
uint8_t fifoBuffer[64];

Quaternion q;
VectorFloat gravity; 
float ypr[3];

int charIdx;
char cmdChar; 
char readChar;
String readString;

double setpoint= 179.5; 
double Kp = 28; 
double Kd = 1.75;
double Ki = 350; 

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

int enA = 9;
int in1 = 4;
int in2 = 5;

int enB = 10; 
int in3 = 6;
int in4 = 7;

double motorAoffset = 0.75;
double motorBoffset = 1.0;

volatile bool mpuInterrupt = false;

void dmpDataReady()
{
  mpuInterrupt = true;
}

void setup() 
{
  Serial.begin(115200);
  BT.begin(9600);
  
    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.setXAccelOffset(youroffset)

  mpu.setXAccelOffset(-4637);
  mpu.setYAccelOffset(605);
  mpu.setZAccelOffset(1943);
  mpu.setXGyroOffset(110);
  mpu.setYGyroOffset(33);
  mpu.setZGyroOffset(-4);
  
  // make sure it worked (returns 0 if so)
  if (devStatus == 0)
  {
    // turn on the DMP, now that it's ready
    Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);

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

    // set our DMP Ready flag so the main loop() function knows it's okay to use it
    Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;

    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();
        
    //setup PID
    pid.SetMode(AUTOMATIC);
    pid.SetSampleTime(10);
    pid.SetOutputLimits(-255, 255);  
  }
  else
  {
      // ERROR!
      // 1 = initial memory load failed
      // 2 = DMP configuration updates failed
      // (if it's going to break, usually the code will be 1)
      Serial.print(F("DMP Initialization failed (code "));
      Serial.print(devStatus);
      Serial.println(F(")"));
  }

  // Initialise the Motor output pins
  pinMode(enA, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  
  pinMode(enB, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);

  //By default turn off both the motors
  Stop();
}

void loop()
{
  // if programming failed, don't try to do anything
  if (!dmpReady) return;

  // wait for MPU interrupt or extra packet(s) available
  while (!mpuInterrupt && fifoCount < packetSize)
  {
    //no mpu data - performing PID calculations and output to motors     
    pid.Compute();   
        
    //Print the value of Input and Output on serial monitor to check how it is working.
    Serial.print(input); Serial.print(" =>"); Serial.println(output);
               
    if (input>150 && input<200){//If the Bot is falling 
          
      if (output>0) //Falling towards front 
        Forward(); //Rotate the wheels forward 
      else if (output<0) //Falling towards back
        Reverse(); //Rotate the wheels backward 
    }
    else //If Bot not falling
      Stop(); //Hold the wheels still

    if (BT.available()){  // if text arrived in from Serial serial
      charIdx = 0;
      while (BT.available()) 
      {
        delay(3);  
        readChar = BT.read();
        if (charIdx == 0)
          cmdChar = readChar;
        else
          readString += readChar;
        charIdx++; 
      }
      if (readString.length() >0) 
      {  

        switch (cmdChar){
          case 'i':
            Ki = readString.toDouble();
            break;
          case 'd':
            Kd = readString.toDouble();
            break;
          case 'p':
            Kp = readString.toDouble();
            break;
          case 's':
            setpoint = readString.toDouble();
            break;
          case 'A':
            motorAoffset = readString.toDouble();
            break;
          case 'B':
            motorBoffset = readString.toDouble();
            break;
        }
      
        debugSettings();        

        readString="";  
      }
      BT.println("\n-----------------");
    }
  }

  // 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[2] * 180/M_PI + 180;
  }
}

void debugSettings()
{
  BT.print("setpoint:     "); BT.println(setpoint);
  BT.print("yaw:          "); BT.println(ypr[1]);
  BT.print("pitch:        "); BT.println(ypr[2]);
  BT.print("Kp:           "); BT.println(Kp);
  BT.print("Kd:           "); BT.println(Kd);
  BT.print("Ki:           "); BT.println(Ki);
  BT.print("motorAoffset: "); BT.println(motorAoffset);
  BT.print("motorBoffset: "); BT.println(motorBoffset);
}
void Forward() // Code to rotate the wheel forward 
{
    // Set Motor A forward
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  
  // Set Motor B forward
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);

  analogWrite(enA, output*motorAoffset); // Send PWM signal to motor A
  analogWrite(enB, output*motorBoffset); // Send PWM signal to motor B
}

void Reverse() // Code to rotate the wheel Backward  
{
    // Set Motor A backward
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  
  // Set Motor B backward
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);

  analogWrite(enA, output*motorAoffset*-1); // Send PWM signal to motor A
  analogWrite(enB, output*motorBoffset*-1); // Send PWM signal to motor B
}

void Stop() // Code to stop both the wheels
{
    // Set Motor A stop
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  
  // Set Motor B stop
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);

  analogWrite(enA, 0); // Send PWM signal to motor A
  analogWrite(enB, 0); // Send PWM signal to motor B
}

[Paul_KD7HB]

An easy to be SURE if the cause is the BT call is to time the call. Get and store the millis before the call. Get the millis after the call. Subtract and Serial.println() the difference.

Paul

[dbdbdb]

Hmm, not sure what that's telling me but I put in the suggested timings and it just prints out "207 ms" and then doesn't respond to any further input, either BT commands or change of orientation/MPU6050 updates, the wheels just continue spinning.

...
   else //If Bot not falling
      Stop(); //Hold the wheels still

    if (BT.available()){  // if text arrived in from Serial serial
      t1=millis();
      charIdx = 0;
      while (BT.available()) 
      {
        delay(3);  
        readChar = BT.read();
        if (charIdx == 0)
          cmdChar = readChar;
        else
          readString += readChar;
        charIdx++; 
      }
      if (readString.length() >0) 
      {  

        switch (cmdChar){
          case 'i':
            Ki = readString.toDouble();
            break;
          case 'd':
            Kd = readString.toDouble();
            break;
          case 'p':
            Kp = readString.toDouble();
            break;
          case 's':
            setpoint = readString.toDouble();
            break;
          case 'A':
            motorAoffset = readString.toDouble();
            break;
          case 'B':
            motorBoffset = readString.toDouble();
            break;
        }
      
        debugSettings();        

        readString="";  
      }
     
      t2=millis();
      Serial.print(t2-t1); Serial.println(" ms");
    }

[Robin2]

It is not a good idea to use the String (capital S) class on an Arduino as it can cause memory corruption in the small memory on an Arduino. This can happen after the program has been running perfectly for some time. Just use cstrings - char arrays terminated with '\0' (NULL).

Have a look at the examples in Serial Input Basics - simple reliable ways to receive data. There is also a parse example to illustrate how to extract numbers from the received text.

I suggest you ensure that every message sent to the Arduino is less than 64 bytes long so it fits in the Serial Input Buffer. Then (if you use my examples) you can call the function to retrieve the data AFTER every PID calculation. That way reading the serial data should not interfere with the balancing process.

What is the interval between your PID calculations ?

...R

[dbdbdb]

Thanks Robin,

I converted the sketch to use a null-terminated char array and atof() to convert the input to double but get the same problem - process the received chars, robot falls over. I've checked all the obvious stuff - no buffer overruns or buggy indexing, and the received characters are being converted to the correct values.

pid.Compute() is being called every 1-2ms under "normal" circumstances, i.e. before I send any data to the BT to trigger the if (BT.available()).

Thanks for the samples - I'm off to read through that now, maybe I'll spot what I'm doing wrong.

David

[dbdbdb]

Just an update ... I'm sure the BT reading code I've written is fine, but just in case I commented it all out and replaced it with

    if (BT.available()){  // if text arrived in from Serial serial
     while (BT.available())
      Serial.println(BT.read()); 
    }

That's enough to cause the problem, which smells like a memory overwrite or an electrical problem or something. I've double and triple-checked all my wiring. The robot balances well until it gets input on the BT lines then it freezes. I've tried an Arduino and an Elgoo board - same problem on both, though the SoftwareSerial just doesn't work on the Elgoo (all characters except the first one are corrupted) so I used the hardware serial on that one.

I also discovered that Serial.begin(115200) is the only baud rate that works - 9600 or 19200 seem to stop the MPU6050 ever sending any interrupts. I tried two different MPU6050 boards - both the same.

So now I'm completely stumped ...

[Robin2]

The Serial.print() statement is very slow.

The Arduino PID library is also very slow because it uses floating point maths. If you are only allowing 1 or 2 millisecs between calls to the library you may be just trying to get your Arduino to do too much.

Can you increase the PID interval to (say) 10 millisecs and see if that helps?

...R

[srnet]

dbdbdb:
OK, call me stupid :-[ I've been beating this to death all day but I can't see my mistake - all advice welcome !

Its stupid not to include even a clue in your post title as to what the subject of your post is.

[dbdbdb]

Thanks, great help srnet

[dbdbdb]

I think you're right Robin - it looks like it's all about the timing. I found loads of other pages reporting similar problems with Serial and MPU6050 together.

It looks like the FIFO buffer on the MPU6050 is getting smashed unless it's emptied fast enough. Just a delay(10) in the loop() will kill it, just the same as anything else that takes up time. So this combination that I'm using is OK for a robot that just balances but it won't work if I want to do anything else useful at the same time.

It might just it's a choice between either playing around trying to shave microseconds every time I add a new device to it, or move to a faster processor. I wanted to add an SR-04 next but I don't think that's a viable option. Maybe I need to ditch the PID library I'm using or something, either way I need to do a lot more research.

Thanks for your help anyway

David

[Robin2]

Do you really need to call PID every millisec? I can't imagine that your robot would fall far in 20 millisecs.

...R

[dbdbdb]

Yes it does sound excessive, and following your suggestion I found that it can do with less for example if I check no less than every 15ms with this:

 if (millis()-last_pid > 15){
      pid.Compute();
      last_pid=millis();
      Serial.println("pid.Compute()");
    }

then it just about balances but is very jittery, though I might be able to iron out the jitters by adjusting the PID values. 10ms is fine and 20ms is too much for it to compensate for.

This is helpful though, as it shows that I can free up some more processing time. It's also interesting that with a 10ms sampling rate I don't need to run the Serial port at 115200 baud - it works fine at 38400.

However the problem is unchanged - as soon as I send a character and trigger the BT read code

if (BT.available()){
      while (BT.available()) 
        Serial.print(BT.read());
      Serial.println("");
}

the robot goes into freefall. The motors continue doing whatever they were doing before it crashed, i.e. stopped or spinning. Sometimes I get the message "FIFO overflow" displayed, but sometimes I think it crashes before it can output this. My suspicion is that some buffer is filling faster than it can be emptied, and is overflowing and corrupting the contents of the memory. I'll keep digging ...

[Robin2]

dbdbdb:
However the problem is unchanged - as soon as I send a character and trigger the BT read code

if (BT.available()){

while (BT.available())
        Serial.print(BT.read());
      Serial.println("");
}

That code provoked a WTF response in my head.

What is the value of receiving a message (actually a single character) and then sending it out without ever using it?

...R

[dbdbdb]

Yes, I mentioned earlier that this is a cut down version of my full code - if you remember I was originally reading into a String but then changed this on your advice to a cstring. The code then parses the received characters to set the Kp, Ki, Kd etc. parameters.

However I reduced the code to an absolute minimum to see which instruction triggers the problem, and in fact the "if BT.available()" is superfluous here too - the "while"/"read" loop is enough to make it happen.

[srnet]

dbdbdb:
Thanks, great help srnet

But you did not change the title of your first post.

From the how to use this forum post;

6. Make a meaningful subject line

Try to summarize your problem with a helpful and informative subject line. Helpful subjects draw in people who might know the answer. Unhelpful ones are likely to be skipped.

Examples of unhelpful subject lines:

Noob here, help needed
Help me, quickly!
Got error
Lost, help me
Problem with my circuit
No idea what I am doing
Something stupid here ... help appreciated

[dbdbdb]

I read a lot, and learned a lot.

In case it helps anyone in future who is trying to build a self-balancing robot with the same hardware as me (Uno, Lynxmotion dual H-bridge, couple of 12V 400rpm DC motors, MPU6050, HC-06, diymore power bank shield), this is the information and the code I've ended up with, which allows me to achieve my goal of tweaking the Kp, Ki, Kd, setpoint and motor offsets over bluetooth.

I still have a minor issue to solve - occasionally the robot still falls over when I read data, but it's recoverable. It seems it temporarily spends more resources reading data than staying balanced so it falls out of its tolerance zone too fast to recover itself, but it does carry on when I just stand it up vertically again. This is infinitely preferable to the original problem where the first call to BT.read() sent it into an endless loop that it never recovered from no matter what I did. I just need to balance the "balancing vs doing other stuff" times

There seem to be a number of fundamental issues that together make the combination of Wire, i2cdevlib and the MPU6050 device somewhat fragile. Timing seems to play a major part, and there are some suggestions that it's possible for the hardware to get into a state that the core libraries don't handle, which result in an endless loop where an i2c reset might be more desirable in particular use cases like self-balancing robots/quadcopters. Others have suggested that the MPU code does unexpected things. Ultimately I couldn't find anything conclusive but some great investigative work has been done (thanks esp. to jrowberg , zhomeslice and batata004.

The discussion on Issues · jrowberg/i2cdevlib · GitHub is illuminating (in my case this one especially MPU6050 with DMP active hangs indefinitely using Arduino Wire library · Issue #252 · jrowberg/i2cdevlib · GitHub), as are the threads Good news!!! DMP from MPU6050 can be used without interrupt pin - Programming Questions - Arduino Forum and MPU6050 freezing/crashing - Programming Questions - Arduino Forum.

All the best,

David

[dbdbdb]

My working code:

#include "I2Cdev.h"
#include <PID_v1.h>
#include "MPU6050_6Axis_MotionApps20.h" 
#include <SoftwareSerial.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

const int HC06_YELLOW=11; // pin 11 is Arduino RX
const int HC06_GREEN=12;  // pin 12 is Arduino TX

SoftwareSerial BT(HC06_YELLOW, HC06_GREEN); // Arduino RX to BT TX, TX  -> BT RX 

int charIdx;
char cmdChar;   // last command character read (zero-th input)   
char readChar; // stores incoming character from other device
String readString;

double setpoint=170; //set the value when the bot is perpendicular to ground using serial monitor. 

double Kp = 25;
double Kd = 1.8;
double Ki = 400; 

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

unsigned long t1 = millis();

// MOTOR CONTROLLER
// enA in1 in2 Result
//  L  N/A N/A Power off
//  H   L   L  Stop (Brake)
//  H   H   L  CW (Fast)
//  H   L   H  CCW (Fast)
//  H   H   H  Stop (Brake)
//  P   H   L  CW (Slow)
//  P   L   H  CCW (Slow)

int enA = 9;
int in1 = 4;
int in2 = 5;

int enB = 10; 
int in3 = 6;
int in4 = 7;

double motorAoffset = 0.96;
double motorBoffset = 1.0;

volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high

void dmpDataReady()
{
  mpuInterrupt = true;
}

void setup() 
{
  Wire.begin();
  TWBR = 24;

  Serial.begin(115200);
  while (!Serial);

  BT.begin(9600);

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

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

  mpu.setXAccelOffset(-4637);
  mpu.setYAccelOffset(605);
  mpu.setZAccelOffset(1943);
  mpu.setXGyroOffset(110);
  mpu.setYGyroOffset(33);
  mpu.setZGyroOffset(-4);
  
  devStatus = mpu.dmpInitialize();

  if (devStatus == 0)
  {
    // turn on the DMP, now that it's ready
    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();
        
    //setup PID
    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(enA, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  
  pinMode(enB, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);

  Stop();
}

void loop()
{
  if (!dmpReady) return;

  while (!mpuInterrupt) {

    pid.Compute();
        
    // If falling take corrective action
               
    if (input > 150 && input < 200){ 
      if (output > 0)       // Falling towards front 
        Forward();          // Rotate the wheels forward 
      else if (output < 0)  // Falling towards back
        Reverse();          // Rotate the wheels backward 
    }
    else      // not falling or fallen too far to recover
      Stop(); // so hold the wheels still

    if (BT.available())
      readInput();
  }

  // got interrupt - MPU data may be available
  mpuInterrupt = false;
  fifoCount = mpu.getFIFOCount();
    
  if (fifoCount == 1024) {
    mpu.resetFIFO();
    Serial.println(F("FIFO overflow!"));
  }
  else {
    if (fifoCount % packetSize != 0) {
      mpu.resetFIFO();
      Serial.println(F("Packet is corrupted!"));
    }
    else {
      while (fifoCount >= packetSize) {
        mpu.getFIFOBytes(fifoBuffer,packetSize);
        fifoCount -= packetSize;
      }    
      mpu.dmpGetQuaternion(&q,fifoBuffer);
      mpu.dmpGetGravity(&gravity,&q);
      mpu.dmpGetYawPitchRoll(ypr,&q,&gravity);          
           
      // my MPU is mounted sideways so use ypr[2] for pitch
      input = ypr[2] * 180/M_PI + 180;
    }
  }
}

void readInput()
{  
  charIdx = 0;

  while (BT.available()) 
  {
    readChar = BT.read();
    if (charIdx == 0)
      cmdChar = readChar;
    else
      readString += readChar;
    charIdx++; 
  }
  if (readString.length() >0) 
  {  
    switch (cmdChar){
      case 'i':
        Ki = readString.toDouble();
        break;
      case 'd':
        Kd = readString.toDouble();
        break;
      case 'p':
        Kp = readString.toDouble();
        break;
      case 's':
        setpoint = readString.toDouble();
        break;
      case 'A':
        motorAoffset = readString.toDouble();
        break;
      case 'B':
        motorBoffset = readString.toDouble();
        break;
    }
    debugSettings();        
    readString="";  
  }
}

void debugSettings()
{
  BT.print("setpoint:     "); BT.println(setpoint);
  BT.print("yaw:          "); BT.println(ypr[1]);
  BT.print("pitch:        "); BT.println(ypr[2]);
  BT.print("Kp:           "); BT.println(Kp);
  BT.print("Kd:           "); BT.println(Kd);
  BT.print("Ki:           "); BT.println(Ki);
  BT.print("motorAoffset: "); BT.println(motorAoffset);
  BT.print("motorBoffset: "); BT.println(motorBoffset);
  BT.println("\n-----------------");
}

void Forward() // rotate wheels forward 
{
    // Motor A
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  
  // Motor B
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);

  analogWrite(enA, output*motorAoffset);
  analogWrite(enB, output*motorBoffset);
}

void Reverse() // rotate wheels Backward  
{
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);

  analogWrite(enA, output*motorAoffset*-1);
  analogWrite(enB, output*motorBoffset*-1);
}

void Stop() // stop both wheels
{
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);

  analogWrite(enA, 0);
  analogWrite(enB, 0);
}