I'm starting the process of building an inverted pendulum robot. I had great success with a stepper motor version designed by Joop Brooking. Search "YABR" and you will be sure to find it.
My new project will be using a Sparfun SAMD21 Mini breakout board instead of the Pro Mini used for the stepper motor bot. Again I am using the same MPU6050 as in the previous bot. I will be using brushed DC motors as well.
My first phase of testing is to make sure I can communicate with the MPU6050.
While I do find it on the bus, and can get values from it, the MSB of the gyro and Accelerometers is always 255. I verified this by just printing the MSBs from all six axis. The odd thing is, if I use the same code, modified for the differences in Serial vs SerialUSB, and sensor and only swap out the micro controller to a Pro Micro, the code works perfectly and I get values in a range that are expected. Yes, I did change the board version and recompiled in between the changes. So, the only real difference I can see is the processor version.
I'm fairly certain the issue lies in the way the Wire.h handles the data with the SAMD21.
Here is the code. (I edited the sercom portion to get exactly 400Khz on the SCL line).
There is also some code in here to read a Nunchuck if it is on the buss. Just ignore that section.
#include <Wire.h>
byte error, MPU_6050_found, nunchuck_found, lowByte, highByte;
int address;
int nDevices;
void setup()
{
Wire.begin();
//TWBR = 12; // Non SAMD21
sercom3.disableWIRE(); // Disable the I2C bus
SERCOM3->I2CM.BAUD.bit.BAUD = SystemCoreClock / ( 2 * 530000) - 1 ; // // Set the I2C SCL frequency to 400kHz
sercom3.enableWIRE(); // Restart the I2C bus
SerialUSB.begin(115200);
Serial1.begin(115200);
}
void loop()
{
SerialUSB.println("Scanning I2C bus...");
nDevices = 0;
for(address = 1; address < 127; address++ )
{
Wire.beginTransmission(address);
error = Wire.endTransmission();
if (error == 0)
{
SerialUSB.print("I2C device found at address 0x");
if (address<16)SerialUSB.print("0");
SerialUSB.println(address,HEX);
nDevices++;
if(address == 0x68 || address == 0x69){
SerialUSB.println("This could be a MPU-6050");
Wire.beginTransmission(address);
Wire.write(0x75);
Wire.endTransmission();
SerialUSB.println("Send Who am I request...");
Wire.requestFrom(address, 1);
while(Wire.available() < 1);
lowByte = Wire.read();
if(lowByte == 0x68){
SerialUSB.print("Who Am I responce is ok: 0x");
SerialUSB.println(lowByte, HEX);
}
else{
SerialUSB.print("Wrong Who Am I responce: 0x");
if (lowByte<16)SerialUSB.print("0");
SerialUSB.println(lowByte, HEX);
}
if(lowByte == 0x68 && address == 0x68){
MPU_6050_found = 1;
SerialUSB.println("Starting Gyro....");
set_gyro_registers();
}
}
if(address == 0x52){
SerialUSB.println("This could be a Nunchuck");
SerialUSB.println("Trying to initialise the device...");
Wire.beginTransmission(0x52);
Wire.write(0xF0);
Wire.write(0x55);
Wire.endTransmission();
delay(20);
Wire.beginTransmission(0x52);
Wire.write(0xFB);
Wire.write(0x00);
Wire.endTransmission();
delay(20);
SerialUSB.println("Sending joystick data request...");
Wire.beginTransmission(0x52);
Wire.write(0x00);
Wire.endTransmission();
Wire.requestFrom(0x52,1);
while(Wire.available() < 1);
lowByte = Wire.read();
if(lowByte > 100 && lowByte < 160){
SerialUSB.print("Data response normal: ");
SerialUSB.println(lowByte);
nunchuck_found = 1;
}
else{
SerialUSB.print("Data response is not normal: ");
SerialUSB.println(lowByte);
}
}
}
else if (error==4)
{
SerialUSB.print("Unknown error at address 0x");
if (address<16)
SerialUSB.print("0");
SerialUSB.println(address,HEX);
}
}
if (nDevices == 0)
SerialUSB.println("No I2C devices found\n");
else
SerialUSB.println("done\n");
if(MPU_6050_found){
SerialUSB.print("Balance value: ");
Wire.beginTransmission(0x68);
Wire.write(0x3F);
Wire.endTransmission();
Wire.requestFrom(0x68,2);
SerialUSB.println((Wire.read()<<8|Wire.read())*-1);
delay(20);
SerialUSB.println("Printing raw gyro values");
for(address = 0; address < 20; address++ ){
Wire.beginTransmission(0x68);
Wire.write(0x43);
Wire.endTransmission();
Wire.requestFrom(0x68,6);
while(Wire.available() < 6);
SerialUSB.print("Gyro X = ");
SerialUSB.print(Wire.read()<<8|Wire.read());
SerialUSB.print(" Gyro Y = ");
SerialUSB.print(Wire.read()<<8|Wire.read());
SerialUSB.print(" Gyro Z = ");
SerialUSB.println(Wire.read()<<8|Wire.read());
Serial1.println("Bang!!!");
}
SerialUSB.println("");
}
else SerialUSB.println("No MPU-6050 device found at address 0x68");
if(nunchuck_found){
SerialUSB.println("Printing raw Nunchuck values");
for(address = 0; address < 20; address++ ){
Wire.beginTransmission(0x52);
Wire.write(0x00);
Wire.endTransmission();
Wire.requestFrom(0x52,2);
while(Wire.available() < 2);
SerialUSB.print("Joystick X = ");
SerialUSB.print(Wire.read());
SerialUSB.print(" Joystick y = ");
SerialUSB.println(Wire.read());
delay(100);
}
}
else SerialUSB.println("No Nunchuck device found at address 0x52");
//while(1); //Comment out to allow continuous testing
delay(1500); // Add to ing evry 1.5 sec.
}
void set_gyro_registers(){
//Setup the MPU-6050
Wire.beginTransmission(0x68); //Start communication with the address found during search.
Wire.write(0x6B); //We want to write to the PWR_MGMT_1 register (6B hex)
Wire.write(0x00); //Set the register bits as 00000000 to activate the gyro
Wire.endTransmission(); //End the transmission with the gyro.
Wire.beginTransmission(0x68); //Start communication with the address found during search.
Wire.write(0x1B); //We want to write to the GYRO_CONFIG register (1B hex)
Wire.write(0x00); //Set the register bits as 00000000 (250dps full scale)
Wire.endTransmission(); //End the transmission with the gyro
Wire.beginTransmission(0x68); //Start communication with the address found during search.
Wire.write(0x1C); //We want to write to the ACCEL_CONFIG register (1A hex)
Wire.write(0x08); //Set the register bits as 00001000 (+/- 4g full scale range)
Wire.endTransmission(); //End the transmission with the gyro
Wire.beginTransmission(0x68); //Start communication with the address found during search
Wire.write(0x1A); //We want to write to the CONFIG register (1A hex)
Wire.write(0x03); //Set the register bits as 00000011 (Set Digital Low Pass Filter to ~43Hz)
Wire.endTransmission(); //End the transmission with the gyro
}
