I've done motors and encoders test, imu test and port manipulation test and everything is OK. So IMU is working. I use dmp sensor fusing to combine values for a better result.
I've attached my code in bb.ino. I've lowered fifo overflow down to 25Hz and then 20Hz from the initial 100Hz but always getting fifo overflow. As said in the comments my code is unefficient so I think code I add is slowing down IMU and nothing works. I've also implemented direct port access to save processor power.
Code is debugged. It's driving me crazy.... How can I make code more efficient?
15 K is too much code to figure out without guidance about the specific problem area.
Presumably your FIFO overflows either because you are not emptying it fast enough or you don't have code to discard new values when the buffer is full (or both).
Robin2:
15 K is too much code to figure out without guidance about the specific problem area.
Seems not to be a specific problem area. IMU works so well without motors settings and commands, but goes
in overflow otherway. I've thought about every possible problem, BUT IMU alone seems to have perfect behaviour.
I've tried to lower fifo frequency, but it does not make sense because it works so well even at 100Hz and it overflows even at 20 or 25 Hz.
Robin2:
Presumably your FIFO overflows either because you are not emptying it fast enough or you don't have code to discard new values when the buffer is full (or both).
What do I have to do?
Don't really understand: single codes are working well, very well. All together, arduino mega LEDs are blinking fast for 5s, then faster for 3, then there's CRASH.
SO FAR The problem occurs when motors settings and pwm commands slow the code down enough that the MPU can't be read fast enough to empty the FIFO buffer and an overflow occurs.
When not using these commands, it reads fine, but while using anything, overflows occur. It is running at 20Hz and this still occurs.
I don't know because you have not provided enough information.
For myself I have no intention of spending an hour or two studying 15k of code in the hope that I might see a problem. Sorry - I'm probably lazy. And I would prefer to devote some of that time to answering other questions.
If it was my project I would try to isolate the section of code, or the specific activity that is causing the problem. Perhaps comment-out calls to some functions and see what happens.
Are you using the delay() function in your program? If so get rid of it and use millis() to manage timing without blocking.
Can you write a short program that illustrates the problem and post that?