Gyro and Accelerometer-Based Toy Car Direction Control

// www.matthewtechub.com
// Control Toy Car Speed and Direction with MPU6050 Tilt
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

Adafruit_MPU6050 mpu;

// Define motor control pins for Left Motor (Motor 1)
const int motor1PWM = 10;  // PWM pin for speed control (Left motor)
const int motor1Dir1 = 9;  // Direction pin 1 (Left motor)
const int motor1Dir2 = 8;  // Direction pin 2 (Left motor)

// Define motor control pins for Right Motor (Motor 2)
const int motor2PWM = 5;   // PWM pin for speed control (Right motor)
const int motor2Dir1 = 6;  // Direction pin 1 (Right motor)
const int motor2Dir2 = 7;  // Direction pin 2 (Right motor)

void setup() {
  Serial.begin(9600);
  while (!Serial)
    delay(10); // Wait for Serial Monitor to open

  // Initialize motor control pins
  pinMode(motor1PWM, OUTPUT);
  pinMode(motor1Dir1, OUTPUT);
  pinMode(motor1Dir2, OUTPUT);
  
  pinMode(motor2PWM, OUTPUT);
  pinMode(motor2Dir1, OUTPUT);
  pinMode(motor2Dir2, OUTPUT);

  // Try to initialize the MPU6050
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

  // Set the accelerometer range
  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);

  // Set the gyro range
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);

  // Set the filter bandwidth
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  delay(100);
}

void loop() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Calculate tilt angles in degrees for X and Y axes
  float angleX = atan2(a.acceleration.y, a.acceleration.z) * 180.0 / PI;
  float angleY = atan2(-a.acceleration.x, a.acceleration.z) * 180.0 / PI;

  // Print tilt angles
  Serial.print("Tilt Angle X: ");
  Serial.print(angleX);
  Serial.print("° | Tilt Angle Y: ");
  Serial.print(angleY);
  Serial.println("°");

  // Control car direction and speed
  if (angleY > 5) {
    // Forward movement
    moveForward(angleY);
  } 
  else if (angleY < -5) {
    // Backward movement
    moveBackward(angleY);
  } 
  else {
    // Stop motors when tilt is between -5° and +5°
    stopMotors();
  }

  // Turning control based on X-axis tilt
  if (angleX > 5) {
    // Right turn
    turnRight(angleX);
  } 
  else if (angleX < -5) {
    // Left turn
    turnLeft(angleX);
  }

  delay(100);
}

// Function to move forward
void moveForward(float angleY) {
  digitalWrite(motor1Dir1, HIGH);
  digitalWrite(motor1Dir2, LOW);
  digitalWrite(motor2Dir1, HIGH);
  digitalWrite(motor2Dir2, LOW);

  int speed = map(angleY, 5, 90, 0, 255); // Map tilt angle to PWM range
  analogWrite(motor1PWM, speed);
  analogWrite(motor2PWM, speed);
  Serial.println("Moving forward with speed: " + String(speed));
}

// Function to move backward
void moveBackward(float angleY) {
  digitalWrite(motor1Dir1, LOW);
  digitalWrite(motor1Dir2, HIGH);
  digitalWrite(motor2Dir1, LOW);
  digitalWrite(motor2Dir2, HIGH);

  int speed = map(angleY, -5, -90, 0, 255); // Map tilt angle to PWM range
  analogWrite(motor1PWM, speed);
  analogWrite(motor2PWM, speed);
  Serial.println("Moving backward with speed: " + String(speed));
}

// Function to stop the motors
void stopMotors() {
  analogWrite(motor1PWM, 0);
  analogWrite(motor2PWM, 0);
  Serial.println("Motors stopped.");
}

// Function to turn right
void turnRight(float angleX) {
  // Slow down left motor to turn right
  int speed = map(angleX, 5, 90, 255, 0); // Reduce left motor speed based on tilt angle
  analogWrite(motor1PWM, speed);
  Serial.println("Turning right with adjusted speed.");
}

// Function to turn left
void turnLeft(float angleX) {
  // Slow down right motor to turn left
  int speed = map(angleX, -5, -90, 255, 0); // Reduce right motor speed based on tilt angle
  analogWrite(motor2PWM, speed);
  Serial.println("Turning left with adjusted speed.");
}