// SainSmart Instabots Upright Rover rev. 2.0
// https://www.sainsmart.com/products/instabots-self-balancing-robot-v2
#include "M5Atom.h"
//M5Stamp_Pico用が未だリリースされてないので
//同じPico系のM5Atomライブラリーを使用しています。
#define BLYNK_PRINT Serial
#define BLYNK_USE_DIRECT_CONNECT
#include "BlynkSimpleEsp32_BLE.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"
char auth[] = "***************************";

MPU6050 accelgyro;
MPU6050 initialize;
int16_t ax, ay, az;
int16_t gx, gy, gz;
#define Gry_offset 0  //The offset of the gyro
#define Gyr_Gain 16.348
#define Angle_offset 0  // The offset of the accelerator
#define pi 3.14159
#define min(X, Y) (((X)<(Y))?(X):(Y))

float kp, ki, kd;
float Angle_Raw, Angle_Filtered, omega;
float Turn_Speed = 0;
float Run_Speed = 0, Run_Speed_K = 0;
float LOutput, ROutput;
unsigned long preTime = 0;
unsigned long lastTime;
float Input, Output;
float errSum, dErr, error, lastErr;
int timeChange;

uint8_t DisBuff[2 + 5 * 5 * 3]; //RGB collar LED
int xVal;
int yVal;

void setBuff(uint8_t Rdata, uint8_t Gdata, uint8_t Bdata) {
  DisBuff[0] = 0x05;
  DisBuff[1] = 0x05;
  for (int i = 0; i < 25; i++) {
    DisBuff[2 + i * 3 + 0] = Rdata;
    DisBuff[2 + i * 3 + 1] = Gdata;
    DisBuff[2 + i * 3 + 2] = Bdata;
  }
}

void setup() {
  Wire.begin(21, 22);
  Serial.begin(115200);
  M5.begin(true, false, true);
  delay(10);
  setBuff(0xff, 0x00, 0x00);
  M5.dis.displaybuff(DisBuff);
  Blynk.setDeviceName("M5Stamp_BLE");
  Blynk.begin(auth);
  accelgyro.initialize();
  initialize.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);;
  for (int i = 0; i < 200; i++) {// Looping 200 times to get the real gesture when starting
    Filter();
  }
  if (abs(Angle_Filtered) < 45) { // Start the robot after cleaning data
    omega = Angle_Raw = Angle_Filtered = 0;
    Filter();
    Output = error = errSum = dErr = Run_Speed = Turn_Speed = 0;
    PID();
  }
  ledcSetup(0, 500, 8);
  ledcSetup(1, 500, 8);
  ledcSetup(2, 500, 8);
  ledcSetup(3, 500, 8);
  ledcAttachPin(19, 0);
  ledcAttachPin(23, 1);
  ledcAttachPin(25, 2);
  ledcAttachPin(33, 3);
  ledcWrite(0, 0);
  ledcWrite(1, 0);
  ledcWrite(2, 0);
  ledcWrite(3, 0);
  yVal = 30;
  xVal = 25;
  delay(100);
}

BLYNK_WRITE(V0) {
  int button1 = param.asInt();
  if (button1 == 1) {
    yVal = 60;
    xVal = 40;
  } else {
    yVal = 30;
    xVal = 25;
  }
}

BLYNK_WRITE(V1) {
  long Joy_x = param[0].asInt();
  long Joy_y = param[1].asInt();

  if (Joy_y >= 100) {
    Run_Speed_K = map(Joy_y, 100, 255, 0, yVal);
    Run_Speed += Run_Speed_K;
    min(80, Run_Speed);
    setBuff(0x40, 0x40, 0x40);
  }
  else if (Joy_y <= -100) {
    Run_Speed_K = map(Joy_y, -100, -255, 0, -yVal);
    Run_Speed += Run_Speed_K;
    min(80, Run_Speed);
    setBuff(0x40, 0x00, 0x00);
  }
  else {
    Run_Speed_K = 0;
  }

  if (Joy_x >= 100) {
    Turn_Speed = map(Joy_x, 100, 255, 0, xVal);
    setBuff(0x40, 0x00, 0x40);
  }
  else if (Joy_x <= -100) {
    Turn_Speed = map(Joy_x, -100, -255, 0, -xVal);
    setBuff(0x00, 0x00, 0x40);
  }
  else {
    Turn_Speed = 0;
  }

  if ((Joy_x == 0) && (Joy_y == 0)) {
    setBuff(0x00, 0x40, 0x00);
  }
  M5.dis.displaybuff(DisBuff);
}

void loop() {
  Blynk.run();
  Filter();
  if (abs(Angle_Filtered) < 45) {
    PID();
    MotorControl();
  }
  else  {
    ledcWrite(0, 0);
    ledcWrite(1, 0);
    ledcWrite(2, 0);
    ledcWrite(3, 0);

    for (int i = 0; i < 100; i++) { // Keep reading the gesture after falling down
      Filter();
    }
    if (abs(Angle_Filtered) < 45) { // Empty datas and restart the robot automaticly
      for (int i = 0; i <= 500; i++) { // Reset the robot and delay 2 seconds
        omega = Angle_Raw = Angle_Filtered = 0;
        Filter();
        Output = error = errSum = dErr = Run_Speed = Turn_Speed = 0;
        PID();
      }
    }
  }
}

void Filter() {
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  Angle_Raw = (atan2(ay, az) * 180 / pi + Angle_offset);
  omega =  gx / Gyr_Gain + Gry_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 PID() {
  kp = 15;
  ki = 0.10;
  kd = 250;
  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 MotorControl() {
  if (LOutput > 0) {
    ledcWrite(0, min(255, abs(LOutput)));
    ledcWrite(1, 0);
  }
  else if (LOutput < 0) {
    ledcWrite(0, 0);
    ledcWrite(1, min(255, abs(LOutput)));
  }
  else {
    ledcWrite(0, 0);
    ledcWrite(1, 0);
  }
  if (ROutput > 0) {
    ledcWrite(2, min(255, abs(ROutput) + 3));
    ledcWrite(3, 0);
  }
  else if (ROutput < 0) {
    ledcWrite(2, 0);
    ledcWrite(3, min(255, abs(ROutput) + 3));
  }
  else {
    ledcWrite(2, 0);
    ledcWrite(3, 0);
  }
  Serial.print("  LOutput: ");
  Serial.print(LOutput);
  Serial.print(",  ROutput: ");
  Serial.println(ROutput);
}