Hello,
So I am doing a project about a 4-wheel car that avoids obstacles with an ultrasonic sensor. When the ultrasonic sensor detects an obstacle near, the motors stop and the car rotates 90º. Here is where the MPU 6050 sensor comes in. I want to rotate 90º exacly so I am perpendicular to the object. I have found several webs and videos where the connection of the sensor uses digital 2 for the INT pin, and Analog pins 4 and 5. But I do not get to make the sensor to work properly. It prints nan.
I am using an Arduino Mega
The code:
#define echoPin 2 // attach pin D2 Arduino to pin Echo of HC-SR04
#define trigPin 3 //attach pin D3 Arduino to pin Trig of HC-SR04
#include <sSoftSerial.h> // We include another Serial port for the board
#include <Servo.h> // We include the servo library
#include <Wire.h> //This library allows communication between the Arduino board and other I2C devices, such as the MPU-6050
#include <I2Cdev.h> // This library provides a set of functions for working with I2C devices
#include <MPU6050.h> //This library is specifically designed for interfacing with the MPU-6050 accelerometer and gyroscope sensor.
sSoftSerial Bluetooth(7, 8); // RX, TX where the bluetooth module will be connected
Servo myservo; // We create a servo object to control the servo
MPU6050 mpu; //We create an object for the IMU sensor
// We define the variables for the ultrasonic sensor
long duration; // variable for the duration of sound wave travel
int distance; // variable for the distance measurement
//define variables for servo
int pos; // variable to read the value from the analog pin
//Motors inputs
int motor1pin1 = 10;
int motor1pin2 = 11;
int motor2pin1 = 12;
int motor2pin2 = 13;
int ax, ay, az;
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void setup() {
Serial.begin(9600); // // Serial Communication is starting with 9600 of baudrate speed
Bluetooth.begin(9600); // Open Serial Port for Bluetooth with 9600 of baudrate speed
Bluetooth.println("Connexion established");
Wire.begin(); //Initialize I2C
mpu.initialize(); //Initialize sensor
if(mpu.testConnection()){
Serial.println("Sensor initialize correctly");
Bluetooth.println("Sensor initialize correctly");
}
else{
Serial.println("Error to initialize sensor");
Bluetooth.println("Error to initialize sensor");
}
pinMode(trigPin, OUTPUT); // Sets the trigPin as an OUTPUT
pinMode(echoPin, INPUT); // Sets the echoPin as an INPUT
myservo.attach(4); // attaches the servo on pin 4 to the servo object
myservo.write(120);
//pinMode(13, OUTPUT); //Sets digital pin 13 as output pin
//Motor 1:
pinMode(motor1pin1, OUTPUT);
pinMode(motor1pin2, OUTPUT);
pinMode(5, OUTPUT); //ENA2 Pin
//Motor 2:
pinMode(motor2pin1, OUTPUT);
pinMode(motor2pin2, OUTPUT);
pinMode(6, OUTPUT); // ENA1 Pin
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void loop() {
mpu.getAcceleration(&ax, &ay, &az); //Read accelerations
//Calculate inclination angles
float accel_ang_x = atan(ax/sqrt(pow(ay,2) + pow(az,2))) * (180.0/3.14);
float accel_ang_y = atan(ay/sqrt(pow(ax,2) + pow(az,2))) * (180.0/3.14);
//Show angles
Serial.print("Inclination in X");
Serial.print(accel_ang_x);
Serial.print("Inclination in Y");
Serial.println(accel_ang_y);
distance = DistanceSensor();
delay(10);
Forward();// Motors moving forward
if(distance < 25){
SlowForward();
}
if(distance < 13){
Serial.println("OBJECT DETECTED IN FRONT");
Bluetooth.println("OBJECT DETECTED IN FRONT");
//Paramos motores 1 y 2
Stop();
Serial.println("WE STOP THE MOTORS");
Bluetooth.println("WE STOP THE MOTORS");
delay(2000);
Backwards();
Serial.println("WE GO BACKWARDS");
Bluetooth.println("WE GO BACKWARDS");
delay(500);
Stop();
Serial.println("WE STOP THE MOTORS");
Bluetooth.println("WE STOP THE MOTORS");
delay(1000);
//We rotate 90º exacly to the left using the IMU sensor to be more precisely
Serial.println("WE ROTATE 90º TO THE LEFT");
Bluetooth.println("WE ROTATE 90º TO THE LEFT");
RotarI();
delay (500);
Stop();
myservo.write(20);
delay(2000); //We wait 2 seconds for the servo to rotate
//Going forward:
SlowForward();
//Una vez el rover se haya movido lo suficiente el obstaculo ya no estara a nustra derecha entonces la distancia sera mayor, no habra nada delante
do{
distance = DistanceSensor(); //We call the sensor function and we get the distance to object
}while(distance < 20);
//In this point the object won't be more in his view
//Stop mootors
delay(1000); //To be sure we pass the object
Stop();
delay(1000);
RotarD();
delay(2500);
Stop();
Serial.println("Lee hasta aqui");
myservo.write(120);
delay(2000); //Time for servo to rotate
//Going forward:
Forward();
}
}
void Forward(){
digitalWrite(motor1pin1, HIGH);
digitalWrite(motor1pin2, LOW);
analogWrite(5, 255); //Velocity of motors
digitalWrite(motor2pin1, HIGH);
digitalWrite(motor2pin2, LOW);
analogWrite(6, 255); //Velocity of motors
}
void SlowForward(){
digitalWrite(motor1pin1, HIGH);
digitalWrite(motor1pin2, LOW);
analogWrite(5, 155); //Velocity of motors
digitalWrite(motor2pin1, HIGH);
digitalWrite(motor2pin2, LOW);
analogWrite(6, 155); //Velocity of motors
}
void Stop(){
digitalWrite(motor1pin1, LOW);
digitalWrite(motor1pin2, LOW);
digitalWrite(motor2pin1, LOW);
digitalWrite(motor2pin2, LOW);
}
void RotarI(){
//Movemos hacia adelante los dos motores de la derecha para rotar
digitalWrite(motor1pin1, HIGH);
digitalWrite(motor1pin2, LOW);
analogWrite(5, 150); //Velocity of motors
//Movemos hacia atras los dos motores de la izquierda para rotar
digitalWrite(motor2pin1, LOW);
digitalWrite(motor2pin2, HIGH);
analogWrite(6, 150); //Velocity of motors
}
void RotarD(){
//Movemos hacia adelante los dos motores de la derecha para rotar
digitalWrite(motor1pin1, LOW);
digitalWrite(motor1pin2, HIGH);
analogWrite(5, 150); //Velocity of motors
//Movemos hacia atras los dos motores de la izquierda para rotar
digitalWrite(motor2pin1, HIGH);
digitalWrite(motor2pin2, LOW);
analogWrite(6, 150); //Velocity of motors
}
void Backwards(){
digitalWrite(motor1pin1, LOW);
digitalWrite(motor1pin2, HIGH);
analogWrite(5, 100); //Velocity of motors
digitalWrite(motor2pin1, LOW);
digitalWrite(motor2pin2, HIGH);
analogWrite(6, 100); //Velocity of motors
}
int DistanceSensor(){
// Clears the trigPin condition
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
// Sets the trigPin HIGH (ACTIVE) for 10 microseconds
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
// Reads the echoPin, returns the sound wave travel time in microseconds
duration = pulseIn(echoPin, HIGH);
// Calculating the distance
distance = duration * 0.0339 / 2; // Speed of sound wave divided by 2 (go and back)
// Displays the distance on the Serial Monitor
Serial.print("Distance: ");
Serial.print(distance);
Serial.println(" cm");
//Displays the distance on the Bluethooth Monitor
Bluetooth.print("Distance: ");
Bluetooth.print(distance);
Bluetooth.println(" cm");
return distance;
}