#include <Wire.h>
#include <MPU6050.h>
#define motor1_pin1 5 //PWM - Digital Pins
#define motor1_pin2 6 //PWM - Digital Pins
#define motor2_pin1 9 //PWM - Digital Pins
#define motor2_pin2 10 //PWM - Digital Pins
MPU6050 gy_521;
int16_t ax, ay, az;
int16_t gx, gy, gz;
int motor1_speed1;
int motor2_speed1;
int motor1_speed2;
int motor2_speed2;
void setup ( ){
Wire.begin( );
Serial.begin (9600);
Serial.println ("Initializing MPU and testing connections");
gy_521.initialize ( );
Serial.println(gy_521.testConnection( ) ? "Successfully Connected" : "Connection failed");
delay(1000);
Serial.println("Reading Values");
delay(1000);
}
void loop ( ){
gy_521.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
ax = map(ax, -17000, 17000, -125, 125);
ay = map(ay, -17000, 17000, -125, 125);
motor1_speed1 = 125+ax; //To move first motor
motor2_speed1 = 125-ax; //To move second motor
motor1_speed2 = 125+ay; //To move first motor
motor2_speed2 = 125-ay; //To move second motor
Serial.print ("Motor1 Speed1 = ");
Serial.print (motor1_speed1, DEC);
Serial.print (" && ");
Serial.print ("Motor2 Speed1 = ");
Serial.println (motor2_speed1, DEC);
Serial.print ("Motor1 Speed2 = ");
Serial.print (motor1_speed2, DEC);
Serial.print (" && ");
Serial.print ("Motor2 Speed2 = ");
Serial.println (motor2_speed2, DEC);
analogWrite (motor1_pin1, motor1_speed1);
analogWrite (motor1_pin2, motor2_speed1);
analogWrite (motor2_pin1, motor1_speed2);
analogWrite (motor2_pin2, motor2_speed2);
delay(200);
}