I have a mpu6050(which I got from here http://www.sainsmart.com/sainsmart-mpu-6050-3-axis-gyroscope-module.html) and I have connected it to special ports on a sensor shield made for the mpu6050(http://www.amazon.co.uk/SainSmart-InstaBots-SS-SBR-1-0-Sensor-Shield/dp/B00N1YANH2) which is on a Arduino uno. I have a sketch which is meant to print stuff out to the serial monitor, however when the mpu6050 is attached nothing is printed out and when it is not attached the Arduino does print out things to the serial monitor. I attached other things to the sensor shield and things were printed out on the serial monitor, so I know it is the mpu6050 that is the problem.
This is my code
#include <Wire.h>
#include <SPI.h>
#include <Mirf.h>
#include <nRF24L01.h>
#include <MirfHardwareSpiDriver.h>
#include <I2Cdev.h>
#include <MPU6050.h>
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
#define Gry_offset 0
#define Gyr_Gain 0.00763358
#define Angle_offset 4
#define RMotor_offset 20
#define LMotor_offset 20
#define pi 3.14159
long data;
int x, y;
float kp, ki, kd;
float r_angle, omega;
float f_angle;
float Turn_Speed = 0, Turn_Speed_K = 0;
float Run_Speed = 0, Run_Speed_K = 0, Run_Speed_T = 0;
float LOutput,ROutput;
unsigned long preTime = 0;
float SampleTime = 0.08;
unsigned long lastTime;
float Input, Output;
float errSum, dErr, error, lastErr;
int timeChange;
int TN1=3;
int TN2=4;
int ENA=9;
int TN3=5;
int TN4=6;
int ENB=10;
void setup() {
Wire.begin();
accelgyro.initialize();
pinMode(TN1,OUTPUT);
pinMode(TN2,OUTPUT);
pinMode(TN3,OUTPUT);
pinMode(TN4,OUTPUT);
pinMode(ENA,OUTPUT);
pinMode(ENB,OUTPUT);
Mirf.spi = &MirfHardwareSpi;
Mirf.init();
Mirf.setRADDR((byte *)"serv1");
Mirf.payload = sizeof(long);
Mirf.config();
Serial.begin(115200);
Serial.print("Hello");
}
void loop() {
Recive();
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
r_angle = (atan2(ay, az) * 180 / pi + Angle_offset);
omega = Gyr_Gain * (gx + Gry_offset);
//Serial.print(" omega="); Serial.print(omega);
if (abs(r_angle)<45){
myPID();
PWMControl();
}
else{
analogWrite(ENA, 0);
analogWrite(ENB, 0);
}
}
void Recive(){
if(!Mirf.isSending() && Mirf.dataReady()){
Mirf.getData((byte *)&data);
Mirf.rxFifoEmpty();
y = data >> 16;
x = data & 0x0000ffff;
//Serial.print(" x="); Serial.print(x);
//Serial.print(" y="); Serial.println(y);
if(x >= 520){
Run_Speed_K = map(x, 520, 1023, 0, 100);
Run_Speed_K = Run_Speed_K / 50;
Run_Speed = Run_Speed + Run_Speed_K;
}
else if(x <= 480){
Run_Speed_K = map(x, 480, 0, 0, -100);
Run_Speed_K = Run_Speed_K / 50;
Run_Speed = Run_Speed + Run_Speed_K;
}
else{
Run_Speed_K = 0;
}
if(y >= 520){
Turn_Speed = map(y, 520, 1023, 0, 20);
}
else if(y <= 480){
Turn_Speed = map(y,480,0,0,-20);
}
else{
Turn_Speed = 0;
}
}
else{
x = y = 500;
}
}
void myPID(){
kp = analogRead(A0)*0.1;
//kp=17;
Serial.print(" kp=");Serial.print(kp);
kd = analogRead(A2)*1.0;
//kd=840;
Serial.print(" kd=");Serial.print(kd);
//ki = analogRead(A3)*0.001; Serial.print(" ki=");Serial.print(ki);
//kp = 0; Serial.print(" kp=");Serial.print(kp);
//kd = 0; Serial.print(" kd=");Serial.print(kd);
ki = 0.08;
Serial.print(" ki=");Serial.print(ki);
unsigned long now = millis();
float dt = (now - preTime) / 1000.0;
preTime = now;
float K = 0.8;
float A = K / (K + dt);
f_angle = A * (f_angle + omega * dt) + (1 - A) * r_angle;
Serial.print(" f_angle=");Serial.print(f_angle);
timeChange = (now - lastTime);
if(timeChange >= SampleTime){
Input = f_angle;
error = Input;
errSum += error * timeChange;
dErr = (error - lastErr) / timeChange;
Output = kp * error + ki * errSum + kd * dErr;
LOutput = Output + Run_Speed + Turn_Speed; Serial.print(" LOutput=");Serial.print(LOutput);
ROutput = Output + Run_Speed - Turn_Speed; Serial.print(" ROutput=");Serial.println(ROutput);
lastErr = error;
lastTime = now;
}
}
void PWMControl(){
if(LOutput > 0){
digitalWrite(TN1, HIGH);
digitalWrite(TN2, LOW);
}
else if(LOutput < 0){
digitalWrite(TN1, LOW);
digitalWrite(TN2, HIGH);
}
else{
digitalWrite(TN1, HIGH);
digitalWrite(TN2, HIGH);
}
if(ROutput > 0){
digitalWrite(TN3, HIGH);
digitalWrite(TN4, LOW);
}
else if(ROutput < 0){
digitalWrite(TN3, LOW);
digitalWrite(TN4, HIGH);
}
else{
digitalWrite(TN3, HIGH);
digitalWrite(TN4, HIGH);
}
analogWrite(ENA, min(255, abs(LOutput) + LMotor_offset));
analogWrite(ENB, min(255, abs(ROutput) + RMotor_offset));
}
and this is a picture of the mpu6050 on the sensor shield
