Abdun Nafay
Pakistan
Asked
— Edited
Hello everyone, I had been trying for some time now and have finally come up with a code for taking values from the IMU. @DJ here it goes; I would also like you to include it in the next update of the SDK if possible.
So, to understand it step by step here is how it goes:
- The latest SDK has some issues in reading method from the I2C port, so I searched over and used one of the early EZ_B dll's. The main issue is that, in the recent sdk, the read method only takes the device address and not the register address. Now, those of you wondering what is the difference between the two, is that in the datasheet of an IMU the device address is always mentioned and the device address points to the starting register of the device. While on the other hand the register address is the register which you want to use. So, initially i declare some and initialize some variables:
byte default_addr = 104; //imu device address 0x68
int GYRO_XOUT_OFFSET;
int GYRO_YOUT_OFFSET;
int GYRO_ZOUT_OFFSET;
long GYRO_XOUT_OFFSET_1000SUM = 0;
long GYRO_YOUT_OFFSET_1000SUM = 0;
long GYRO_ZOUT_OFFSET_1000SUM = 0;
byte GYRO_XOUT_L;
byte GYRO_XOUT_H;
byte GYRO_YOUT_L;
byte GYRO_YOUT_H;
byte GYRO_ZOUT_L;
byte GYRO_ZOUT_H;
int ACCEL_XOUT;
int ACCEL_YOUT;
int ACCEL_ZOUT;
byte ACCEL_XOUT_L;
byte ACCEL_XOUT_H;
byte ACCEL_YOUT_L;
byte ACCEL_YOUT_H;
byte ACCEL_ZOUT_L;
byte ACCEL_ZOUT_H;
float ACCEL_XANGLE;
float ACCEL_YANGLE;
float ACCEL_ZANGLE;
float gyro_xsensitivity = 66.5f; //66.5 Dead on at last check
float gyro_ysensitivity = 66.5f; //72.7 Dead on at last check
float gyro_zsensitivity = 65.5f;
int GYRO_XOUT;
int GYRO_YOUT;
int GYRO_ZOUT;
float GYRO_XRATE;
float GYRO_YRATE;
float GYRO_ZRATE;
float dt = 0.05f;
float GYRO_XANGLE;
float GYRO_YANGLE;
float GYRO_ZANGLE;
float COMPLEMENTARY_XANGLE;
float COMPLEMENTARY_XANGLEPREV;
float COMPLEMENTARY_YANGLE;
float COMPLEMENTARY_YANGLEPREV;
- So, enough with the talking; moving onto taking the actual values from the imu. The first part is to check whether the I2C link is functioning or not. Now how is that done? Here is the part when you open your IMU's datasheet/Register address sheet and search around for an "WHO_AM_I" named register address. For my IMU it is 0x75. This register contains the chips I2C address/device address, which in my case is 0x68.
byte ret = ezB_Connect1.EZB.I2C.Read(device_addr, 0x75);
If this communication is successful with an output of 0x68 in the "ret" variable, then you know your chip functions partially, congratulations!
- The next step is to write to all the configuration registers:
public void setup()
{
//Sets sample rate to 8000/1+7 = 1000Hz
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x19, 0x07 });
//Disable FSync, 256Hz DLPF
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x1A, 0x00 });
//Disable gyro self tests, scale of 500 degrees/s
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x1B, 0x08 });
//Disable accel self tests, scale of +-2g, no DHPF
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x1C, 0x00 });
//Freefall threshold of |0mg|
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x1D, 0x00 });
//Freefall duration limit of 0
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x1E, 0x00 });
//Motion threshold of 0mg
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x1F, 0x00 });
//Motion duration of 0s
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x20, 0x00 });
//Zero motion threshold
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x21, 0x00 });
//Zero motion duration threshold
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x22, 0x00 });
//Disable sensor output to FIFO buffer
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x23, 0x00 });
//AUX I2C setup
//Sets AUX I2C to single master control, plus other config
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x24, 0x00 });
//Setup AUX I2C slaves
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x25, 0x00 });
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x26, 0x00 });
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x27, 0x00 });
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x28, 0x00 });
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x29, 0x00 });
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x2A, 0x00 });
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x2B, 0x00 });
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x2C, 0x00 });
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x2D, 0x00 });
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x2E, 0x00 });
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x2F, 0x00 });
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x30, 0x00 });
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x31, 0x00 });
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x32, 0x00 });
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x33, 0x00 });
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x34, 0x00 });
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x35, 0x00 });
//MPU6050_RA_I2C_MST_STATUS //Read-only
//Setup INT pin and AUX I2C pass through
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x37, 0x00 });
//Enable data ready interrupt
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x38, 0x00 });
//Slave out
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x63, 0x00 });
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x64, 0x00 });
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x65, 0x00 });
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x66, 0x00 });
//More slave config
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x67, 0x00 });
//Reset sensor signal paths
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x68, 0x00 });
//Motion detection control
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x69, 0x00 });
//Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x6A, 0x00 });
//Sets clock source to gyro reference w/ PLL
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x6B, 0x02 });
//Controls frequency of wakeups in accel low power mode plus the sensor standby modes
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x6C, 0x00 });
//Data transfer to and from the FIFO buffer
ezB_Connect1.EZB.I2C.Write(default_addr, new byte[] { 0x74, 0x00 });
}
- The setting up is done! I have also tried to outline what configuration changes I am making with every register write, in the comments above each line. The major part is done! Next is to read and convert the sensor data:
public void Calibrate_Gyros()
{
for (int i = 0; i < 50; i++)
{
GYRO_XOUT_H = ezB_Connect1.EZB.I2C.Read(default_addr, 0x43);
GYRO_XOUT_L = ezB_Connect1.EZB.I2C.Read(default_addr, 0x44);
GYRO_YOUT_H = ezB_Connect1.EZB.I2C.Read(default_addr, 0x45);
GYRO_YOUT_L = ezB_Connect1.EZB.I2C.Read(default_addr, 0x46);
GYRO_ZOUT_H = ezB_Connect1.EZB.I2C.Read(default_addr, 0x47);
GYRO_ZOUT_L = ezB_Connect1.EZB.I2C.Read(default_addr, 0x48);
GYRO_XOUT_OFFSET_1000SUM += ((GYRO_XOUT_H << 8) | GYRO_XOUT_L);
GYRO_YOUT_OFFSET_1000SUM += ((GYRO_YOUT_H << 8) | GYRO_YOUT_L);
GYRO_ZOUT_OFFSET_1000SUM += ((GYRO_ZOUT_H << 8) | GYRO_ZOUT_L);
//System.Threading.Thread.Sleep(1);
}
GYRO_XOUT_OFFSET = Convert.ToInt32(GYRO_XOUT_OFFSET_1000SUM / 50);
GYRO_YOUT_OFFSET = Convert.ToInt32(GYRO_YOUT_OFFSET_1000SUM / 50);
GYRO_ZOUT_OFFSET = Convert.ToInt32(GYRO_ZOUT_OFFSET_1000SUM / 50);
}
public void Get_Accel_Values()
{
ACCEL_XOUT_H = ezB_Connect1.EZB.I2C.Read(default_addr, 0x3B);
ACCEL_XOUT_L = ezB_Connect1.EZB.I2C.Read(default_addr, 0x3C);
ACCEL_YOUT_H = ezB_Connect1.EZB.I2C.Read(default_addr, 0x3E);
ACCEL_YOUT_L = ezB_Connect1.EZB.I2C.Read(default_addr, 0x3D);
ACCEL_ZOUT_H = ezB_Connect1.EZB.I2C.Read(default_addr, 0x3F);
ACCEL_ZOUT_L = ezB_Connect1.EZB.I2C.Read(default_addr, 0x40);
ACCEL_XOUT = ((ACCEL_XOUT_H << 8) | ACCEL_XOUT_L);
ACCEL_YOUT = ((ACCEL_YOUT_H << 8) | ACCEL_YOUT_L);
ACCEL_ZOUT = ((ACCEL_ZOUT_H << 8) | ACCEL_ZOUT_L);
}
public void Get_Accel_Angles()
{
ACCEL_XANGLE = (float)(57.295 * Math.Atan((float)ACCEL_YOUT / Math.Sqrt(Math.Pow((float)ACCEL_ZOUT, 2) + Math.Pow((float)ACCEL_XOUT, 2))));
ACCEL_YANGLE = (float)(57.295 * Math.Atan((float)-ACCEL_XOUT / Math.Sqrt(Math.Pow((float)ACCEL_ZOUT, 2) + Math.Pow((float)ACCEL_YOUT, 2))));
}
public void Get_Gyro_Rates()
{
GYRO_XOUT_H = ezB_Connect1.EZB.I2C.Read(default_addr, 0x43);
GYRO_XOUT_L = ezB_Connect1.EZB.I2C.Read(default_addr, 0x44);
GYRO_YOUT_H = ezB_Connect1.EZB.I2C.Read(default_addr, 0x45);
GYRO_YOUT_L = ezB_Connect1.EZB.I2C.Read(default_addr, 0x46);
GYRO_ZOUT_H = ezB_Connect1.EZB.I2C.Read(default_addr, 0x47);
GYRO_ZOUT_L = ezB_Connect1.EZB.I2C.Read(default_addr, 0x48);
GYRO_XOUT = ((GYRO_XOUT_H << 8) | GYRO_XOUT_L) - GYRO_XOUT_OFFSET;
GYRO_YOUT = ((GYRO_YOUT_H << 8) | GYRO_YOUT_L) - GYRO_YOUT_OFFSET;
GYRO_ZOUT = ((GYRO_ZOUT_H << 8) | GYRO_ZOUT_L) - GYRO_ZOUT_OFFSET;
GYRO_XRATE = (float)GYRO_XOUT / gyro_xsensitivity;
GYRO_YRATE = (float)GYRO_YOUT / gyro_ysensitivity;
GYRO_ZRATE = (float)GYRO_ZOUT / gyro_zsensitivity;
GYRO_XANGLE += GYRO_XRATE * dt;
GYRO_YANGLE += GYRO_YRATE * dt;
GYRO_ZANGLE += GYRO_ZRATE * dt;
}
public void Zero_Sensors()
{
float BUFFER_XANGLE = 0;
float BUFFER_YANGLE = 0;
int x = 0;
for (x = 0; x < 100; x++)
{
Get_Accel_Values();
Get_Accel_Angles();
BUFFER_XANGLE += ACCEL_XANGLE;
BUFFER_YANGLE += ACCEL_YANGLE;
//System.Threading.Thread.Sleep(1);
}
COMPLEMENTARY_XANGLE = (float)(BUFFER_XANGLE / 100.0);
COMPLEMENTARY_YANGLE = (float)(BUFFER_YANGLE / 100.0);
GYRO_XANGLE = (float)(BUFFER_XANGLE / 100.0);
GYRO_YANGLE = (float)(BUFFER_YANGLE / 100.0);
}
- Finally call the methods:
while (true)
{
if (i == 0) //only do this once
{
byte ret = ezB_Connect1.EZB.I2C.Read(default_addr, 0x75);
listBox1.Items.Add(ret);
i = 1;
setup();
Calibrate_Gyros();
}
Get_Gyro_Rates();
Zero_Sensors();
}
The code is still under some fine refinements such as adding the complementary or kalman filter to it to reduce the error. Once I write those too, I will include it as well. I am attaching the dll i have used along with the register description file of the IMU.
EZ_B(dll).zip RegisterMapandDescriptions.pdf
Enjoy!
Cool, glad you got it sorted, And thanks for sharing!
very awesome! I like your coding style also. You get A+!
Hahaha... Thanks @DJ and @Rich!
@DJ here is an image of what I recently did! Applied the complementary filter as well.
I have a concern, the readings are too slow at about 60milliseconds per read on average so it takes about a second to take readings from the imu. Is it due to the bluetooth communication or the internal clock speed?
You can do a benchmark check in ARC to find the communications limit, I'd guess it's due to the bluetooth to be honest.
So, instead of bluetooth, wifi would help then, right?
To be honest I'm not sure, a wireless connection will always be slower than a wired one.
Either way, run the benchmark first and check it out, it may not be due to the communications. I don't have an EZ-B to hand so can't check mine (although have posted it before so I'll look for the post).
Edit: I just found it... my benchmark come back with reading the ADC port 25 times per second.
Thanks alot @Rich, but what if using R/C communication would help, with about 2.4GHz transfer speed? What do you suggest?
Hello, Abdun Nafay. I am doing a segway and I am using a MPU6050, but when I set the Offset the value it is 998 (I am using at the register 0x1B a value of 0x08) so my maximun value it is 1000. So if I move on one direcction the value gorw up, but in the other side dosen´t decreases. So I was wondering wich is the value of your GYRO_XOUT_OFFSET?.
Thanks.
J
Hello guys I'm trying to use a MPU6050 i got from local store
What I'm trying to do is using the same code as @abdun-nafay but it's not working. I stucked at the first connection attempt. With latest sdk 05/2016 it reads all bytes at 255. With the one provided by @abdun-nafay the ez board will disconnect.
This is the code (using latest sdk)
where
WhoIAm = 0x75 DeviceAddr = 0x68
it could be the chip that is defected?
You'll need to replace the pullup resistors on that board, they are much too weak. The 4.7k resistors on the SDA and SCL lines will have to drop down to 330ohms. Also try to keep your wires short as possible.
Also, we need to see more code than you provided. Are you initializing the mpu? Etc
Hi. It works now. I was using the instructions from @abdun-nafay of this topic. Then I found that @dj-sures already did a great job adding everything to the windows sdk. But the way to fix it was to solder the pins to the board (i was holding them with my hand).
Now I'm having some fun with this:
I also tried using an off the shelf MPU-6050 on the first I2C port on Board 0 I used the ARC control and a separate script following the instructions in the tutorial. The ControlCommand("MPU6050", Init) command threw an error. The MPU got hot - no smoke but unusually hot. Connected SCL to SCL, SDA to SDA Any ideas? Will the EZ 9250 version EVER be available? It' been a year now...
Thanks!
Alan