// speedSpeedometerStepperServo.pde
// -*- mode: C++ -*-
//
// Use a Potentiometer to control the speed of several motors:
// a stepper motor with drivers with speed proportional to pot
// a stepper motor with drivers with position proportional to pot
// a servo motor porportional to pot
// a led/DC motor proportional to pot
// a led/DC motor proportional and opposite to pot
// leds/reversibleDC motor proportional to bidirectioal pot 
//

// Sim: https://wokwi.com/projects/330649945363186260
// Discussion: https://discord.com/channels/787627282663211009/787630013658824707/957769150556667954
//         and https://github.com/wokwi/wokwi-features/issues/191
// Docs:  https://docs.wokwi.com/parts/wokwi-stepper-motor
//   
// See also: https://wokwi.com/projects/327381547863769683 W/o drivers
//           https://wokwi.com/projects/327613078439985746 MultiStepper
// 

#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <MPU6050.h>
#include <AccelStepper.h>
#include <Servo.h>
#include <Ultrasonic.h>

// Define LCD properties
LiquidCrystal_I2C lcd(0x27, 20, 4); // Change the address and dimensions as needed

// Define MPU6050 object
MPU6050 mpu;

// Define motor pins
  
const byte ledUpPin = 2;  
const byte ledDownPin = 6; 
const byte dirPin = 11;  
const byte stepPin = 12;
const byte dirPin1 = 5;  
const byte stepPin1 = 8;

// Define motor variables
const int step_per_rev = 200;
AccelStepper stepper(AccelStepper::DRIVER, stepPin, dirPin);
AccelStepper stepper2(AccelStepper::DRIVER, stepPin1, dirPin1);

// Define servo pin
const byte servoPin = 3;
Servo myservo;

// Define HC-SR04 pins
const byte trigPin = 10;
const byte echoPin = 9;
Ultrasonic ultrasonic(trigPin, echoPin);

// Variables for motor speed and RPM
float motorSpeed = 0;
float motorRPM = 0;
float maxRPM = 50;

void setup() {
  // Initialize Serial and I2C communication
  Serial.begin(115200);
  Wire.begin();
  
  // Initialize LCD
  lcd.init();
  lcd.backlight();
  lcd.clear();
  
  // Initialize MPU6050
  mpu.initialize();
  
  // Initialize stepper motors
  stepper.setMaxSpeed(2000);
  stepper.setSpeed(0);
  stepper2.setMaxSpeed(2000);
  stepper2.setSpeed(0);
  
  // Initialize servo
  myservo.attach(servoPin);
}


void loop() {
  // Read accelerometer and gyroscope values from MPU6050
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  
  // Read motor speed and calculate RPM
  int potState = analogRead(A0);
  motorSpeed = map(potState, 0, 1023, 0, 255);
  motorRPM = (motorSpeed / 255.0) * maxRPM;
  
  // Read distance from ultrasonic sensor
  long distance = ultrasonic.read(CM);
  
  // Display values on LCD
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Motor RPM: ");
  lcd.print(motorRPM);
  
  lcd.setCursor(0, 1);
  lcd.print("Acc: ");
  lcd.print(ax);
  lcd.print(", ");
  lcd.print(ay);
  lcd.print(", ");
  lcd.print(az);
  
  lcd.setCursor(0, 2);
  lcd.print("Gyro: ");
  lcd.print(gx);
  lcd.print(", ");
  lcd.print(gy);
  lcd.print(", ");
  lcd.print(gz);
  
  lcd.setCursor(0, 3);
  lcd.print("Distance: ");
  lcd.print(distance);
  lcd.print(" cm");
  
  // Check obstacle distance and control motors accordingly
  if (distance < 50) {
    digitalWrite(ledDownPin, LOW); 
    digitalWrite(ledUpPin, HIGH);
  } else {
    digitalWrite(ledDownPin, HIGH);
    digitalWrite(ledUpPin, LOW);
  }
  
  
  delay(500); // Adjust delay as needed
}