Where has the delay gone?

Projects Programming
1

Hi everyone,

My code below is obtaining angles from an MPU6050, but the problem I am having is with the last 10 lines of code where it sets a delay so that the time of one loop is always the same. Having set the loop time to 2 seconds, when you read the serial data port the data is coming through at 40Hz, so the adaptive delay if statement isn't working for some reason and I really don't know why. Thanks!

#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"
const int MPU_addr=0x68;  // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
float accelangleY;  float accelangleX;
float gyroY;        float gyroX;        double gyroZ;
float compangleX=0; float compangleY=0; double gyroangleZ = 0;
unsigned long starttime=0;
unsigned long delta;
unsigned long endtime;
long looptime = 2000; long looptimemicros;// Loop time in milli seconds
float gain = 0.9;  // complementary filter gain

MPU6050 mpu;

//      Offset Values:  XA      YA      ZA      XG      YG      ZG
int MPUOffsets[6] = {  -1745,  1704,   1362,    79,    -18,     21}; 

void setup(){
  delay(100);
  looptimemicros = looptime*1000;
  Wire.begin();
  Serial.begin(9600);
  
  mpu.initialize(); 
  // verify connection
  Serial.println("Testing device connections...");
  Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  
  mpu.setXAccelOffset(-1745);
  mpu.setYAccelOffset(1704);
  mpu.setZAccelOffset(1362);
  mpu.setXGyroOffset(79);
  mpu.setYGyroOffset(-18);
  mpu.setZGyroOffset(21);
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
  
  /* 
  AFS_SEL | Full Scale Range | LSB Sensitivity |   Accelerometer Set up
     0    |      ±2g         |   16384 LSB/g   |
     1    |      ±4g         |   8192 LSB/g    |
     2    |      ±8g         |   4096 LSB/g    |
     3    |      ±16g        |   2084 LSB/g    |
     
  FS_SEL  | Full Scale Range | LSB Sensitivity |    Gyro Setup (Leave at default value FS_SEL=0)
     0    |      ±250 deg/s  |  131 LSB/deg/s  |
     1    |      ±500 deg/s  |  65.5 LSB/deg/s |
*/
  
  Wire.endTransmission(true);
}

void loop(){
  
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
  AcX = Wire.read()<<8;
  AcX |= Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)     
  AcY = Wire.read()<<8;
  AcY |= Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ = Wire.read()<<8;
  AcZ |= Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp = Wire.read()<<8;
  Tmp |= Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyX = Wire.read()<<8;
  GyX |= Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY = Wire.read()<<8;
  GyY |= Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ = Wire.read()<<8;
  GyZ |= Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  
  accelangleY = atan2(AcX, AcZ)*4068/71;
  accelangleX = atan2(AcY, AcZ)*4068/71;
  
  gyroX = (GyX-3)/131.00; //turn value into deg/s
  gyroY = (GyY)/131.00; //turn value into deg/s
  gyroZ = (GyZ)/-131.00; //turn value into deg/s
  
  compangleX = (compangleX + (gyroX*looptime)/1000)*gain + (1-gain)*accelangleX;
  compangleY = (compangleY + (gyroY*looptime)/1000)*gain + (1-gain)*accelangleY;
  gyroangleZ = (gyroangleZ + (gyroZ*looptime)/1000);
  

 
  Serial.print("   | gyroangleZ = "); Serial.println(gyroangleZ);
  
  
  // GET PWM SIGNALS
  
  // PID Code 
  
  // Write to Servos/motors
  
   
  endtime = micros();
  delta = endtime - starttime;
  Serial.println(delta);
  
  if (delta < looptimemicros)
  {
    delayMicroseconds( looptimemicros - delta  );
  }
    else
    {
    Serial.println("     .....  increase delay time    ......   "); //DELETE once code is functioning 
  }
  starttime = micros();
  
} // final bracket
2

Don't use delay() or delayMicroseconds(). See how timing is managed in the demo Several Things at a Time. The same technique also works with micros().

It will automatically allow for the time taken by your code.

...R

3

Okay thanks for that I'll have a read.

Out of interest, do you know why the delay code doesn't work? Can you only put an integer into a delay() function?

5

Ok cheers, I think I understand what I need to do from the example above.

My next question is how do you mitigate overrunning the loop time?
For example if I split my code into 4 void functions that run in order, if I get to the time critical one and its just under the required loop time, then it will continue through the program. But if the other 3 functions take a period of time to run, perhaps 50% of the loop time, then by the time it returns back to the time critical function it may have overrun by nearly 50% of the loop time, and thus giving an inaccuracy.

6

Ok thoughts; could you do something like this:

void loop(){

function1();
function2();
function1();
function3();
function4();

}

And check it more than once per loop of the main program loop?

Basically 3 of the 4 functions can within reason be done whenever, they are non time critical, whereas one relies on being done at a specific time interval to provide accurate measurements.

Or the alternative is to have a dT that will change, but I read somewhere that is bad practice?

7

We just had this discussion on the (could be wrong) thread. You have two options:

  1. Use the Blink-Without-Delay technique and cut your long function down into smaller parts so that it can never exceed the time budgeted for the loop.

  2. Put your time sensitive function on a timer interrupt.

#2 is the more difficult technique. There are very few libraries to help you with this. Most of the libraries such as Timer and SimpleTimer actually use the #1 technique. Timer1 might be a useful one although I have not used it myself.

8

jaddion82052:
My next question is how do you mitigate overrunning the loop time?

...SNIP

it may have overrun by nearly 50% of the loop time, and thus giving an inaccuracy.

One possibility is that a 16MHz Arduino is not fast enough for what you want.

How often do you want the time critical function to repeat?

Is it essential for all of the other functions to run between each call to the time critical function? Maybe it would be sufficient to call one of those functions in turn between calls to the time-critical function. For example

void loop() {
   timeCrtical();
   if (count == 0) {
      otherFunctionA();
   }
   else if (count == 1) {
      otherFunctionB();
   }
   // etc
   count ++;
   if (count > maxCount) {
     count = 0;
   }
}

OR, maybe the count should only be incremented within the timeCritical() function so that it changes once for every run of the timeCritical() function?

...R

9

Robin2:
One possibility is that a 16MHz Arduino is not fast enough for what you want.

How often do you want the time critical function to repeat?

I think 16MHz should be fast enough, I just need to do it in an intelligent way.

Roughly speaking, the total loop for the 4 functions wants to be around 50mS. From the code I posted initially I'll have some interrupts to add in and then a servo write part, not sure how much or if it all that will take it over the 50mS target. I will compile everything, see how long it takes to run then that will be the loop time as the faster you sample the more accurate the data becomes.

10

All the answers posted are relevant...

using the millis() technique, you could keep more than one 'prev' value...
Then you can rank the priority of your time critical functions... calling them perhaps every 10ms, while less time sensitive processes could run every 50, or 100ms.. and some low priorities try tasks only every 1000ms or less often.

With this strategy, you can have as many layers of responsiveness - and if your 'fastest' is say every 10ms, you still have the main loop() and interrupts for very fast (microsecond) latency calls.

If you're concerned how long 'slow' functions take to execute, you can use a spare output pin to watch the entry & exit timing with a scope - which can help you determine the minimum call timing that will allow that function to execute and return.