/*このスケッチを丸ごとコピーするだけでは再現できません。
特に倒立は、フレームの形状や使用部品及び部品配置によってバランスが異なり、
安定させるには機体に合わせた設定値の変更が必要です。
------------------------------------------------------------------------------------*/
#include "Servo.h" //サーボ用ライブラリー
#include "IRremote.h" //赤外線リモコン用ライブラリー
//ArduinoとMPU6050とのI2C通信用に以下3つのライブラリー追加する。
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"

Servo leftservo; //左ドライブ用サーボの宣言
Servo rightservo; //右ドライブ用サーボの宣言
Servo elv_servo; //エレベーターアーム用サーボの宣言
Servo spa_servo; //サポートアーム用サーボの宣言
//MPU6050の設定値
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
#define Gry_offset 0
#define Gyr_Gain 0.00763358
#define pi 3.14159
/* 重要“Angle_offset”は倒立時の前後バランスの設定（この設定が狂ってると上手く倒立出来ません）
センサーの取り付けは、振動などで動かないように固定します。*/
#define Angle_offset -2.5

//IRリモコン関連の設定
#define MAX_TIME 150 // max ms between codes
long lastPressTime = 0;
int state = HIGH;
int receiver = 4; //D2:IR受信モジュールの入力端子を指定
IRrecv irrecv(receiver);
decode_results results;

//PID制御関連
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;//0.08
unsigned long lastTime;
float Input, Output;
float errSum, dErr, error, lastErr;
int timeChange;
float kidt;

//超音波センサーの入出力端子設定
int trigger_front = 12;
int echo_front = 13;
int trigger_left = 10;
int echo_left = 11;
int trigger_right = 6;
int echo_right = 7;
//mode表示LEDの設定
int modeLED1 = 14;
int modeLED2 = 15;
int modeLED3 = 16;
int powerLED = 17;

char contcommand;
int modecontrol = 0; //初期モード設定
int power = 0; //初期モード設定
int pos1 = 175; //ELVサーボの初期値設定
int pos2 = 160; //SPAサーボの初期値設定

void setup() { //初期設定
  Wire.begin();
  accelgyro.initialize();
  Serial.begin(115200);
  irrecv.enableIRIn();
  elv_servo.attach(3);
  elv_servo.write(pos1);
  spa_servo.attach(5);
  spa_servo.write(pos2);
  leftservo.attach(8);
  leftservo.write(90);
  rightservo.attach(9);
  rightservo.write(90);
  pinMode(trigger_front, OUTPUT);
  pinMode(echo_front, INPUT);
  pinMode(trigger_left, OUTPUT);
  pinMode(echo_left, INPUT);
  pinMode(trigger_right, OUTPUT);
  pinMode(echo_right, INPUT);
  pinMode(modeLED1, OUTPUT);
  pinMode(modeLED2, OUTPUT);
  pinMode(modeLED3, OUTPUT);
  pinMode(powerLED, OUTPUT);
  digitalWrite(modeLED1, LOW);
  digitalWrite(modeLED2, HIGH);
  digitalWrite(modeLED3, HIGH);
  digitalWrite(powerLED, HIGH);
}

void loop() {
  if (irrecv.decode(&results)) { // 赤外線リモコンのボタンが放された時の処理
    INVremote();
    irrecv.resume(); // Receive the next value
    if (state == HIGH) {
      state = LOW;  // Button pressed, so set state to LOW
    }
    lastPressTime = millis();
    irrecv.resume(); // Receive the next value
  }
  if (state == LOW && millis() - lastPressTime > MAX_TIME) {
    state = HIGH;
    Run_Speed_K = 0;//リモコンボタンが放された時0に戻す
    Turn_Speed = 0;//リモコンボタンが放された時0に戻す

/*IRリモコンの送信機について
使用するIRリモコンはボタンが押されてる間、同じコードが連続出力するタイプで有ること。
その理由は、倒立運転時に前進/後進ボタンが押されると出力されたコード数を加減算して
前後進の速度制御を行うからです。
使用するIRリモコンにより、出力コードが異なるので先にコード確認用ソフトで使用ボタンの
コードを調べて書き換えます。以下のコードはシャープのテレビ用リモコンの例です。*/

    if (results.value == 0xF1480148) { //"テレビ用赤外線リモコンの青ボタン" mode0（停止）
      Stop();
      modecontrol = 0;
      power = 0;
      digitalWrite (powerLED, HIGH);
      digitalWrite (modeLED1, LOW);
      digitalWrite (modeLED2, HIGH);
      digitalWrite (modeLED3, HIGH);
    }
    if (results.value == 0xF1488140) { //"テレビ用赤外線リモコンの赤ボタンに設定" mode1（自律走行開始）
      modecontrol = 0;
      power = 1;
      digitalWrite (powerLED, LOW);
      digitalWrite (modeLED1, LOW);
      digitalWrite (modeLED2, HIGH);
      digitalWrite (modeLED3, HIGH);
    }
    if (results.value == 0xF148414C) { //"テレビ用赤外線リモコンの緑ボタンに設定" mode2（手動運転開始）
      modecontrol = 1;
      //digitalWrite (powerLED, HIGH);
      digitalWrite (modeLED1, HIGH);
      digitalWrite (modeLED2, LOW);
      digitalWrite (modeLED3, HIGH);
      Stop();
    }
    if (results.value == 0xF148C144) { //"テレビ用赤外線リモコンの黄ボタンに設定" mode3（倒立自動運転開始）
      INV_start(); //起立アームの起動
      delay(2000); //安定時間の設定
      INV_end1(); //起立補助アームの格納 
      digitalWrite (modeLED1, HIGH);
      digitalWrite (modeLED2, HIGH);
      digitalWrite (modeLED3, LOW);
    }
  }

  if ( power == 1) { // 超音波のセンサー受信距離による自律運転
// mode1　各超音波センサーの制御
    long duration_front, duration_left, duration_right, right, left, front;
    digitalWrite(trigger_front, LOW);
    delayMicroseconds(2);
    digitalWrite(trigger_front, HIGH);
    delayMicroseconds(5);
    digitalWrite(trigger_front, LOW);
    duration_front = pulseIn(echo_front, HIGH);
    front = duration_front / 29 / 2;
    Serial.print("front"); Serial.println(front);

    digitalWrite(trigger_left, LOW);
    delayMicroseconds(2);
    digitalWrite(trigger_left, HIGH);
    delayMicroseconds(5);
    digitalWrite(trigger_left, LOW);
    duration_left = pulseIn(echo_left, HIGH);
    left = duration_left / 29 / 2;
    Serial.print("left"); Serial.println(left);

    digitalWrite(trigger_right, LOW);
    delayMicroseconds(2);
    digitalWrite(trigger_right, HIGH);
    delayMicroseconds(5);
    digitalWrite(trigger_right, LOW);
    duration_right = pulseIn(echo_right, HIGH);
    right = duration_right / 29 / 2;
    Serial.print("right"); Serial.println(right);

//以下の障害物回避制御は簡単な方法ですが、工夫によってはもっと複雑に制御が可能です。

    if (front > 15 && right > 10 && left > 10 ) { //前方の及び左右の設定距離内に障害物が無い場合。
      forward(); //後進
    }
    if (front < 15 && right < left) { //前方の及び左右の設定距離内に障害物が無い場合。
      back();
      turnleft(); //左旋回
    }
    if (front < 15 && right > left) { //今度は、右側が障害物に近い場合の処理。
      back();
      turnright(); //右旋回 
    }
    if (right < 10) { //左ショート旋回
      s_left();
    }
    if (left < 10) { //右ショート旋回 
      s_right();
    }
  }

  if (modecontrol == 1) { //　mode2 手動運転時の赤外線リモコン操作
    switch (results.value) {
      case 0xF148EA81: //テレビ用赤外線リモコンの十字キーの“↑キー”に設定
        forward();
        break;
      case 0xF148EB80: //テレビ用赤外線リモコンの十字キーの“←キー”に設定
        s_left();
        break;
      case 0xF1484A8B: //テレビ用赤外線リモコンの十字キー中心の“決定ボタン”に設定
        Stop();
        break;
      case 0xF1481B8F: //テレビ用赤外線リモコンの十字キーの“→キー”に設定
        s_right();
        break;
      case 0xF1480481: //テレビ用赤外線リモコンの十字キーの“↓キー”に設定↓
        back();
        break;
      case 0xF148AF80: //テレビ用赤外線リモコンの“終了ボタン”に設定
        modecontrol = 0;
        Stop();
        digitalWrite (modeLED1, HIGH);
        digitalWrite (modeLED2, LOW);
        digitalWrite (modeLED3, HIGH);
        break;
      default:
        ;
    }
    delay(500);
  }

  if (modecontrol == 2 ) { //　mode3 倒立制御
    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) { //前後の倒立角度が45度以内の場合の処理。
      myPID();
      ServoControl();
    }
    else { //前後の倒立角度が45度以上に傾いた場合はサーボ出力を中立(90)にする。
      leftservo.write(90);
      rightservo.write(90);
    }
    if (pos1 <= 50) { //倒立成立時の処理：ELVサーボの設定値が50以下の場合の処理。
      delay (50);
      spa_servo.write(pos2 = 160); //設定値は機体に応じて設定が必要
      elv_servo.write(pos1 = 175); //設定値は機体に応じて設定が必要
      delay (130);
      kidt = 0.12;
    }
  }
}

void INV_start() { //起立アーム操作
  spa_servo.write(pos2 = 145); //設定値は機体に応じてカット＆トライの設定が必要
  for (pos1 = 175; pos1 > 43.5; pos1 -= 1) { //設定値は機体に応じてカット＆トライの設定が必要
    elv_servo.write(pos1);
    delay (10);
  }
}

void INV_end1() { //アーム格納操作1
  spa_servo.write(pos2 = 146);//144.9 //設定値は機体に応じてカット＆トライの設定が必要
  elv_servo.write(pos1 = 42.5);//44.2 //設定値は機体に応じてカット＆トライの設定が必要
  kidt  = 0.02; //倒立起動時のみki値を下げます。
  modecontrol = 2;
}

void INV_end2() { //アーム格納操作2
  spa_servo.write(pos2 = 143.0); //設定値は機体に応じてカット＆トライの設定変更が必要
  elv_servo.write(pos1 = 46.5); //設定値は機体に応じてカット＆トライの設定変更が必要
  for (pos1 = 45.0; pos1 < 175; pos1 += 1) { //設定値は機体に応じてカット＆トライの設定が必要
    elv_servo.write(pos1);
    delay (15);
  }
  delay (100);
  spa_servo.write(pos2 = 160);
  leftservo.write(90);
  rightservo.write(90);
  digitalWrite (modeLED2, LOW);
  digitalWrite (modeLED3, HIGH);
  modecontrol = 0;
}

void INVremote() { //mode3 倒立運転時のリモコン操作
  switch (results.value) {
    case 0XF148EA81: // forward：前進
      Serial.print("FORWARD"); Serial.println(Run_Speed);
      Run_Speed_K = -10; 
      Run_Speed = Run_Speed + Run_Speed_K;//十字キーの“↑キー”を押してる間、-10を加算し続けます。
      break;
    case 0XF1480481: // back：後進
      Serial.print("BACK"); Serial.println(Run_Speed);
      Run_Speed_K = 10;
      Run_Speed = Run_Speed + Run_Speed_K;//十字キーの“↓キー”を押してる間、10を加算し続けます。
      break;
    case 0XF148EB80: // left：左旋回
      Serial.print("LEFT"); Serial.println(Turn_Speed);
      Turn_Speed = -20;
      break;
    case 0XF1481B8F: // right：右旋回
      Serial.print("RIGHT"); Serial.println(Turn_Speed);
      Turn_Speed = 20;
      break;
    default:
      Run_Speed_K = 0;
      Turn_Speed = 0;
  }
}

void myPID() { //倒立PID制御
  kp = 14; //要調整 設定値は機体に応じてカット＆トライの設定変更が必要
  ki = kidt;
  kd = 200; //要調整 設定値は機体に応じてカット＆トライの設定変更が必要

  unsigned long now = millis();
  float dt = (now - preTime) / 1000.0;
  preTime = now;
  float K = 0.8;//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 ServoControl() { // mode3　倒立運転時のサーボモーター制御
  LOutput = constrain(LOutput, -110, 100);
  ROutput = constrain(ROutput, -110, 100);
  if (LOutput > 0) { // 左サーボモーターの制御
    leftservo.write(LOutput = map(LOutput, 0, -100, 90, 110));//(LOutput, 0, 255, 90, 180)
  }
  else if (LOutput < 0) {
    leftservo.write(LOutput = map(LOutput, 0, 100, 90, 70));//(LOutput, 0, -255, 90, 0)
  }
  else {
    leftservo.write(90);
  }
  if (ROutput > 0) { // 右サーボモーターの制御
    rightservo.write(ROutput = map(ROutput, 0, -100, 90, 65));//(ROutput, 0, 255, 90, 0)
  }
  else if (ROutput < 0) {
    rightservo.write(ROutput = map(ROutput, 0, 100, 90, 115));//(ROutput, 0, -255, 90, 180)
  }
  else {
    rightservo.write(90);
  }
}

//超音波運転及び手動運転時のサーボモータ制御
void forward() { //前進
  leftservo.write(79.5);//80
  rightservo.write(100);//100
}

void Stop() { //停止
  leftservo.write(90);
  rightservo.write(90);
}

void turnright() { //右旋回（右超信地旋回）
  leftservo.write(80);//0
  rightservo.write(80);//0
  delay(300);
}

void turnleft() { //左旋回（左超信地旋回）
  leftservo.write(100);//180
  rightservo.write(100);//180
  delay(300);
}

void s_right() { //右ショート旋回
  leftservo.write(75 );//75
  rightservo.write(100);//100
  delay(100);
}

void s_left() { //左ショート旋回
  leftservo.write(80);//80
  rightservo.write(105);//105
  delay(100);
}

void back() { //後進
  leftservo.write(97);//100
  rightservo.write(80);//80
  delay(600);
}