/*このスケッチを丸ごとコピーするだけでは再現できません。
特に倒立は、フレームの形状や使用部品及び部品配置によってバランスが異なり、
安定させるには機体に合わせた設定値の変更が必要です。
------------------------------------------------------------------------------------*/
#include "ESP8266WiFi.h"
#include "WiFiUDP.h"
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"

WiFiUDP UDP;
char recbuff[2];
IPAddress myIP(192, 168, 4, 1);
IPAddress HOSTIP (192, 168, 4, 2);
const char *ssid = "WiFi_BR";
const char *password = "123456789";
int Val_x, Val_y ;
int rx, ry;

MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
#define I2C_SDA  4
#define I2C_CLK  5
#define Gry_offset 0
#define Gyr_Gain 0.00763358
#define Angle_offset -1.0 //* Adjustment required
#define RMotor_offset 0 //* Adjustment required
#define LMotor_offset 0 //* Adjustment required
#define pi 3.14159
#define min(X, Y) (((X)<(Y))?(X):(Y))
float kp, ki, kd;
float r_angle, f_angle, omega;
float Turn_Speed = 0, Turn_Speed_K = 0;
float Run_Speed = 0, Run_Speed_K = 0;
float LOutput, ROutput;
unsigned long preTime = 0;
float SampleTime = 0.08;
unsigned long lastTime;
float Input, Output;
float errSum, dErr, error, lastErr;
int timeChange;

int IN1 = 14;
int IN2 = 13;
int ENA = 12;
int IN3 = 2;
int IN4 = 16;
int ENB = 15;
int CONNECT_LED = 0;

void setup() {
  Wire.begin();
  accelgyro.initialize();
  WiFi.softAP(ssid, password);
  WiFi.config(myIP, WiFi.gatewayIP(), WiFi.subnetMask());
  UDP.begin(8000); // setting UDP port
  Serial.begin(115200);

  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);
  pinMode(CONNECT_LED, OUTPUT);
  delay(100);
}

void WiFi_Receive() {
  if (UDP.parsePacket() > 0) {
    UDP.read(recbuff, 2);
    UDP.flush();
  }
  int buff;
  buff = recbuff[1];
  //Serial.print("recbuff: ");  Serial.println(buff);
  switch (recbuff[0]) {
    case 'X':
      rx = buff;
      break;
    case 'Y':
      ry = buff;
      break;
    default:
      rx = ry = 130;
  }
  //Serial.print("rx: ");  Serial.print(rx);
  //Serial.print("ry: ");  Serial.println(ry);
  if (ry >= 140) {
    Run_Speed_K = map(ry, 140, 255, 0, 100);
    Run_Speed_K = Run_Speed_K / 70; //* 70 Adjustment required
    Run_Speed = Run_Speed + Run_Speed_K;
  }
  else if (ry <= 120) {
    Run_Speed_K = map(ry, 120, 2, 0, -100);
    Run_Speed_K = Run_Speed_K / 70; //* 70 Adjustment required
    Run_Speed = Run_Speed + Run_Speed_K;
  }
  else {
    Run_Speed_K = 0;
  }
  if (rx >= 140) {
    Turn_Speed = map(rx, 130, 255, 0, 40); //* 40 Adjustment required
  }
  else if (rx <= 120) {
    Turn_Speed = map(rx, 120, 2, 0, -40); //* 40 Adjustment required
  }
  else {
    Turn_Speed = 0;
  }
}


void loop() {
  WiFi_Receive();
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  r_angle = (atan2(ay, az) * 180 / pi + Angle_offset);
  omega =  Gyr_Gain * (gx +  Gry_offset);
  if (abs(r_angle) < 45) {
    myPID();
    PWMControl();
  }
  else {
    analogWrite(ENA, 0);
    analogWrite(ENB, 0);
  }
  
  if (WiFi.status() != WL_CONNECTED) {
    digitalWrite(0, HIGH); // WiFi CONNECT_LED OFF
  } else {
    digitalWrite(0, LOW ); // WiFi CONNECT_LED ON  
  }  
}

void myPID() {
  kp = 30;   //* Adjustment required
  ki = 0.15;   //* Adjustment required
  kd = 900.0;   //* Adjustment required

  unsigned long now = millis();
  float dt = (now - preTime) / 1000.0;
  preTime = now;
  float K = 0.8;
  float A = K / (K + dt);
  f_angle = A * (f_angle + omega * dt) + (1 - A) * r_angle;
  timeChange = (now - lastTime);
  if (timeChange >= SampleTime) {
    Input = f_angle;
    error = Input;
    errSum += error * timeChange;
    dErr = (error - lastErr) / timeChange;
    Output = kp * error + ki * errSum + kd * dErr;
    LOutput = Output + Run_Speed + Turn_Speed;
    ROutput = Output + Run_Speed - Turn_Speed;
    lastErr = error;
    lastTime = now;
  }
}

void PWMControl() {
  if (LOutput > 0) {
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
  }
  else if (LOutput < 0) {
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
  }
  else {
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, HIGH);
  }
  if (ROutput > 0) {
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);
  }
  else if (ROutput < 0) {
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, HIGH);
  }
  else {
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, HIGH);
  }
  analogWrite(ENA, min(800, abs(LOutput * 4) + LMotor_offset)); //max1023
  analogWrite(ENB, min(800, abs(ROutput * 4) + RMotor_offset)); //max1023
}