I am currently trying to build an inverted pendulum and am using SimpleFOCShield 2.0.1 to control my motor. The motor works fine, but the gy-521 that should provide the angle of the pendulum stops working when I add the SimpleFOC motor, encoder and driver code. When I comment out the SimpleFOC stuff, the angle sensor works just the way I want it to, but when the code runs alongside it, the angles get all weird. The gy-521 uses the Arduino-Uno analog pins A4 and A5 (or the SCL and SDA), so I was wondering if SimpleFOC interferes in any way with those pins. The angle I get isn’t noisy or anything, it looks like the original angle is multiplied by a factor (for example, if I moved the pendulum 10 degrees, the angle would output that it moved 180 degrees).
For the driver, I use the recommended pinout with PWM pins 9, 5, and 6. For the encoder (AMT103-V), I use interrupt pins 2 and 3.
BLDCDriver3PWM Driver = BLDCDriver3PWM(9, 5, 6, 8);
Encoder = Encoder(2, 3, 500);
For the gy-521 i use the TinyMPU6050.h lib. Unfortunatly i dont know much about that lib or the lines of code that make it work.
This is used for init:
MPU6050 mpu (Wire);
This will be called to get the information:
pendulumAngleZ = mpu.GetAngZ();
pendulumAngleVelocityZ = mpu.GetGyroZ();
I hope it’s enough information to work with and thank you all in advance.
That library does acc/gyro fusion in software. It probably expects that execute() is called regularly as it needs to integrate the gyro velocity to get accurate angle.
I wonder whether simplefoc is slowing the loop time to the point where gyro isn’t called often enough. A library that uses mpu6050’s digital motion processor might help as the fusion is done in hardware and thus not affected by slow loop().
I guess the motor could also be adding vibration that is making the gyros job much harder.
I dont think that a slow loop is the problem, cause if i only use the init of the motor and driver i get this problem, the motor or driver dont even have to be involved in the loop. Plus im already using MPU6050 for the acc/gyro calculations. The vibration can be crosst out as well, cause the motor isnt moving and the problem still persists .
My best guess would be that the SimpleFOC shield is doing something on the same pins. My driver is using the PWM pins 9, 5 and 6 and my gy-521 is using analogpins A4 and A5. Does someone know if these pins are connected in anyway or cant be used all at the same time?
SimpleFOCShield does not change those pins you can use them without any problem with the shield. It even provides you pull-up resistors if you need some.
I think the reason why this happens is as @Owen_Williams suggested due to the software sensor fusion done in the TinyMPU6050 library. The reason why it misbehaves is because in order to have a high frequency PWM we have to chnage the Arduino’s time settings and the library in question uses the
micros() functions in the code which will have wrong values due the reconfiguration of the timers.
There are two things you can do:
- Do not use pins 5 and 6 for the PWM (they belong to the timer 1 which is responsible for keeping time -
- If you have to use pins 5 and 6, then go into the TinyMPU6050 and either use the wrapper funcitons
_micros() that are provided by the
SimpleFOC.h or if that is too complicated just divide every
millis call by 32.
Hope it helps!
Yes, it did help and thank you very much for the answer. I changed the TinyMPU6050 calls of millis() to _millis() (after i implemented it cause SimpleFOC’s time_utils.h only provides _micros()). I have also changed the delay() calls in TinyMPU to _delay() and the angles are messured correcly again. So that problem is out of the way, however i got a new one.
For some reason i cant run the motor on higher speeds for long with the agnle messure code. If i messure the angle and just let the motor run on 5V its crashes the programm. But if i run the same code but with the modification that i run the motor on 2V it works and gives the right angles.
Any ideas why the programm would crash just because of higher motor speed ?
i noticed that you used the same MPU6050 board to get the angle of your balancer robot in your newest video. Im having lots of truble getting it to work properly with my simpleFOCShield. I have tried so much and it just doesnt work consistently. Some times i can run a test with both the motor and the MPU working the way they should, but other times (or majority of times) the programm will just deadlock somewhere or the MPU will stop getting new angle values and instead just gives an old one over and over.
I tried running the script you uploaded for the balancer video, but its giving me the same problems again and again, seemingly at random. Im using the same motor you are using and the simpleFOCShield2.0.1 the same SimpleFOC.h and a nucleo F411RE whitch you said you tested outside of the video. I also use the I2C pullups for the MPU. I use the same pwm and encoder pins, but all that does not show the same results.
When i use another encoder (AMT-103) for the angle of the pendulum like you did in your pendulum project it all works like a charm, but i can only use that for the prototype and it doesnt help for the final thing i wanted to make.
Sorry for the late reply.
I’ve had a lot of trouble with I2C communication in myself as well. That is the reason I’ve put my mpu6050 on the top of the robot.
In my opinion the reason is the electromagnetic field that is being generated by the motors and the drivers.
I2C communication is very valnerable to it unfortunately.
If you look in the balancer video,you’ll see that I’ve twisted the wire to reduce the emf effect, it did help a bit but did not remove the problem.
Honestly, i am not sure how to resolve this problem. Most of my imu + motor projects have had the same issues, dc/stepper or bldc motor based.
So what you can try is to limit the voltage
motor.voltage_limit to 5-6volts and see if it helps. Aslo try to reduce the wire length, this might help.
If someone has some suggestions if be happy to hear about them
i’ll try the twisting and if that doesnt do it i think i got some shielded wires around somewhere and try those out. Thanks for the info. I didnt think the effect would be so massiv on the IMU cause the motor isnt drawing that much current.
- As you say Antun, twisting wires - SDA with GND and SCL with VCC for example. Not SDA with SCL because they aren’t differential
- Keep cables short as possible
- clean cabling in general - try to route it away from the BLDC phases, make sure it isn’t being pulled around as things move, etc…
- Use 5V if its an option, rather than 3.3
- Use a I2C buffer or I2C bus accelerator - they add drive capacity to the I2C bus and ensure nice waveforms/transitions.
- But maybe it is also power-related rather than EMI? If the higher voltages (more power) cause voltage drops on the supply, or couple too much noise into the ground?
That’s what I can think of atm…
A few suggestions from my own experience when building a balancing robot with a MPU6050:
- For my last mechatronics course at school last month, the school received a faulty batch of MPU6050s, maybe yours is also faulty!
- If I powered the MPU6050 directly from an arduino, then I got lots of I2C communication errors. When I feeded it direcly from the 5V battery, these I2C errors dissapeared. So check if your MPU6050 receives enough power.
Strange. I do not have any problems with MPU6050. But I not use any library for this. I read the registers directly.
Can you share your code for this? Or any link on tutorial about direct registers working ?