/* 
 * author: Mohamed Hemdan
 * breif : this code for applied PID Control shuch that it take reading
 *        from IMU Mpu 9250 (Pitch & Roll & Yaw) Sensor and Ultrasonic(measure Depth)
 *        Sensor and convert it to Values for Throttle Motors 
 * 
 * LinkedIn: https://www.linkedin.com/in/mohamed-hemdan-6b9488248
 */


/*---------------------------------------*/
/*              INCLUDES                 */
/*---------------------------------------*/
#include <PID_v1_bc.h>
#include <Wire.h>
#include <Servo.h>


/*---------------------------------------*/
/*              MACROS                   */
/*---------------------------------------*/
#define PIN_ONE   (1)
#define PIN_TWO   (2)
#define PIN_THREE (3)
#define PIN_FOUR  (4)
#define PIN_FIVE  (5)
#define PIN_SIX   (6)
#define PIN_SEVEN (7)
#define PIN_EIGHT (8)
#define PIN_NINE  (9)

#define WHO_AM_I           (0x75)        // Register 117 – Who Am I
#define DEFAULT_WHO_AM_I   (0x68) // Default value of register Who am I in Mpu9250 is 0x71 & mpu6050 0x68
#define MPU9250_ADDRESS    (0x68)      // The I2C address of MPU9250.
#define PWR_MGMT_1         (0x6B)           //Register 107 – Power Management 1
#define GYRO_CONFIG        (0x1B)
#define ACCEL_CONFIG         (0x1C)

/*---------------------------------------*/
/*              DATA TYPES               */
/*---------------------------------------*/
typedef struct {
  int16_t x;
  int16_t y;
  int16_t z;
}GyroAcc_c;

typedef struct {
  float x;
  float y;
  float z;
}GyroAcc_f;

typedef enum {
  GYRO_FS_250 = 0,
  GYRO_FS_500,
  GYRO_FS_1000,
  GYRO_FS_2000
}Gyro_FS_t;

typedef enum {
  ACCEL_FS_2G = 0,
  ACCEL_FS_4G,
  ACCEL_FS_8G,
  ACCEL_FS_16G
}Accel_FS_t;

typedef struct {
  float accelRes = ACCEL_FS_2G;
  float gyroRes = GYRO_FS_250;
}Sens_Res_t;




/*---------------------------------------*/
/*           GLOBAL VARAIBLE             */
/*---------------------------------------*/
float setpoint = 0;
float actualRoll, Roll;
float actualPitch, Pitch;
float actualDepth, depthOutput, desiredDepth;  
float kp = 0, ki = 0, kd = 0;
float roll, pitch, depth;
float rowRoll, rowPitch;

GyroAcc_c GyroRaw;
GyroAcc_c AccRaw;

Sens_Res_t sensRes;

/*---------------------------------------*/
/*              ROV MOTORS               */
/*---------------------------------------*/

Servo front_right_motor; // Forward and Backward Motor   => Yaw
Servo front_left_motor;  // Forward and Backward Motor   => Yaw


Servo middle_right_motor;  // Up and Down Motor     => Roll
Servo middle_left_motor;   // Up and Down Motor     => Roll

Servo back_right_motor;  // Right and Lift Motor     => Yaw
Servo back_left_motor;   // Right and Lift Motor     => Yaw

Servo back_middle_motor;  //Tilt motor     => Pitch


/*---------------------------------------*/
/*              PID   OBJECTS            */
/*---------------------------------------*/

// PID rollPID(&actualRoll, &rollOutput, &setpoint, kp, ki, kd, DIRECT);
// PID pitchPID(&actualPitch, &pitchOutput, &setpoint, kp, ki, kd, DIRECT);
// PID depthPID(&actualDepth, &depthOutput, &desiredDepth, kp, ki, kd, DIRECT);



void setup() {

  // Initialize Serial Monitor & i2c with 400kH
  Wire.begin();
  Wire.setClock(400000);
  Serial.begin(9600);
  begin();

  setGyroFullScaleRange(GYRO_FS_250);

  /* Attach Motor*/
  front_right_motor.attach(PIN_ONE); 
  front_left_motor.attach(PIN_TWO);

  middle_right_motor.attach(PIN_THREE);
  middle_left_motor.attach(PIN_FOUR);

  back_right_motor.attach(PIN_FIVE);
  back_left_motor.attach(PIN_SIX);

  back_middle_motor.attach(PIN_SEVEN);

  

  // Initalize_Propellers(1500);

  // rollPID.SetOutputLimits(1000, 1900);

    delay(250);
}

void loop() {

ReadGyroValues();
delay(500);


}




/*--------------------------------------------------*/
/*             FUNCTION DEFINITION                  */
/*--------------------------------------------------*/

void Initalize_Propellers(int min_thorttles) {
  front_right_motor.writeMicroseconds(min_thorttles);
  front_left_motor.writeMicroseconds(min_thorttles);

  middle_right_motor.writeMicroseconds(min_thorttles);
  middle_left_motor.writeMicroseconds(min_thorttles);

  back_right_motor.writeMicroseconds(min_thorttles);
  back_left_motor.writeMicroseconds(min_thorttles);

  back_middle_motor.writeMicroseconds(min_thorttles);
}



GyroAcc_f ReadGyroValues(void)
{
  GyroAcc_c rawGyro;
  GyroAcc_f gyroValues;

  // Wire.beginTransmission(0x68);
  // Wire.write(0x1A);
  // Wire.write(0x05);
  // Wire.endTransmission();



  Wire.beginTransmission(0x68);
  Wire.write(0x43);
  Wire.endTransmission();
  Wire.requestFrom(0x68, 6);

  // while(Serial.available() < 6);

  rawGyro.x = Wire.read() << 8 | Wire.read();
  rawGyro.y = Wire.read() << 8 | Wire.read();
  rawGyro.z = Wire.read() << 8 | Wire.read();

    // Serial.print("X: "); Serial.print(rawGyro.x);
    // Serial.print("y: "); Serial.print(rawGyro.y);
    // Serial.print("z: "); Serial.println(rawGyro.z);



  //converts it to DpS;
  gyroValues.x = (float)rawGyro.x / sensRes.gyroRes;   
  gyroValues.y = (float)rawGyro.y / sensRes.gyroRes;
  gyroValues.z = (float)rawGyro.z / sensRes.gyroRes;

  Serial.print("X: "); Serial.print(gyroValues.x);
  Serial.print("\tY:"); Serial.print(gyroValues.y);
  Serial.print("\tZ:"); Serial.println(gyroValues.z);
  
  return gyroValues;
}


void setGyroFullScaleRange(Gyro_FS_t gyroRange){
  uint8_t temp = 0x00;
  Wire.beginTransmission(MPU9250_ADDRESS);
  Wire.write(GYRO_CONFIG);
  temp |= gyroRange << 3;
  Wire.write(temp);
  Wire.endTransmission();

  switch(gyroRange){
    case GYRO_FS_250:  sensRes.gyroRes = (float)131.0; break;
    case GYRO_FS_500:  sensRes.gyroRes = (float)65.5; break;
    case GYRO_FS_1000: sensRes.gyroRes = (float)32.8; break;
    case GYRO_FS_2000: sensRes.gyroRes = (float)16.4; break;

  }
}

void setAccFullScaleRange(Accel_FS_t accelRange){
  uint8_t temp = 0x00;
  Wire.beginTransmission(MPU9250_ADDRESS);
  Wire.write(ACCEL_CONFIG);
  temp |= accelRange << 3;
  Wire.write(temp);
  Wire.endTransmission();

  switch(accelRange)
    {
    case ACCEL_FS_2G:  sensRes.accelRes = (float)16384; break;
    case ACCEL_FS_4G:  sensRes.accelRes = (float)8192; break;
    case ACCEL_FS_8G:  sensRes.accelRes = (float)4096; break;
    case ACCEL_FS_16G: sensRes.accelRes = (float)2048; break;

   }
}

void begin()
{
  Wire.beginTransmission(MPU9250_ADDRESS);
  Wire.write(PWR_MGMT_1);
  Wire.write(0x00);
  Wire.endTransmission();
  setAccFullScaleRange(ACCEL_FS_2G);
  setGyroFullScaleRange(GYRO_FS_250);
  
  Serial.println("IMU Wake up Now. Be Ready..!");
  Serial.println("Gyroscope Full Scale Range is: +/-250dps");
  Serial.println("Accelerometer Full Scale Range is: +/-2G");
}