/***************************************************************************************
Tracking controller for RoboDesigner    by S.Yamakawa 201701121
  
RDS-Xシリーズ,typeIII用　開発環境はArduino 1.0.5-r2
単相エンコーダの出力（HIGH/LOW）は割り込みでカウント(JAPAN ROBOTECH サンプルより).
PID制御を用いて目標軌道yr(x)に追従する.VC++と通信
コントローラ選択用.

********************************************************************************/
//============ロボットのパラメータ（※ロボットに合わせて変更）=========================
float d=0.120;    //d=0.110;                 //タイヤ間距離[m]
float r=0.064;    //r=0.068;               //タイヤ直径[m]
float a = r*3.14159265/768.0;  //1pulseあたり進む距離[m]，ギア比48×1回転16pulse
//============ロボット制御の目標値と制御ゲイン（※自分で設定・調整する）=================
float vd = 0.1;  //並進速度目標値[m/s]
float yr = 0.1; //目標軌道[m]
float xd = 0.5; //x軸方向目標位置[m]
float kp = 30.0;    				//kp gain（の例．実際はPCから入力）
float ki = 10.0;    				//ki gain（の例．実際はPCから入力）
float kd = 45.0;    				//kd gain（の例．実際はPCから入力）
//============基板のピン設定======================================================
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
//============オドメトリ・制御用パラメータ===========================================
volatile int enCounter_r, enCounter_l;  //エンコーダの直近のカウント数
int sum = 0;		//（右エンコーダ）＋（左エンコーダ）；カウント値
float vr = 0.0;		//右タイヤの目標速度[m/s]
float vl = 0.0;		//左タイヤの目標速度[m/s]
float x = 0.0;		//車軸中心位置[m]
float y = 0.0;		//車軸中心位置[m]
float theta = 0.0;	//車両向き[rad]
float theta_old = 0.0;
float v2 = 0.0;         //車両の旋回角速度[rad/s]
float v1 = 0.1;		//並進速度[m/s]
float sign = 1.0;	//車両進行方向，1で前進，-1で後退
float u1,u2;    	//並進，旋回速度入力[m/s]
float v1i = 0.0;        //v1の偏差の積算値 
float e;                //偏差 
float e0 = yr;          //１時刻前の偏差
float ei = 0.0;         //偏差の積算値 
float ed = 0.0;         //偏差の微分値 
float m2=0.0;		//状態制御部入力
float u2d=0.0;      	//非線形旋回制御の理想値
float u2i=0.0;      	//非線形旋回制御の誤差積分値
long time;              //時刻[ms]
long time_old = 0;      //一回前の時刻[ms]
long dt = 0;            //サンプリング時間[ms]
long smpltime;
float temp;
//===========VC==========
int law =2;
int input =0;
int inputend =0;
float para[3]={0.0, 0.0, 0.0};
//===========VC==========

/*******************************************************************************
　　初期化処理
*******************************************************************************/
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;

//============PCからパラメータの読み込み×(３＋３）==================================
  while( inputend < 7 ) {
    if(Serial.available() > 0 ) {
      input = serialreadint();
      if(inputend==0) law=input;
      else if(inputend==1) yr=float(input)/1000.0; //目標軌道[m]
      else if(inputend==2) xd=float(input)/1000.0; //停止位置[m]
      else if(inputend==3) vd=float(input)/1000.0; //目標速度[m/s]
      else para[inputend-4]=float(input); //kp,ki,kd または k0,k1+dummy
      inputend=inputend+1;
    };
  };
  e0 = yr; //yrを読み込んだら積分値初期値更新

//============ボタンを押してから0.5秒後にスタート====================================
  while( digitalRead(12)==1){};
  delay(500);
  time_old=millis();
  smpltime=millis();
}

/*******************************************************************************
　　繰り返しメインループ
*******************************************************************************/
void loop(){
//============自己位置計算=========================================================
  time=millis();
  dt = (time - time_old);					                //経過時間[msec]
  time_old = time;
  theta =float(enCounter_r - enCounter_l)*a/d;		//旋回角度[rad]＝左右タイヤの走行距離差[m]÷タイヤ間の距離[m], floatを入れないと0になる．
    if(dt != 0){
    v2 =(theta-theta_old)/dt*1000.0;              //旋回角速度[rad/s]
    temp = float((enCounter_r + enCounter_l)-sum);
    sum = enCounter_r + enCounter_l;
    v1 = temp /2.0/ float(dt) *1000.0 * a;		    //並進速度[m/s]＝(左右カウンタ平均の増加分÷経過時間[sec])*a,dt=0のときは変更しない
    x = x + (temp * cos(theta) * a /2.0);		      //車軸中心x座標[m]
    y = y + (temp * sin(theta) * a /2.0);		      //車軸中心y座標[m]
  };
  theta_old=theta;
//============制御則==============================================================
  v1i = v1i + (vd - v1)*float(dt)/1000.0;
  u1 =2.0*(vd - v1)+ 8.0*v1i;    //並進方向速度入力(PI control)
  if(law==1){          //PID control
    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;    //旋回方向速度入力(PID control)
    e0 = e;
  }else if(law==2){    //nonlinear control
    m2 = (-para[0]* (y - yr) -para[1] * (tan(theta) - 0.0))/10.0 + 0.0;
    u2d = v1 * cos(theta) * cos(theta)* cos(theta)* m2;
    u2i = u2i + (u2d - v2) * float(dt)/1000.0;
    u2 = (0.08 * (u2 - 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{
//============モータへの出力=======================================================
    motorDrive(M1_1, M1_2, M1_PWM, vl);
    motorDrive(M2_1, M2_2, M2_PWM, vr);
//============シリアル通信=========================================================
    Serial.print("S");
    Serial.print(x,DEC);
    Serial.print(",");
    Serial.print(y,DEC);
    Serial.print(",");
    Serial.print(theta,DEC);
    Serial.print(",");
    Serial.print(yr,DEC);
/*      Serial.print(",");
      Serial.print(dt*10,DEC);
      Serial.print(",");
  Serial.print(para[0]*1000,DEC);
      Serial.print(",");
Serial.print(para[1]*1000,DEC);
      Serial.print(",");
Serial.print(para[2]*1000,DEC);
      Serial.print(",");
      Serial.print(kp*10000,DEC);
      Serial.print(",");
      Serial.print(ki*10000,DEC);
      Serial.print(",");
      Serial.print(kd*10000,DEC);
*/  Serial.println("E");
  }
}

/*******************************************************************************
　　モータ入力の関数
*******************************************************************************/
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);      //速度からduty比への変換
  } else if(v<0) {
    digitalWrite(m1Pin, LOW);
    digitalWrite(m2Pin, HIGH);
    duty=(int)(-v*km);      //速度からduty比への変換(←正の整数値)
  } else {
    digitalWrite(m1Pin, LOW);
    digitalWrite(m2Pin, LOW);
    duty = 0;
  }
  if(duty>maxDuty) duty=maxDuty;    //リミッタ
  analogWrite(PWMpin, duty);	    // モータのPWM設定
}
/*******************************************************************************
　　エンコーダのカウント割り込み関数
*******************************************************************************/
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--;
}
/*******************************************************************************
　　シリアルポートの読み込み割り込み関数
*******************************************************************************/
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 );
}