#include "ESP8266WiFi.h"
#include "WiFiUDP.h"
#include "OSCMessage.h"
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"

//WiFiの定義
IPAddress myIP(192, 168, 4, 1);
IPAddress HOSTIP (192, 168, 4, 2);
const char *ssid = "TROBOT";
const char *password = "12345678";
int localPort = 8000;
WiFiUDP Udp;

//ジャイロセンサーの定義
MPU6050 accelgyro;
MPU6050 initialize;
int16_t ax, ay, az;
int16_t gx, gy, gz;
#define Gyr_offset 0.0  //The offset of the gyro
#define Gyr_Gain 16.348
#define Driving_Angle  45
#define pi 3.1415692
float Angle_offset = -4.8 ;

//機体の定義
float kp, ki, kd;
float Angle_Raw, Angle_Filtered, omega;
float Turn_Speed = 0, Turn_Speed_K = 0;
float Run_Speed = 0, Run_Speed_K = 0;
float LOutput,ROutput;
float Input, Output;
float errSum, dErr, error, lastErr;
unsigned long preTime = 0;
unsigned long lastTime;
int timeChange;
int led = 12; // Head Lump
int distance;
int value;

//モータードライバDRV8835の接続
int AN1 = 14;
int AN2 = 13;
int BN1 = 2;
int BN2 = 16;

void setup() {
  WiFi.softAP(ssid, password);
  IPAddress myIP = WiFi.softAPIP();
  Udp.begin(localPort);
  Serial.begin(115200);
  int tries = 0;
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    tries++;
    if (tries > 30) {
      break;
    }
  }
  Wire.begin();
  accelgyro.initialize();
  initialize.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
  for(int i = 0; i < 200; i++) {
    Filter();
   }
  if (abs(Angle_Filtered) < Driving_Angle) {
    omega = Angle_Raw = Angle_Filtered = 0;
    Filter();
    Output = error = errSum = dErr = Run_Speed = Turn_Speed  = 0;
    myPID();
  }
  pinMode(AN1, OUTPUT);
  pinMode(AN2, OUTPUT);
  pinMode(BN1, OUTPUT);
  pinMode(BN2, OUTPUT);
  pinMode(A0,OUTPUT);
  }

  void OSCMsgReceive() {
  OSCMessage msgIN;
  int size;
  if ((size = Udp.parsePacket()) > 0) {
    while (size--)
      msgIN.fill(Udp.read());
    if (!msgIN.hasError()) {
       msgIN.route("/micro_BR/fader1",fader1);//turn left & right
       msgIN.route("/micro_BR/fader2",fader2);//forward & reverse
    }
  }
}

 void fader1(OSCMessage &msg, int addrOffset ){ //turn left & right
  int value1 = msg.getFloat(0);
  delayMicroseconds(10);
  if (value1 >= 145) {
    Turn_Speed = map(value1, 145, 255, 0, 125);
    }
  else if (value1 <= 110) {
    Turn_Speed = map(value1, 110, 0, 0, -125);
    }
  else {
    Turn_Speed = 0;
  }
}

 void fader2(OSCMessage &msg, int addrOffset) { //forward & reverse
  int value2 = msg.getFloat(0);
  delayMicroseconds(10);
  if (value2 >= 145) {
    Run_Speed_K = map(value2, 145, 255, 0, 125);
    Run_Speed += Run_Speed_K;
  }
  else if (value2 <= 110) {
    Run_Speed_K = map(value2, 110, 0, 0, -125);
    Run_Speed += Run_Speed_K;
  }
  else {
    Run_Speed_K = 0;
  }
}

 void loop() {
  OSCMsgReceive();
  Filter();
  Serial.print("  Angle = ");
  Serial.println(Angle_Filtered);
  if (abs(Angle_Filtered) < Driving_Angle)  {
    myPID();
    PWMControl();
    LED_ONOFF();
   }
  else  {  // Stop the wheels
    analogWrite(BN2, 0);
    analogWrite(AN2, 0);
   for(int i = 0; i < 100; i++)  {
    Filter();
    }
   if(abs(Angle_Filtered) < Driving_Angle) {
   for(int i = 0; i <= 500; i++)  {
   omega = Angle_Raw = Angle_Filtered = 0;
   Filter();
    Output = error = errSum = dErr = Run_Speed = Turn_Speed = 0;
    myPID();
      }
    }
   }
   distance_from_object();
  if( distance < 10){
     for(int i = 0; i <= 1
     00; i++)  {
    digitalWrite(AN1, HIGH);
    digitalWrite(BN1, LOW);
    analogWrite(AN2, 150 );
    analogWrite(BN2, 150 );
    Filter();
    myPID();
     }
   }
  }

void Filter() {
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  Angle_Raw = (atan2(ay, az) * 180/ pi) + Angle_offset;
  omega =  gx / Gyr_Gain + Gyr_offset;
  unsigned long now = millis();
  float dt = (now - preTime) / 1000.00;
  preTime = now;
  float K = 0.8;
  float A = K / (K + dt);
  Angle_Filtered = A * (Angle_Filtered + omega * dt) + (1 - A) * Angle_Raw;
 }

void myPID() {
  //PID係数
  kp = 20;
  ki = 0.40;
  kd = 250;
 // Calculating the output values using the PID values.
  unsigned long now = millis();
  timeChange = (now - lastTime);
  lastTime = now;
  error = Angle_Filtered;  // Proportion
  errSum += error * timeChange;  // Integration
  dErr = (error - lastErr) / timeChange;  // Differentiation
  Output = kp * error + ki * errSum + kd * dErr;
  lastErr = error;
  LOutput = Output + Turn_Speed + Run_Speed;
  ROutput = Output - Turn_Speed + Run_Speed;
}

void PWMControl() {
  if (LOutput > 0) {
    digitalWrite(BN1, HIGH);
  }
  else if (LOutput < 0) {
    digitalWrite(BN1, LOW);
  }
  else {
    digitalWrite(BN1, LOW);
  }
  if (ROutput > 0) {
    digitalWrite(AN1, HIGH);
  }
  else if (ROutput < 0) {
    digitalWrite(AN1, LOW);
  }
  else {
    digitalWrite(AN1, LOW);
  }
  float Lrpm = min(600, abs(LOutput) );
  float Rrpm = min(600, abs(ROutput) );
  analogWrite(BN2, Lrpm);
  analogWrite(AN2, Rrpm);
 }

 void LED_ONOFF() {
   if(millis() % 500 > 250) {
    analogWrite(led, 255);
  }
  else {
    analogWrite(led, 0);
  }
 }

 void distance_from_object(){  //測距
  value = analogRead(A0);
  distance = (6787/(value-3))-4;
  Serial.print(distance);
  Serial.println("cm");
 }