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

#include "Servo.h"//サーボ用ライブラリー
#include "IRremote.h"//赤外線リモコン用ライブラリー
//ArduinoとMPU6050とのI2C通信用に以下3つのライブラリー追加する。
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"

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 86.5

/*ギアモーターによっては左右の回転数が異なる場合があります。真直ぐに前進するように
RM_offsetとLM_offsetの値を変えて回転差を補正する必要があります。*/
#define RM_offset 10
#define LM_offset 19

//IRリモコン関連の設定
#define MAX_TIME 150
long lastPressTime = 0;
int state = HIGH;
int recv_pin = 2;//D2:IR受信モジュールの入力端子を指定
IRrecv irrecv(recv_pin);
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;
unsigned long lastTime;
float Input, Output;
float errSum, dErr, error, lastErr;
float kidt;
int timeChange;

//超音波センサーの入出力端子設定
int trigger_right = 3;//D3
int echo_right = 4;//D4
int trigger_front = 7;//D7
int echo_front = 8;//D8
int trigger_left = 9;//D9
int echo_left = 10;//D10
int modeLED1 = 13;//D13:入出力端子不足でLEDが1個しか使えない。
int RMA = 16;//A2：右モータ－ドライバーA端子へ
int RMB = 17;//A3：右モータ－ドライバーB端子へ
int REN = 6;//右モータ－ドライバーイネーブル端子へ
int LMA = 14;//A0：左モータ－ドライバーA端子へ
int LMB = 15;//A1：左モータ－ドライバーB端子へ
int LEN = 5;//左モータ－ドライバーイネーブル端子へ
char contcommand;
int modecontrol = 0;//初期モード設定
int power = 0;//初期モード設定
int pos1 = 2;//ELVサーボの初期値設定
int pos2 = 90;//SPAサーボの初期値設定

void setup() { //初期設定
  Wire.begin();
  accelgyro.initialize();
  Serial.begin(115200);
  elv_servo.attach(11); //D11
  elv_servo.write(pos1);
  spa_servo.attach(12); //D12
  spa_servo.write(pos2);
  irrecv.enableIRIn();
  pinMode(RMA, OUTPUT);
  pinMode(RMB, OUTPUT);
  pinMode(LMA, OUTPUT);
  pinMode(LMB, OUTPUT);
  pinMode(REN, OUTPUT);
  pinMode(LEN, OUTPUT);
  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);
  digitalWrite(modeLED1, HIGH);
}

void loop() {
  if (irrecv.decode(&results)) { // 赤外線リモコンのボタンが放された時の処理
    INV_remote();
    irrecv.resume();
    if (state == HIGH) {
      state = LOW;
    }
    lastPressTime = millis();
    irrecv.resume(); 
  }
  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（停止）
      digitalWrite( modeLED1, HIGH);
      Stop();
      modecontrol = 0;
      power = 0;
    }
    if (results.value == 0xF1488140) { //"テレビ用赤外線リモコンの赤ボタンに設定" mode1（自律走行開始）
      digitalWrite( modeLED1, HIGH); //倒立運転インジケータLED OFF 
      modecontrol = 0;
      power = 1;
    }
    if (results.value == 0xF148414C) { //"テレビ用赤外線リモコンの緑ボタンに設定" mode2（手動運転開始）
      digitalWrite( modeLED1, HIGH); //倒立運転インジケータLED OFF 
      modecontrol = 1;
      Stop();
    }
    if (results.value == 0xF148C144) { //"テレビ用赤外線リモコンの黄ボタンに設定" mode3（倒立自動運転開始）
      digitalWrite( modeLED1, LOW ); //倒立運転インジケータLED ON
      INV_start(); //起立アームの起動
      delay(2000); //安定時間の設定
      INV_end1(); //起立補助アームの格納 
    }
  }

  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 > 20 && right > 15 && left > 15 ) { //前方の及び左右の設定距離内に障害物が無い場合。
      forward();//前進
    }
    if (front < 20 && right < left) { //前方設定距離内に障害物があり、かつ左右を比較し、右側が障害物に近い場合の処理。
      back(); //後進
      delay(600); //backの距離は、この値を変えて調整を行います。
      turnleft(); //右旋回     
      delay(60); //右旋回時間の調整
      Stop; //停止
    }
    if (front < 20 && right > left) { //今度は、右側が障害物に近い場合の処理。
      back(); //後進
      delay(600); //backの距離は、この値を変えて調整を行います。
      turnright(); //右旋回 
      delay(60); //旋回時間の調整
      Stop; //停止
    }
    if (right < 10) { //右側が設定値に達した場合の処理。
      turnleft(); //左旋回
      delay(60); //旋回時間の調整
      Stop; //停止
    }
    if (left < 10) { //左側が設定値に達した場合の処理。
      turnright(); //右旋回
      delay(60); //右旋回旋回時間の調整
      Stop; //停止
    }
    else {
      Stop; //停止
    }
  }

  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();
        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();
      MotorControl();
    }
    else { //前後の倒立角度が45度以上に傾いた場合は左右モーターの出力を0にする。
      analogWrite(REN, 0);
      analogWrite(LEN, 0);
    }
    if (pos1 >= 30) { //倒立成立時の処理：ELVサーボの設定値が30以上の場合の処理。
      delay (100);
      spa_servo.write(pos2 = 90); //設定値は機体に応じて設定が必要
      elv_servo.write(pos1 = 2); //設定値は機体に応じて設定が必要
      delay (50);
      kidt = 0.1; //ki値を戻す
    }
  }
}

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

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

void INV_end2() { //アーム格納操作2
  spa_servo.write(pos2 = 113); //設定値は機体に応じてカット＆トライの設定変更が必要
  elv_servo.write(pos1 = 110); //設定値は機体に応じてカット＆トライの設変更定が必要
  delay (100);
  for (pos1 = 110; pos1 > 2; pos1 -= 1) { //設定値は機体に応じてカット＆トライの設定が必要
    elv_servo.write(pos1);
    delay (20);
  }
  delay (100);
  spa_servo.write(pos2 = 90);
  digitalWrite( modeLED1, LOW );
  modecontrol = 0;
  Stop();
  power = 0;
}

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

  unsigned long now = millis();
  float dt = (now - preTime) / 500.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 INV_remote() { //mode3 倒立運転時のリモコン操作
  switch (results.value) {
    case 0XF148EA81: // forward：前進
      Serial.print("FORWARD"); Serial.println(Run_Speed);
      Run_Speed_K = -15;
      Run_Speed = Run_Speed + Run_Speed_K; //十字キーの“↑キー”を押してる間、-15を加算し続けます。
      break;
    case 0XF1480481: // back：後進
      Serial.print("BACK"); Serial.println(Run_Speed);
      Run_Speed_K = 15;
      Run_Speed = Run_Speed + Run_Speed_K; //十字キーの“↓キー”を押してる間、15を加算し続けます。
      break;
    case 0XF148EB80: // left：左旋回
      Turn_Speed = 20;
      break;
    case 0XF1481B8F: // right：右旋回
      Turn_Speed = -20;
      break;
    default:
      Run_Speed_K = 0;
      Turn_Speed = 0;
  }
}

void MotorControl() { //倒立運転時のモーター制御
  if (LOutput > 0) {
    digitalWrite(RMA, LOW);
    digitalWrite(RMB, HIGH);
  }
  else if (LOutput < 0) {
    digitalWrite(RMA, HIGH);
    digitalWrite(RMB, LOW);
  }
  else {
    digitalWrite(RMA, LOW);
    digitalWrite(RMB, LOW);
  }
  if (ROutput > 0) {
    digitalWrite(LMA, LOW);
    digitalWrite(LMB, HIGH);
  }
  else if (ROutput < 0) {
    digitalWrite(LMA, HIGH);
    digitalWrite(LMB, LOW);
  }
  else {
    digitalWrite(LMA, LOW);
    digitalWrite(LMB, LOW);
  }
  analogWrite(REN, min(255, abs(LOutput)));
  analogWrite(LEN, min(255, abs(ROutput)));
}

//超音波運転及び手動運転時のサーボモータ制御
void forward() { //前進
  digitalWrite(RMA, LOW);
  digitalWrite(RMB, HIGH);
  digitalWrite(LMA, LOW);
  digitalWrite(LMB, HIGH);
  analogWrite(REN, 60 + RM_offset);
  analogWrite(LEN, 60 + LM_offset);
  delay(500);
}

void Stop() { //停止
  digitalWrite(RMA, HIGH);
  digitalWrite(RMB, HIGH);
  digitalWrite(LMA, HIGH);
  digitalWrite(LMB, HIGH);
  analogWrite(REN, 255);
  analogWrite(LEN, 255);
}

void turnright() { //右旋回（右超信地旋回）
  digitalWrite(RMA, HIGH);
  digitalWrite(RMB, LOW);
  digitalWrite(LMA, LOW);
  digitalWrite(LMB, HIGH);
  analogWrite(REN, 60 + RM_offset);
  analogWrite(LEN, 60 + LM_offset);
}

void turnleft() { //左旋回（左超信地旋回）
  digitalWrite(RMA, LOW);
  digitalWrite(RMB, HIGH);
  digitalWrite(LMA, HIGH);
  digitalWrite(LMB, LOW);
  analogWrite(REN, 60 + RM_offset);
  analogWrite(LEN, 60 + LM_offset);
}

void s_right() { //右ショート旋回
  digitalWrite(RMA, LOW);
  digitalWrite(RMB, HIGH);
  digitalWrite(LMA, LOW);
  digitalWrite(LMB, HIGH);
  analogWrite(REN, 40 + RM_offset);
  analogWrite(LEN, 65 + LM_offset);
}

void s_left() { //左ショート旋回
  digitalWrite(RMA, LOW);
  digitalWrite(RMB, HIGH);
  digitalWrite(LMA, LOW);
  digitalWrite(LMB, HIGH);
  analogWrite(REN, 65 + LM_offset);
  analogWrite(LEN, 45 + RM_offset);
}

void back() { //後進
  digitalWrite(RMA, HIGH);
  digitalWrite(RMB, LOW);
  digitalWrite(LMA, HIGH);
  digitalWrite(LMB, LOW);
/*ギアモーターによっては正転時と反転時に回転数が異なる場合があります。真直ぐに後進するように
REN&LENのPWM値を変えて回転差を補正する必要があります。*/
  analogWrite(REN, 64.5 + RM_offset);
  analogWrite(LEN, 60 + LM_offset);
}