/********************************************************************************
Tracking controller for RoboDesigner ver.2   by S.Yamakawa 20190422
  
Robot: RDS-X, typeIII
SDK:  Arduino 1.0.5-r2

********************************************************************************/
//============ Parameters of robot (change according to your robot) =============
float d=0.120;      //tread[m]
float r=0.064;      //tire's diameter[m]
float a = r*3.14159265/768.0;   //768 is encoder's resolution [pulse/rot]
//============ Pin assignment (change according to your robot) ==================
int M1_1 = 4;
int M1_2 = 5;
int M1_PWM = 6;
int M2_1 = 7;
int M2_2 = 8;
int M2_PWM = 9;
int P_PUSH = 12;
int encoder1 = 2; //D0
int encoder2 = 3; //D1
//============ Control parameter(default value) =================================
float vd = 0.1;     //translational velocity[m/s]
float yr = 0.0;     //desired trajectory[m]
int fc = 0;         //desired function 0:constant, 1:trianglar wave, 2:cosine wave
float amp = 0.0;    //desired value[m]
float pd = 1.0;     //period of desired trajectory[m]
float xd = 0.5;     //limit of x[m]
float kp = 30.0;    //kp gain
float ki = 10.0;    //ki gain
float kd = 45.0;    //kd gain
//============ Parameters for odometry and control ==============================
volatile int enCounter_r, enCounter_l;  
int sum = 0;		
float vr = 0.0;	
float vl = 0.0;
float x = 0.0;	
float y = 0.0;	
float theta = 0.0;
float theta_old = 0.0;
float v1 = 0.1;		
float v2 = 0.0;   
float yrd = 0.0;  
float yrdd = 0.0;
float u1 =0;
float u2 =0;
float v1i = 0.0;
float e = 0.0;
float e0 = 0.0;
float ei = 0.0;
float ed = 0.0;
float m2=0.0;
float u2d=0.0;
float u2i=0.0;
long time;
long time_old = 0;
long dt = 0;
long smpltime;
float temp;
int j=0;
int law =2;
int input =0;
int inputend =0;
float para[3]={0.0, 0.0, 0.0};
void motorDrive(int, int, int, float);
void count_l();
void count_r();
int serialreadint();

/*******************************************************************************
  Initialization
*******************************************************************************/
void setup() {                
  Serial.begin(9600);
  pinMode(M1_1, OUTPUT);
  pinMode(M1_2, OUTPUT);
  pinMode(M1_PWM, OUTPUT);
  pinMode(M2_1, OUTPUT);
  pinMode(M2_2, OUTPUT);
  pinMode(M2_PWM, OUTPUT);
  pinMode(P_PUSH,INPUT_PULLUP);
  attachInterrupt(encoder1, count_l, CHANGE);
  attachInterrupt(encoder2, count_r, CHANGE);
  enCounter_r = 0;
  enCounter_l = 0;

//============ Read parameters from PC ==========================================
  while( inputend < 9 ) {
    if(Serial.available() > 0 ) {
      input = serialreadint();
      if(inputend==0) law=input;
      else if(inputend==1) fc=int(input);
      else if(inputend==2) amp=float(input)/1000.0;
      else if(inputend==3) pd=float(input)/1000.0;
      else if(inputend==4) xd=float(input)/1000.0;
      else if(inputend==5) vd=float(input)/1000.0;
      else para[inputend-6]=float(input); 
      inputend=inputend+1;
    };
  };
if(fc==0){
   yr = amp;
   e0 = yr;
  };

//============ Wait until the button is pushed ==================================
  while( digitalRead(12)==1){};
  delay(500);
  time_old=millis();
  smpltime=millis();
}

/*******************************************************************************
  Main loop
*******************************************************************************/
void loop(){
//============ Odometry =========================================================
  time=millis();
  dt = (time - time_old);
  time_old = time;
  theta =float(enCounter_r - enCounter_l)*a/d;
    if(dt != 0){
    v2 =(theta-theta_old)/dt*1000.0;
    temp = float((enCounter_r + enCounter_l)-sum);
    sum = enCounter_r + enCounter_l;
    v1 = temp /2.0/ float(dt) *1000.0 * a;
    x = x + (temp * cos(theta) * a /2.0);
    y = y + (temp * sin(theta) * a /2.0);
  };
  theta_old=theta;
//============ Control law ======================================================
  v1i = v1i + (vd - v1)*float(dt)/1000.0;
  u1 =2.0*(vd - v1)+ 8.0*v1i;    //translational control(PI control)
  if(law==1){          //PID control
    if(fc==1){
      j= 2 * x / pd;
      if ((j % 2) == 0) yr = amp/pd * ( 2*x - j*pd );
      else yr = amp - amp/pd * ( 2*x - j*pd );
    } else if (fc==2){
      yr=amp/2*(1-cos(2*3.14159/pd*x));
    };
    e = (yr - y);
    ei = ei + e*float(dt)/1000.0;
    ed = (e - e0)/float(dt)*1000.0;
    u2 = (para[0]*e + para[1]*ei + para[2]*ed)/100.0;
    e0 = e;
  }else if(law==2){    //nonlinear control
    if(fc==1){
      j= 2 * x / pd;
      yrdd=0.0;
      if((j % 2) == 0) {
       yr = amp/pd * ( 2*x - j*pd );
       yrd= 2*amp/pd;
      } else {
       yr = amp - amp/pd * ( 2*x - j*pd );
       yrd= -2*amp/pd;
      }
    }else if(fc==2){
      yr=amp/2*(1-cos(2*3.14159/pd*x));
      yrd=amp*3.14159/pd*sin(2*3.14159/pd*x);     //amp/2*(2Pi/pd)*sin(2Pi/pd*x)
      yrdd=amp*19.7392/pd/pd*cos(2*3.14159/pd*x); //amp/2*(2Pi/pd)^2*cos(2Pi/pd*x)
    };
    m2 = (-para[0]* (y - yr) -para[1] * (tan(theta) - yrd))/10.0 + yrdd;
    u2d = v1 * cos(theta) * cos(theta)* cos(theta)* m2;
    u2i = u2i + (u2d - v2) * float(dt)/1000.0;
    u2 = (0.08 * (u2d - v2)+20.0 * u2i);
  } else {    //uncontrolled
    u2 = 0.0;
  };
  vr = u1 + u2*d/2.0;
  vl = u1 - u2*d/2.0;
  if(x>xd){
    motorDrive(M1_1, M1_2, M1_PWM, 0.0);
    motorDrive(M2_1, M2_2, M2_PWM, 0.0);
  }else{
//============ Motor input ======================================================
    motorDrive(M1_1, M1_2, M1_PWM, vl);
    motorDrive(M2_1, M2_2, M2_PWM, vr);
//============ Serial communication =============================================
    Serial.print("S");
    Serial.print(x,DEC);
    Serial.print(",");
    Serial.print(y,DEC);
    Serial.print(",");
    Serial.print(yr,DEC);
    Serial.print(",");
    Serial.print(theta,DEC);
    Serial.println("E");
  }
}

/*******************************************************************************
  Motor function  (change according to your robot)
*******************************************************************************/
void motorDrive(int m1Pin, int m2Pin, int PWMpin, float v){
  int maxDuty = 150;
  int km = 600.0;
  int duty;
  if (v > 0){
    digitalWrite(m1Pin, HIGH);
    digitalWrite(m2Pin, LOW);
    duty=(int)(v*km);
  } else if(v<0) {
    digitalWrite(m1Pin, LOW);
    digitalWrite(m2Pin, HIGH);
    duty=(int)(-v*km);
  } else {
    digitalWrite(m1Pin, LOW);
    digitalWrite(m2Pin, LOW);
    duty = 0;
  }
  if(duty>maxDuty) duty=maxDuty;
  analogWrite(PWMpin, duty);
}
/*******************************************************************************
  Interrupt function for encoder
*******************************************************************************/
void count_l() {
  if (vl>=0)  enCounter_l++;
  if (vl<0)  enCounter_l--;
}
void count_r() {
  if (vr>=0)  enCounter_r++;
  if (vr<0)  enCounter_r--;
}
/*******************************************************************************
  Serial communication function
*******************************************************************************/
int serialreadint() {
    char c[10] = "0";
    for ( int i = 0; i < 10; i++ ) {
        c[ i ] = Serial.read();
        if ( c[ i ] == '\0' )
            break;
    } 
    return atoi( c );
}