/*************************************************************************
   2016.1.27 mtakapii  L3GD20_Inv_Pendulum.  Please call me "Yaoki Voice".
   2017.12.5           Added Voice Synthesizer use I2C
   L3GD20_Inv_Pendulum_digital_daruma_Yaoki_RemoconV8.ino   444g
   Arduino IDE 1.8.0
   Gear ratio of Radial Tire ---> 114.7:1 

   analogpin 2 = > OFF ... Continuance mode
               = > ON  ... Demo mode
   ref.
   http://blog-yama.a-quest.com/?eid=970150
   http://www.instructables.com/id/Another-Easier-Inverted-Pendulum-in-Japanese/

   mtakapii のコメントのある部分はこの著者が付け加えました。
   ・setup 時に電源電圧を読み、電圧と電気残量をしゃべります
   ・setup 時に本体附近の温度を測定し、著者個人の体感をしゃべります
   ・起き上がり（Rise up）関係のスケッチを追加しました
   ・
*/

byte countS   = 0; //03
//long zeroOmegaI = 0; //04 (This line is omitted in this version.)
int recOmegaI[10];  //05
int omegaI    = 0;     //06
long thetaI   = 0;     //07
long sumPower = 0;     //08
long sumSumP  = 0;     //09

// 硬床でバランスすれば布団でもok　  mtakapii
const int kAngle  = 170;   //10 躯体の傾き（角度）50(default) 129 131 137
// 170 150 180 175 170 165 150 200 170 180 190 200 190 150 190 140 120 145
// 180 170 160 150 160 170 160 180 170 150 175 170
const int kOmega  = 600;  //11 躯体の傾きの変化率（角速度）500 480 600 690
// 800 690 650 600 550 500 700 800 750 700 600 650 610 600 650 700 650 640
// 670 690 670 660 600
const long kSpeed = 70 ; //12 車輪軸の移動速度 60 70 65 60 55 60 55 50
// 60 50 60 65 61 62 65 70 60 80 90 80 70
const long kDistance = 1; //13 車輪軸の移動距離 20 布団1〜40、40 10 5 3
// 13 1 40  20 10 5 1 20 50 70 20 10 0.5 100 20 1

long powerScale;              //14
//int  power;                 //15
float  power;                 //15

#define offset_norm 40        // mtakapii
long offset = offset_norm;
// 43 42 39 40 50 60 55 51 40 35 30 32 38 45 47 37 30 25 20 30 40 38
// 32 37 39 38 37 35 33 30 27 25 23 21 19 17 18 19 22 23 25 27 30 35
// 38 46 55 60 40 50 40
#define samp_num 32

long distance = 20;
float turn = 0;
float turn_deg = 3.;
long diff_6 = 0;
char in ;

long vE5 = 0;   //16
long xE5 = 0;   //17
// Copyright (C) 2014 ArduinoDeXXX All Rights Reserved.

#include <SPI.h>
#include <Wire.h>  // I2C
#include <AquesTalk.h>
AquesTalk atp;  //

//#define I2C_ADDR_AQUESTALK 0x2E

//DL1 (These 17 lines, DL1-DL17, are added in this version.)
int ry;     //DL2
long R;     //DL3

void L3GD20_write(byte reg, byte val) { //DL4
  digitalWrite(10, LOW);           //DL5
  SPI.transfer(reg);               //DL6
  SPI.transfer(val);               //DL7
  digitalWrite(10, HIGH);          //DL8
} //DL9

byte L3GD20_read(byte reg) {      //DL10
  byte ret = 0;                   //DL11
  digitalWrite(10, LOW);          //DL12
  SPI.transfer(reg | 0x80);       //DL13
  ret = SPI.transfer(0);          //DL14
  digitalWrite(10, HIGH);         //DL15
  return ret;                     //DL16
} //DL17

//  LED　　mtakapii
void FlashLED(char i) {
  while (i--) {
    digitalWrite(LED_BUILTIN, 1); delay(170);
    digitalWrite(LED_BUILTIN, 0); delay(170);
  }
}

/********* Rise up variables  mtakapii *************/
float   stp =  1;     // step
int     dec100, dec10, dec1;

float   angle;
float   pre_angle;
int     int_angle;

float   length_time = 0.9 / 60;     //Servo speed..[s]/[deg.]
int     int_delay_time;
char    Servo_no;

int   hensoku1 = 145;                // Change Gear point 1
int   hensoku2 = 155;                // Change Gear point 2

long log_min =  10;               // logical minimum
long log_max = 168;               // logical maximum
float Number_1_begin = log_max;
float Number_1_end   =       0;
float Number_2_begin =       0;
float Number_2_end   = log_max;

boolean action_sw   = false;
boolean continue_sw = false;
String str;
#define down_count 360
/*****************************************************/

void setup () {             //18
  float volt;

  Wire.begin();             //
  Serial.begin(38400);      //19
  //Serial.begin(115200);      //19
  //Serial.println("Test Serial println");
  //pre_angle = 10;
  atp.SetSpeed(170);
  term();
  delay(1000);

  volt = talk_volt();
  talk_temerature(volt);
  delay(500);

  atp.SetSpeed(180);
  while (atp.IsBusy()) ;
  atp.Synthe("#J\r");
  while (atp.IsBusy()) ;
  atp.Synthe("#K\r");

  //while (atp.IsBusy()) ;
  //atp.Synthe("watashiwa nanakorobi yaoki desu--  ");
  while (atp.IsBusy()) ;
  atp.Synthe("korekara okiagatte barannsutourituo shimasu  ");

  if ( analogRead(2) > 100) {
    continue_sw = true;          // ... A2 switch OFF > 100
    //wipe(hensoku2, log_max, 50);   // to Stand Mode
    while (atp.IsBusy()) ;
    atp.Synthe("nao imawa renzoku touritu simasu");
  }
  while (atp.IsBusy()) ;
  delay(1000);

  pinMode(4, OUTPUT); //20      --- IN1
  pinMode(5, OUTPUT); //20-a    --- IN2
  pinMode(6, OUTPUT); //21      --- Enable A
  pinMode(7, OUTPUT); //            N.C  ==> 正しくはIN3 mtakapii
  pinMode(8, OUTPUT); //        --- IN4
  pinMode(9, OUTPUT); //        --- Enable B

  for ( int i = 0 ; i < 10 ; i++ ) {
    recOmegaI[i] = 0;
  } //25 ("int" is added instead of line 2 omitted.)
  pinMode(10, OUTPUT);
  //DL18 (These 8 lines, DL18-DL25, are added in this version.)
  digitalWrite(10, HIGH); //DL19
  SPI.begin(); //DL20
  SPI.setBitOrder(MSBFIRST); //DL21
  SPI.setDataMode(SPI_MODE3); //DL22
  SPI.setClockDivider(SPI_CLOCK_DIV2); //DL23
  L3GD20_write(0x20, B11001111); //DL24
  L3GD20_write(0x23, B00000000); //DL25
  delay(300); //26
  // training(); // (This line is omitted in this version.)
  // MsTimer2::set(5, chkAndCtl);
  // (This line is omitted in ver.2.0 and the later.)
  // MsTimer2::start();
  // (This line is omitted in ver.2.0 and the later.)
} //30

float map_f(float x, float in_min, float in_max, float out_min, float out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}


#define servo 0.9
float servo_trim = servo;

void loop () {    //31
  int count = 0;

  action_sw = false; delay(500);
  while (1) {
    count = 0; delay(2000);
    Rise_up();
    while ( count < 1000 ) {
      chkAndCtl();  // NL1 (This line is added in ver.2.0 and the later.)
      if ( action_sw ) {
        wave_hand2();
        delay(700);
        action_sw = false;
        break;
      }
      if ( power > 0 ) {    //32
        // Right side Motor
        analogWrite ( 6, (power + turn) );
        digitalWrite( 4, HIGH );
        digitalWrite( 5, LOW );     //35
        // Left side Motor
        analogWrite ( 9, (power + servo_trim) );
        digitalWrite( 7, HIGH );
        digitalWrite( 8, LOW );
      }
      else {
        // Right side Motor
        analogWrite ( 6, - (power + turn) ); //40
        digitalWrite( 4, LOW );
        digitalWrite( 5, HIGH );
        // Left side Motor
        analogWrite ( 9, - (power + servo_trim));
        digitalWrite( 7, LOW );
        digitalWrite( 8, HIGH ); //45
      }
      // delayMicroseconds(3600); // NL2 (This is omitted in this version.)
      /* ***********************************************/
      if ( thetaI == 0 ) {
        if ( !continue_sw ) {
          count++;
          switch (count) {
            case 250: atp.Synthe("arayotto");   break;
            //case 250: atp.Synthe("u--n");     break;
            case 300: wave_hand1();             break;
            case 350: atp.Synthe("otto tto");   break;
            //case 450: atp.Synthe("u--n madamada");  break;
            case 450: atp.Synthe("daibu tukareta"); break;
            case 550: wave_hand2();             break;
            case 600: atp.Synthe("meccha meccha tukareta--"); break;
            case 700: wave_hand3();             break;
            case 800: atp.Synthe("korobuyo-- korobuyo--"); break;
            case 850: wave_hand4();             break;
          }
        }
      }
    }
    omegaI = 0;
    thetaI = 0;
    // **** power off ****
    // Right side Motor
    analogWrite ( 6, 0 ); //40
    digitalWrite( 4, LOW );
    digitalWrite( 5, HIGH );
    // Left side Motor
    analogWrite ( 9, 0);
    digitalWrite( 7, LOW );
    digitalWrite( 8, HIGH ); //45
    // *****************
    delay(500);
    wave_hand2();
    delay(500);
    start_position();
    delay(1200);
    while (atp.IsBusy());
    atp.Synthe("a-----------  koronnjimatta ");
    while (atp.IsBusy());
    atp.Synthe("sikujitta----   ");
    while (atp.IsBusy());
    atp.Synthe("yudan teiteki   ");

    while (atp.IsBusy());
    atp.Synthe(" mouichido ganbarima-----su  ");
  }
}

//void training(){
//48 (These 7 lines, 48-54, are omitted in this version.)
// delay (1000);
// for ( i = 0 ; i < 500 ; i++ ){ //50
// zeroOmegaI = zeroOmegaI + analogRead(A5);
// }
// zeroOmegaI = zeroOmegaI / i;
//} //54

void chkAndCtl() { //55
  //***********   mtakapii  *************
  if (Serial.available() > 0) {
    in = Serial.read();
    //atp.SetSpeed(200);
    switch (in) {
      case '1': offset -= distance;
        atp.Synthe("mae"); break;
      case '0': offset = offset_norm; turn = 0;
        atp.Synthe("teisi"); break;
      case '2': offset += distance;
        atp.Synthe("ushiro"); break;
      case '3': turn += turn_deg;
        atp.Synthe("hidari"); break;
      case '4': turn -= turn_deg;
        atp.Synthe("migi"); break;
      case '5': action_sw = true;
        atp.Synthe("tento-- tento---- "); break;
    }
    //atp.SetSpeed(180);
  }
  //***************************************

  // omegaI = 0;
  // NL3 (These 6 lines, NL3-NL8, are omitted in this version.)
  // for ( i = 0 ; i < 10 ; i++ ) { //NL4
  // omegaI = omegaI + analogRead(A5) - zeroOmegaI; //NL5
  // delayMicroseconds(10); //NL6
  // } //NL7
  // omegaI = omegaI / 10; //NL8

  R = 0; //DL26 (These 7 lines, DL26-DL32, are added in this version.)
  for ( int i = 0 ; i < 45 ; i++ ) {
    //DL27 ("int" is added instead of line 2 omitted.)
    ry = ( (L3GD20_read(0x2B) << 8) | L3GD20_read(0x2A) ); //DL28
    //R = R + ry; //DL29
    R = R - ry; //DL29    2017.7.21 mtakapii
    delayMicroseconds(90); //DL30
  } //DL31
  omegaI = R * 0.00875 / 45; //DL32
  // omegaI = analogRead(A5) - zeroOmegaI;
  // 56 (This line is omitted in ver.2.0 and the later.)

  if ( abs( omegaI ) < 2 ) {
    omegaI = 0;
  }             //57 (The lower bound is less than 2 in this version.)
  recOmegaI[0] = omegaI;
  thetaI = thetaI + omegaI;
  countS = 0; //60

  for ( int i = 0 ; i < 10 ; i++ ) {
    // ("int" is added instead of line  2 omitted.)
    if ( abs( recOmegaI[i] ) < 4 ) {
      countS++;
    }         //62 (The lower bound is less than 4 in this version.)
  }

  if ( countS > 9 ) {
    thetaI = 0; //65
    //vE5 = 0;
    //xE5 = 0;
    vE5 = sumPower;         //2017.7.21 mtakapii
    xE5 = sumSumP / 1000;   //2017.7.21 mtakapii

    sumPower = 0;
    sumSumP  = 0;
  }     //70

  for ( int i = 9 ; i > 0 ; i-- ) {
    recOmegaI[ i ] = recOmegaI[ i - 1 ];
  }     // ("int" is added instead of line 2 omitted.)

  powerScale = ( kAngle * thetaI / 100 ) + ( kOmega * omegaI / 100 )
               + ( kSpeed * vE5 / 1000 ) + ( kDistance * xE5 / 1000 );
  // 72
  power = max ( min ( 95 * powerScale / 100 , 255 ) , -255 );
  sumPower = sumPower + power + offset;          // 2017.9.. mtakapii
  sumSumP = sumSumP + sumPower; //75

  // vE5 = ??? //76
  // xE5 = ??? //77
  vE5 = sumPower;         //76a
  xE5 = sumSumP / 1000;   //77a
  // Copyright (C) 2014 ArduinoDeXXX All Rights Reserved.

}       //78
// Copyright (C) 2014 ArduinoDeXXX All Rights Reserved. //79


/********************  /mtakapii ****************************/
// Measuring temperature
void talk_temerature(float A0) {
  float   A3_in, A3;
  int temp;

  for (int j = 0 ; j < 1 ; j++) {
    atp.Synthe("ondowa  ");
    A3_in = 0;
    for ( int i = 0 ; i < samp_num ; i++) {
      A3_in += analogRead(3);
    }
    A3_in /= samp_num;
    A3 = ((A3_in * A0) / 1024) * 100 * 0.97;
    temp = A3;
    str = "<NUMK VAL=";
    str += String(temp, DEC); str += ">";
    while (atp.IsBusy());
    atp.SyntheS(str);
    while (atp.IsBusy());
    atp.Synthe("dodesu ");

    if ( temp >= 35 ) {
      while (atp.IsBusy());
      atp.Synthe("a-- gekiatu--");
    }
    if ( temp >= 25 && temp < 35 ) {
      while (atp.IsBusy());
      atp.Synthe("atuina---");
    }
    if ( temp >= 20 && temp < 25 ) {
      while (atp.IsBusy());
      atp.Synthe("kaiteki kaiteki--");
    }
    if ( temp >= 15 && temp < 20 ) {
      while (atp.IsBusy());
      atp.Synthe("chotto samui--");
    }
    if ( temp < 15 ) {
      while (atp.IsBusy());
      atp.Synthe("meccha samuiyan sinu--");
    }
  }
}

//  Measuring Volatage      /mtakapii
float talk_volt() {
  float   A0_in, A0, Ratio;
  int     D1, D2, D3, Ratio_int, i, temp;

  for (int j = 0; j < 1 ; j++) {
    A0_in = 0;
    for ( i = 0 ; i < samp_num ; i++) {
      A0_in += analogRead(0);
    }
    A0_in /= samp_num;
    A0_in /= 100.;
    A0 = (A0_in *= 1.245);     // 1.295 1.25 1.245

    D1 =  A0_in;               // float ==> integer
    D2 = (A0_in -= D1) * 10;
    D3 = (A0_in * 10 - D2) * 10;

    if ( A0 > 3.9) {
      Ratio = map_f(A0, 3.9, 4.2, 60, 100.);
    }
    if ( A0 < 3.8999) {
      Ratio = map_f(A0, 3.8, 3.8999, 32, 60.);
    }
    Ratio_int = Ratio;          // float ==> integer

    while (atp.IsBusy());
    atp.Synthe("  denatsuwa  ");

    while (atp.IsBusy());
    str = "<NUMK VAL=";
    str += String(D1, DEC); str += ">";
    while (atp.IsBusy());
    atp.SyntheS(str);
    while (atp.IsBusy());
    atp.Synthe("ten");

    str = "<NUMK VAL=";
    str += String(D2, DEC); str += ">";
    while (atp.IsBusy());
    atp.SyntheS(str);

    str = "<NUMK VAL=";
    str += String(D3, DEC); str += ">";
    while (atp.IsBusy());
    atp.SyntheS(str);
    while (atp.IsBusy());
    atp.Synthe(" boruto");

    while (atp.IsBusy());
    atp.Synthe(" nokori ");

    str = "<NUMK VAL=";
    str += String(Ratio_int, DEC); str += ">";
    while (atp.IsBusy());
    atp.SyntheS(str);
    while (atp.IsBusy());
    atp.Synthe(" pa-sennto desu  ");
    return A0;
  }
}

/*******  Rise up Sequence  2017.9.29   /mtakapii *********/
/* 2017.12.8 現在は角度の上位ゼロは省略可能です。　　      */
// Use Serial Servo by mtakapii
// #1 ==> Normal   #2 ==> Reverse

void Rise_up() {
  start_position();
  delay(2000);
  while (atp.IsBusy()) ;
  atp.Synthe("yoisho");
  stp = 3;
  wipe( 26, hensoku1, 1);           // start, stop, speed
  while (atp.IsBusy()) ;
  atp.Synthe("dokkoisho");
  stp = 2;
  wipe(hensoku1, hensoku2, 20);     // start, stop, speed
  while (atp.IsBusy()) ;
  atp.Synthe("otto tto tto");
  stp = 1;
  wipe(hensoku2, log_max, 100);     // start, stop, speed
  while (atp.IsBusy());
  atp.Synthe("hajimari hajimari---  ");
  while (atp.IsBusy());
  start_position();
  Serial.flush();
}

void term() {
  Serial.println("");
}

void wave_hand1() {
  term();
  Serial.println("1115");
  delay(20);
  term();
  Serial.println("2032");
  delay(20);
}
void wave_hand2() {
  term();
  Serial.println("1085");
  delay(20);
  term();
  Serial.println("2065");
  delay(20);
}
void wave_hand3() {
  term();
  Serial.println("1053");
  delay(20);
  term();
  Serial.println("2104");
  delay(20);
}
void wave_hand4() {
  term();
  Serial.println("1030");
  delay(20);
  term();
  Serial.println("2120");
  delay(20);
}
void start_position() {          // Set start position
  term();                        // Reset Serial Servo
  Serial.println("1152");         // #1 (Reverse)
  delay(20);
  term();                        // Reset Serial Servo
  Serial.println("2000");       // #2 (Normal)
  delay(20);
}

void Set_angle_S1S2( float angle) {
  float delay_time;

  if ( angle > log_max ) {
    angle = log_max;  // Fale Safe
  }
  int_angle = map(angle, log_min, log_max, Number_1_begin, Number_1_end);

  dec100 = int_angle / 100;  int_angle = int_angle % 100;
  dec10  = int_angle / 10 ;  dec1  = int_angle %  10;
  term();                       // Reset Serial Servo

  Serial.print ('1');
  Serial.print ( dec100 );
  Serial.print ( dec10 );
  Serial.println ( dec1 );
  delay(20);
  /*
    Serial.print ('1');
    Serial.println (int_angle);
    delay(10);
  */
  int_angle = angle;
  dec100 = int_angle / 100;  int_angle = int_angle % 100;
  dec10  = int_angle / 10 ;  dec1  = int_angle %  10;

  term();                     // Reset Serial Servo

  Serial.print ('2');
  Serial.print ( dec100 );
  Serial.print ( dec10  );
  Serial.println ( dec1   );
  delay(20);
  /*
    Serial.print ('2');
    Serial.println (int_angle);
    delay(12);
  */

  //int_delay_time = abs(pre_angle - angle) * length_time * 1000;
  //delay( int_delay_time);
  //pre_angle = angle;
}

void wipe(float start_ang, float stop_ang, int spd) {
  float ang;
  float diff;

  Set_angle_S1S2(start_ang);         // Rotate to Start position
  diff = start_ang - stop_ang;
  if ( diff > 0 ) {
    for ( ang = start_ang ; ang > stop_ang ; ang -= stp ) {
      Set_angle_S1S2(ang);
      delay(spd);
    }
  }
  else {
    for ( ang = start_ang ; ang < stop_ang ; ang += stp ) {
      Set_angle_S1S2(ang);
      delay(spd);
    }
  }
  //pre_angle = stop_ang;
}
/************************** mtakapii/ *****************************/