//Inverted Pendulum Robot
//DRV8835 driver
//wheele dia = 55mm
//Battery: NiHM x 4
//MPU: ProMini 3.3V
//Gyro: MPU6050 (I2C pullup 4.7kohm)
//Motor driver: DRV8835 connected to Arduino pin 8,9,10,11
//Distance sensor: Sharp GP2Y0A21 connected to Arduino pin A1
//Trace sensor: Reflectance (distance from axis ~= 30mm) connected to Arduino pin A0
//IR sensor PL-IRM2161-XD1 connected to Arduino pin 2

#include <Wire.h>
#include <IRremote.h>
IRrecv receiveIR(2);
decode_results results;

void motor();
void drvMotorR(int pwm);
void drvMotorL(int pwm);
void drive();
void drvMotor(int pinPhase, int pinEnable, int pwm);
void pulseMotor(int n);
void calibrate();
void getBaselineAcc();
void getBaselineGyro();
bool standing();
bool laid();
bool upright();
void calcTarget();
void getDistance();
void getGyro();
void readGyro();
void getIR();
void monitorVoltage();
void selectMode();
void getBaselineGyro();
void resetMotor();
void resetVar();
void blinkLED(int n);
void checkVoltage();
void sendSerial();

int accX, accY, accZ, temp, gyroX, gyroY, gyroZ;
int counter=0;
unsigned long time1=0;
unsigned long time0=0;
float power, powerR, powerL;
float gyroZdps, accXg;
float varAng, varOmg, varSpd, varDst, varIang, varIdst;
float gyroZoffset, gyroYoffset, accXoffset;
float gyroZdata, gyroYdata, accXdata, accZdata;
float gyroLSB=1.0/131.0; // deg/sec
float accLSB=1.0/16384.0; // g
float clk = 0.01; // in sec,
unsigned long interval = (unsigned long) (clk*1000.0);  // in msec 
float cutoff = 0.1; // 2 * PI * f (f=Hz)

float yawPower;
float moveTarget, moveRate=0.0; //moveRate 0:stop, +:forward, -:backward
float moveStep = 0.0013;
float spinDestination, spinTarget, spinRate;
float spinStep = 30.0*clk;
float orientation;
float dirStep = 30.0 * clk; //30deg/sec

int state=-1;
int traceSensor;

bool serialMonitor=;
bool spinContinuous=false;
bool follow=false;
bool trace=false;

int ipowerR = 0;
int ipowerL = 0;
int motorRdir=0; //stop 1:+ -1:-
int motorLdir=0; //stop
float battFactor=1.0;
float bVolt;

float distance;
int traceThLevel=500;
int counterOverSpd;
float aveAccX=0.0;
float aveAccZ=0.0;

float  Kang=120.0;
float  Komg=2.8;
float  KIang=2300.0;
float  Kyaw=15.0;
float  Kdst=100.0;
float  Kspd=7.0;
float  KIdst=0.0;
float  mechFactorR=0.4;
float  mechFactorL=0.4;
int  backlashSpd=15;
int  motorDeadPWM=0;
float maxSpd=250.0;

void setup() {
  TCCR1B = TCCR1B & B11111000 | B00000001;
  pinMode(2,INPUT); //IR
  pinMode(8,OUTPUT); //APhase
  pinMode(9,OUTPUT); //AEnable
  pinMode(11,OUTPUT); //BPhase
  pinMode(10,OUTPUT); //BEnable
  pinMode(13, OUTPUT); //LED
  
  Wire.begin();
  Wire.setClock(50000L);
  Wire.beginTransmission(0x68); //Gyro sensor
  Wire.write(0x6B); // Power management register
  Wire.write(0); // wake up MPU-6050
  Wire.endTransmission(true);

  receiveIR.enableIRIn(); // Start the receiver
  Serial.begin(115200);
  checkVoltage();
  resetVar();
  delay(50); //stabilize gyro
  getBaselineGyro();
  selectMode();
}

void loop() {
   monitorVoltage();
   getIR();
   traceSensor = analogRead(A0);
   getGyro();
   getDistance();
   switch (state) {
    case -1:
       calcTarget();
       if (upright()) state =0;
       break;
    case 0: //baseline
       calibrate();
       state=1;
       break;
    case 1: //run
       if (standing() && counterOverSpd <= 30) {
         calcTarget();
         drive();
       }
       else {
         sendSerial();
         resetMotor();
         resetVar();
         if (upright()) state=0;
         else state=9;
       }
       break;
    case 2: //wait until upright
       if (upright()) state=0;
       break;
    case 9: //fell
       if (laid()) state=2;
       break;
   }
   counter += 1;
   if (counter >= 100) {
     counter = 0;
     if (serialMonitor)  sendSerial();
   }
   do time1 = millis();
   while (time1 - time0 < interval);
   time0 = time1;
}

void getIR() {
    if (receiveIR.decode(&results)) {
//    Serial.println(results.value, HEX);
    switch (results.value & 0x0000FFFF) {
      case 0x728D: //LAPIO up
      case 0x7C83: //TOSHIBA up
        moveRate=4.0;  //forward
        break;
      case 0xF20D: //LAPIO down
      case 0xFC03: //TOSHIBA down
        moveRate=-4.0;  //backward
        break;
      case 0xFA05: //LAPIO&TOSHIBA left = spin left
        spinDestination -= 30.0;
        break;
      case 0xDA25: //LAPIO&TOSHIBA right = spin right
        spinDestination += 30.0;
        break;
      case 0xBC43: //LAPIO&TOSHIBA OK = stop
        trace=false;
        follow=false;
        spinContinuous=false;
        moveRate=0.0;
        spinDestination = spinTarget;
        break;
      case 0x807F: //LAPIO&TOSHIBA button 1
        spinContinuous=true;
        follow=false;
        trace=false;
        moveRate=3.0;
        spinStep=0.30;
      break;
    }
    receiveIR.resume(); // Receive the next value
  }
}

void calcTarget() {
   if (trace) {
     float tmp = (float) (traceSensor - traceThLevel);
     spinTarget += 0.001 * tmp;
     moveRate = 2.0;
   }
   else {
     if (spinContinuous) {
       spinTarget += spinStep;
     }
     else {
       if (spinTarget < spinDestination) spinTarget += spinStep;
       if (spinTarget > spinDestination) spinTarget -= spinStep;
     }
   }
   
   if (follow) {
     if (distance > 180.0) moveRate=2.8;
     else if (distance > 130.0) moveRate=2.4;
     else if (distance > 90.0) moveRate=0.0;
     else moveRate=-1.0;
   }
   moveTarget += moveStep * moveRate;
}

void drive() {
  varSpd += power * clk;
  varDst += Kdst * (varSpd * clk - moveTarget);
  varIang += KIang * varAng * clk;
  varIdst += KIdst * varDst * clk;
  power = varIang + varIdst + varDst + (Kspd * varSpd) + (Kang * varAng) + (Komg * varOmg);
  if (abs(power) > 3000.0) counterOverSpd += 1;
  else counterOverSpd =0;
  if (counterOverSpd > 30) return;
  yawPower = (orientation - spinTarget) * Kyaw;
  powerR = power + yawPower;
  powerL = power - yawPower;
  motor();
}

void motor() {
     ipowerR = (int) (constrain(powerR * mechFactorR * battFactor, -maxSpd, maxSpd));
    if (ipowerR > 0) {
      if (motorRdir == 1) drvMotorR(ipowerR + motorDeadPWM);
      else drvMotorR(ipowerR + motorDeadPWM + backlashSpd); //compensate backlash 
      motorRdir = 1;
    }
    else if (ipowerR < 0) {
      if (motorRdir == -1) drvMotorR(ipowerR - motorDeadPWM);
      else drvMotorR(ipowerR - motorDeadPWM - backlashSpd);
      motorRdir = -1;
    }
    else {
      drvMotorR(0);
      motorRdir = 0;
    }

    ipowerL = (int) (constrain(powerL * mechFactorL * battFactor, -maxSpd, maxSpd));
    if (ipowerL > 0) {
      if (motorLdir == 1) drvMotorL(ipowerL + motorDeadPWM);
      else drvMotorL(ipowerL + motorDeadPWM + backlashSpd); //compensate backlash 
      motorLdir = 1;
    }
    else if (ipowerL < 0) {
      if (motorLdir == -1) drvMotorL(ipowerL - motorDeadPWM);
      else drvMotorL(ipowerL - motorDeadPWM - backlashSpd);
      motorLdir = -1;
    }
    else {
      drvMotorL(0);
      motorLdir = 0;
    }
}

void getDistance() { //distance in mm
  int dist=analogRead(A1);
  if (dist>0) distance = constrain(95000.0 / ((float) dist) -30.0, 60.0, 400.0);
  else distance = -1.0;
}

void getGyro() {
  readGyro();
  gyroZdps = (gyroZdata - gyroZoffset) * gyroLSB;
  varOmg = (gyroYdata - gyroYoffset) * gyroLSB; // unit:deg/sec
  accXg = (accXdata - accXoffset) * accLSB; //unit:g
  orientation += gyroZdps * clk;
  varAng += (varOmg + (accXg * 57.3 - varAng) * cutoff ) * clk;
}

void readGyro() {
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  Wire.endTransmission(false); //enable incremental read
  Wire.requestFrom(0x68, 14, (int)true); //used to be requestFrom(0x68, 14, true) but got warning.
  accX=Wire.read()<<8|Wire.read(); //0x3B
  accY=Wire.read()<<8|Wire.read(); //0x3D
  accZ=Wire.read()<<8|Wire.read(); //0x3F
  temp=Wire.read()<<8|Wire.read(); //0x41
  gyroX=Wire.read()<<8|Wire.read(); //0x43
  gyroY=Wire.read()<<8|Wire.read(); //0x45
  gyroZ=Wire.read()<<8|Wire.read(); //0x47
  gyroZdata = (float) gyroY;
  gyroYdata = (float) gyroZ;
  accXdata = -(float) accX;
  aveAccX = aveAccX * 0.9 + accXdata * 0.1;
  accZdata = (float) accY;
  aveAccZ = aveAccZ * 0.9 + accZdata * 0.1;
}

bool laid() {
  if (abs(aveAccX) >13000.0) return true;
  else return false;
}

bool upright() {
  if (abs(aveAccZ) >13000.0) return true;
  else return false;
}

bool standing() {
  if (abs(aveAccZ) > 10000.0 && abs(varAng) < 40.0) return true;
  else return false;
}

void calibrate() {
  resetVar();
  resetMotor();
  delay(2000);
  if (serialMonitor)  sendSerial();
  pulseMotor(1);
  delay(100);
  if (serialMonitor)  sendSerial();
  getBaselineAcc();
}

void getBaselineAcc() {
  accXoffset=0.0;
  for (int i=1; i <= 30; i++) {
    readGyro();
    accXoffset += accXdata;
    delay(20);
  }
  accXoffset /= 30;
}

void getBaselineGyro() {
  gyroYoffset=gyroZoffset=0.0;
  for (int i=1; i <= 30; i++) {
    readGyro();
    gyroYoffset += gyroYdata;
    gyroZoffset += gyroZdata;
    delay(20);
  }
  gyroZoffset /= 30;
  gyroYoffset /= 30;
}

void resetVar() {
  power=0.0;
  moveTarget=0.0;
  spinDestination=0.0;
  spinTarget=0.0;
  spinRate=0.0;
  orientation=0.0;
  varAng=0.0;
  varOmg=0.0;
  varDst=0.0;
  varSpd=0.0;
  varIang=0.0;
  varIdst=0.0;
}

void resetMotor() {
  drvMotorR(0);
  drvMotorL(0);
  counterOverSpd=0;
  sendSerial();
}

void checkVoltage() {
  bVolt = ((float) analogRead(A2)) / 1023.0 * 6.6;
  if (bVolt > 5.0) blinkLED(3);
  else if (bVolt > 4.8) blinkLED(2);
  else if (bVolt > 4.6) blinkLED(1);
  else blinkLED(100);
}

void blinkLED(int n) {
    for (int i=0; i<n; i++) {
      digitalWrite(13,HIGH);
      delay(10);
      digitalWrite(13,LOW);
      delay(200);
    }
}

void monitorVoltage() {
    digitalWrite(13, LOW);
    bVolt = ((float) analogRead(A2)) / 1023.0 * 6.6;
    battFactor = 5.0 / bVolt;
    if (bVolt<=4.4) {
      digitalWrite(13, HIGH);
      if (serialMonitor) {
        Serial.print("Batt=");
        Serial.println(bVolt);
      }
    }
}

void selectMode() {
  getDistance();
  traceSensor = analogRead(A0);
  if (distance > 100.0) {
    if (traceSensor > traceThLevel) {  //follow & trace
      trace=true;
      follow=true;
      blinkLED(3);
    }
    else {                   //stand still
      trace=false;
      follow=false;
      blinkLED(1);
    }
  }
  else if (distance > 0.0) {  //circle
    follow=false;
    trace=false;
    moveRate=3.0;
    spinStep=0.30;
    spinContinuous=true;
    blinkLED(5);
  }
}

void pulseMotor(int n) {
  for (int i=0; i<n; i++) {
  drvMotorR(motorDeadPWM+50);
  delay(15);
  drvMotorR(0);
  delay(50);
  drvMotorR(-motorDeadPWM-50);
  delay(15);
  drvMotorR(0);
  delay(200);
  }
}

void drvMotorR(int pwm) {
  drvMotor(8,9, constrain(pwm, -255, 255));
}

void drvMotorL(int pwm) {
  drvMotor(11,10, constrain(pwm, -255, 255));
}

void drvMotor(int pinPhase, int pinEnable, int pwm) {
  if (pwm>=0) {
//    digitalWrite(pinEnable,LOW);
//    delayMicroseconds(10);
    digitalWrite(pinPhase,HIGH);
    analogWrite(pinEnable,pwm);
  }
  else {
//    digitalWrite(pinEnable,LOW);
//    delayMicroseconds(10);
    digitalWrite(pinPhase,LOW);
    analogWrite(pinEnable,-pwm);
  }
}

void sendSerial () {
    Serial.print(millis()-time0);
    Serial.print(" state=");Serial.print(state);
//    Serial.print(" dist=");Serial.print(distance);
//    Serial.print(" accX="); Serial.print(accXdata);
//    Serial.print(" accOfs=");Serial.print(accXoffset);
//    Serial.print(" gyYoffset="); Serial.print(gyroYoffset);
//    Serial.print(" ang=");Serial.print(varAng);
//   Serial.print(" temp = "); Serial.print(temp/340.0+36.53);
//   Serial.print(" Omg="); Serial.print(varOmg);
//   Serial.print(" power="); Serial.print(power);
//   Serial.print(" Dst="); Serial.print(varDst);
     Serial.print(" bVolt="); Serial.print(bVolt);
//   Serial.print(" Iang="); Serial.print(varIang);
//   Serial.print(" mTarg="); Serial.print(moveTarget);
//   Serial.print(" follow="); Serial.print(follow);
//   Serial.print(" interval="); Serial.print(interval);
    Serial.print(" "); 
    Serial.println(millis()-time0);
}