//  This is the motor controller code for the Hcr and this include the encoder
//  2014.3.3
//  V 0.1
//  By Pepin

#include <Metro.h>
#include <Wire.h>
#define Pbn  8  //declare the number of the I2C code
int E1 = 9;     //M1 Speed Control
int E2 = 10;     //M2 Speed Control
int M1 = 8;     //M1 Direction Control
int M2 = 11;     //M1 Direction Control
int a; //output of the motor() 
int b; 
int counter=0;
const byte encoder0pinA = 2;//A pin -> the interrupt pin 0
const byte encoder0pinB = 4;//B pin -> the digital pin 4
const byte encoder1pinA = 3;//A pin -> the interrupt pin 3
const byte encoder1pinB = 5;//B pin -> the digital pin 5
// used to calculate the encoder pulses
byte encoder0PinALast;
byte encoder1PinALast;
int Lduration;//the number of the pulses
int Rduration;
boolean LDirection;//the rotation direction 
boolean RDirection;
//
byte wheelDir = 0x00;
int _CMDspeed[2] = {
  0,0};
//
float _speedleft,_speedright; //speed of the wheels
float _perimeterA,_FirmPulsePG;
int pastCoder[2];
long totalCoder[2];
#define LF 0
#define RT 1
float _proportion,_integral,_derivative,_maximum,_minimum;
int _speedtarget[2];
double _lasterror[2];
double _preverror[2];
int i;
int _Loutput,_Routput;
//



//


Metro DataTrans = Metro(1000,true);//used to check the output
Metro BehaviorInterval = Metro(25,true);// every 25ms output the speed of the motors

void setup()
{  
  int i;
  for(i=4;i<=7;i++)
    pinMode(i, OUTPUT);
   delay(1000);
  Wire.begin(10);
  Wire.onRequest(requestEvent); // register event
  Wire.onReceive(readEvent);
  digitalWrite(E1,LOW);   
  digitalWrite(E2,LOW); 
  

  Serial.begin(57600);//Initialize the serial port
  EncoderInit();//Initialize the module
  //declare some const 
  _perimeterA = 42.72566*1000;
  _FirmPulsePG = 1326;
// PID
  _proportion = 3;
  _integral = 0.5;
  _derivative = 0.6;
  _maximum = 500;
  _minimum = _maximum*(-1);
  i=0;

}

void loop()
{
 // int _Loutput,_Routput;
  if(DataTrans.check())
  {    
/* these comment help to check some data from encoder and input */
//    Serial.print(_speedtarget[LF]);
//    Serial.print(",");
//    Serial.print(_speedtarget[RT]);
//    Serial.print(",");
//    Serial.print(pastCoder[LF]);
//    Serial.print(",");
//    Serial.print(pastCoder[RT]);
//    Serial.print(",");
//    Serial.print(_Loutput);
//    Serial.print(",");
//    Serial.print(_Routput);
//    Serial.print("\n");
//pastCoder[LF]=0;
//pastCoder[RT]=0;
    /*
    Serial.print(Lduration);
    Serial.print(",");
    Serial.println(Rduration);
     */
  }

     if(BehaviorInterval.check())
  {
    static int lastLspeed = 0;
    static int lastRspeed = 0;
    ResentSpeed();
  
    float Lpara,Rpara;
 //calculate the targetspeed to the PWM number
    Lpara = TVPIDcal(_speedleft,true);
    Rpara = TVPIDcal(_speedright,false);

    _Loutput = int(TVAffect(Lpara));
    _Routput = int(TVAffect(Rpara));
 
    Motor(_Loutput,LF);
    Motor(_Routput,RT);
  }
   

}

void EncoderInit() //init of the encoder
{
  LDirection = true;
  RDirection = true;//default -> Forward  
  pinMode(encoder0pinA,INPUT_PULLUP);
  pinMode(encoder0pinB,INPUT_PULLUP); 
  pinMode(encoder1pinA,INPUT_PULLUP);
  pinMode(encoder1pinB,INPUT_PULLUP);  
  attachInterrupt(0, LwheelSpeed, CHANGE);
  attachInterrupt(1, RwheelSpeed, CHANGE);
}

void LwheelSpeed()  // the encoder code 
{
  int Lstate = digitalRead(encoder0pinA);
  if((encoder0PinALast == LOW) && Lstate==HIGH)
  {
    int val = digitalRead(encoder0pinB);
    if(val == LOW && LDirection)
    {
      LDirection = false; //Reverse
    }
    else if(val == HIGH && !LDirection)
    {
      LDirection = true;  //Forward
    }
  }
  encoder0PinALast = Lstate;

  if(!LDirection)  Lduration++;
  else  Lduration--;
}

void RwheelSpeed()
{
  int Rstate = digitalRead(encoder1pinA);
  if((encoder1PinALast == LOW) && Rstate==HIGH)
  {
    int val = digitalRead(encoder1pinB);
    if(val == LOW && RDirection)
    {
      RDirection = false; //Reverse
    }
    else if(val == HIGH && !RDirection)
    {
      RDirection = true;  //Forward
    }
  }
  encoder1PinALast = Rstate;

  if(!RDirection)  Rduration++;
  else  Rduration--;
}

/************************************************ calculate speed (cm/s) ***********************************************/

float lastspeed(int longs,int diff)
{
  double internum = _perimeterA;
  internum /= diff;
  float dist = float(internum*longs);
  dist /= _FirmPulsePG;
  return dist;
}

void ResentSpeed()
{
  static int pasttime;
  static unsigned long lasttime;
  static unsigned long now;
  now = millis();
  pasttime = now - lasttime;
  lasttime = now;

  _speedleft = lastspeed(Lduration , pasttime);
  _speedright = lastspeed(Rduration , pasttime);
/* check the encoder */
  //  Serial.println(pasttime);
  //  Serial.print("  ");
  //  Serial.println(Lduration);
  
  pastCoder[LF] += Lduration;
  pastCoder[RT] += Rduration;

  Lduration = 0;
  Rduration = 0;
}

/************************************************* Motor Control ***********************************************/

void Motor(int value,byte whichwheel)
{
  value = constrain(value,1000,2000);
   
  if(whichwheel == LF)
  {
    if(value>1500)
    {
      a=(value-1500)/1.961;
      analogWrite (E1,a);
      digitalWrite(M1,HIGH);
    }
    else
    {
      a=(1500-value)/2;
      analogWrite (E1,a);
      digitalWrite(M1,LOW);
    }
  }
  else if(whichwheel == RT)
  {
    if(value>1500)
    {
      b=(value-1500)/1.961;
      analogWrite (E2,b);
      digitalWrite(M2,HIGH);
    }
    else
    {
      b=(1500-value)/2;
      analogWrite (E2,b);
      digitalWrite(M2,LOW);
    }
  }
}

/************************************************* PID Control ***********************************************/


float TVPIDcal(float prevspeed,boolean target)
{
  static int sumerror[2];
  static int i;
  if(target)
    i = 0;
  else
    i = 1;
  int derror;
  int error = _speedtarget[i] - prevspeed;

  sumerror[i] += error;
  sumerror[i] = min(_maximum,sumerror[i]); //limit the range of intergral segment
  sumerror[i] = max(_minimum,sumerror[i]);

  derror = _lasterror[i] - _preverror[i];
  _preverror[i] = _lasterror[i];
  _lasterror[i] = error;

  return (_proportion*error+_integral*sumerror[i]+_derivative*derror);
}

int TVAffect(float pidpara)
{
  float result = 500;
  float factor;

  if(pidpara>_maximum)
    factor = 1;
  else if(pidpara>0)
    factor = pidpara/_maximum;
  else if(pidpara<_minimum)
    factor = -1;
  else
    factor = pidpara/_maximum;

  result *= factor;
  result += 1500;

  return result;
}

/********************** I2C Slaver ***********************/

byte readByte[10];
unsigned long timer = 0;

void readEvent(int HowMany)
{
  unsigned long St = micros();
  int length = 0;
//  Serial.println(micros());

  if(Wire.available())
  {

    byte HeaderByte = Wire.read();
    if(HeaderByte == 0x46)
    {
      int i;
      //delay(10);
      for(i = 0 ;i <10 ;i ++)  readByte[i] = 0;
      i = 0;
//      Serial.print("OK\t");
      readByte[i] = HeaderByte;
      i ++;
      while(Wire.available())
      {
        readByte[i] = Wire.read();
        //Serial.print(readByte[i],BYTE);
        length = i;
        if(readByte[5] == 0x0A || (micros() - St > 2000))
        { 
          while(Wire.available())
          {
            byte BufferClear = Wire.read();
          }
          break;
        }
        i ++;
      }
      //Serial.println();
      //Serial.println(micros() - St);
      //while(Wire.available())
    }
    else
    {
      Serial.print("Err");
      while(Wire.available())
      {
        byte BufferClear = Wire.read();
      }
      //Serial.println(micros() - St);
    }
  }
    /* check the transfer data */
  /*
  for( int j = 0;j <= length;j++)
  {
    Serial.print(j);
    Serial.print(":");
    Serial.println(readByte[j],HEX);
  }
  Serial.println();
   */
  
  
  if(readByte[5] == 0x0A)  
  {
    wheelDir = readByte[2];
    _CMDspeed[LF] = readByte[3];
    _CMDspeed[RT] = readByte[4];

    if(wheelDir == 0x00||wheelDir == 0x01)
      _speedtarget[LF] = _CMDspeed[LF];
    else
      _speedtarget[LF] = _CMDspeed[LF]*(-1);

    if(wheelDir == 0x00||wheelDir == 0x10)
      _speedtarget[RT] = _CMDspeed[RT];
    else
      _speedtarget[RT] = _CMDspeed[RT]*(-1);

    for(int i = 0; i < 2; i++)  _speedtarget[i] = constrain(_speedtarget[i],-100,100);
/* test the time cost */
//     Serial.print(millis() - timer);
//     timer = millis();
     
//     Serial.print(_speedtarget[LF]);
//     Serial.print(",");
//     Serial.print(_speedtarget[RT]);
//     Serial.println("");
  }
}

void requestEvent()
{  
  byte RequestString[Pbn] = {
    'F','E',0,0,0,0,0,10  };
  
  RequestString[2] = 0x00;
  
  if(pastCoder[LF] < 0)  bitWrite(RequestString[2],4,1);
  if(pastCoder[RT] < 0)  bitWrite(RequestString[2],0,1);
//  Serial.println(RequestString[2],HEX);
  
  RequestString[3] = abs(pastCoder[LF]) / 256;  
  RequestString[4] = abs(pastCoder[LF]) % 256; 
  RequestString[5] = abs(pastCoder[RT]) / 256;  
  RequestString[6] = abs(pastCoder[RT]) % 256; 
  
  totalCoder[LF] += pastCoder[LF];
  totalCoder[RT] += pastCoder[RT];
  
  pastCoder[LF] = 0;
  pastCoder[RT] = 0;
  
  Wire.write(RequestString,Pbn);
  // test the data transfered */ 
  /*
  for( int j = 0;j < Pbn;j++)
  {
    Serial.print(j);
    Serial.print(":");
    Serial.println(RequestString[j],HEX);
  }
  Serial.println();
   */
  
}