#include<iostream>
#include<string>
#include"eyebot++.h"
#include"PID.h"
using namespace std;

float Kp = 0.815, Ki = 0.0083, Kd = 0.167;   //定义PID的参数
float Kpid;
int Kpid_speed;
const int speed_l = 18;
const int speed_r = speed_l - 2;
const int speed_err = 2;
const float Kpid_base = 1;

/*---定义全局变量---*/
int err, err_old, err_sum, err_diff;    //定义误差，上一次误差，积分误差，微分误差
const int eI_max = 200;
/*---定义全局常量---*/
/*---定义全局常量---*/
const int DIST_cell = 310;
const int DIST_wall_f = 75;
const int DIST_wall_rl = 90;
/*---全局各种变量与常量定义完成---*/


/*---对PID控制的积分部分I限幅---*/
void eI_xianfu(int eI_max)
{
  if (err_sum >= eI_max)    //如果err_sum大于限幅，则直接让err_sum=限幅，防止err_sum过大，影响PID控制
  {
    err_sum = eI_max;
  }
  else if (err_sum <= -eI_max)
  {
    err_sum = -eI_max;
  }
  else
  {
    //此处为空;
  }
}

/*---位置式PID算法---*/
void PID_AL()
{
  err_sum += err;   //积分累计误差值
  eI_xianfu(eI_max);   //积分部分限幅
  err_diff = err - err_old;   //微分部分误差值
  /*---打印输出PID控制的各项参数1---*/
  cout << "err:" << err << " " ;
  cout << "err_old:" << err_old << " " ;
  cout << "err_sum:" << err_sum << " " ;
  cout << "err_diff:" << err_diff << " " ;
  /*---打印结束---*/
  err_old = err;    //将误差幅值到上一次误差
  
  Kpid = Kp*err + Ki*err_sum + Kd*err_diff;   //PID控制算法输出的倍率
  Kpid_speed = Kpid * Kpid_base;
  if (Kpid_speed >= speed_err)  //对pid输出值进行限制，防止过大电机过快，或者减的过小使电机不转动
  {
    Kpid_speed = speed_err;
  }
  else if (Kpid_speed <= -speed_err)
  {
    Kpid_speed = -speed_err;
  }
  else 
  {
    //error
  }
  /*---打印输出PID控制的各项参数2---*/
  cout << "PID_out:" << Kpid_speed << " " ;
  /*---打印结束---*/
  
  MOTORDriveRaw(2,Kpid_speed + speed_r);   //控制小车的行驶速度，角速度
  MOTORDriveRaw(1,speed_l);

}

/*---PID控制下的直线行驶---*/
void PIDStraight()
{
  int DIST_move;    //定义基础角速度和速度,和移动距离
  int L_PSD, R_PSD, F_PSD;    //定义左侧，右侧，前方的距离值
  //~ int DIST_move_x1;
  //~ int DIST_move_x2;
  //~ int L_PSD_old, R_PSD_old, F_PSD_old;
  int x_1,x_2, y_1,y_2, phi_1,phi_2;    //VWGetPosition的参数定义
  // speed1 = speed;
  
  //~ F_PSD = PSDGetRaw(1);
  //~ F_PSD_old = F_PSD;
  
  //~ L_PSD = PSDGetRaw(2) - 8;
  //~ L_PSD_old = L_PSD;
  
  //~ R_PSD = PSDGetRaw(3);
  //~ R_PSD_old = R_PSD;

  VWGetPosition(&x_1, &y_1, &phi_1);    //获取机器人在移动前的x,y,phi值
  //~ DIST_move_x1 = F_PSD;
  
  MOTORDriveRaw(2,speed_r);
  MOTORDriveRaw(1,speed_l);


  do 
  {
    F_PSD = PSDGetRaw(1) - 5;    //读取前方和墙壁的距离
    //~ F_PSD = PSDGetRaw(1) * 0.3 + F_PSD_old * 0.7;
    //~ F_PSD_old = F_PSD;
    
    L_PSD = PSDGetRaw(2) - 10;   //读取左侧和墙壁的距离
    //~ L_PSD = PSDGetRaw(2) * 0.3 + L_PSD_old * 0.7;
    //~ L_PSD_old = L_PSD;
    
    R_PSD = PSDGetRaw(3);    //读取右侧和墙壁的距离
    //~ R_PSD = PSDGetRaw(3) * 0.3 + R_PSD_old * 0.7;
    //~ R_PSD_old = R_PSD;
    
    /*---打印输出PSD数值---*/
    cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << " " ;
    /*---打印结束---*/
    if (20<L_PSD && L_PSD<180  &&  20<R_PSD && R_PSD<180)   //如果两侧都有墙的情况
    {
      err = L_PSD - R_PSD;
      PID_AL();
    }
    else if (20<L_PSD && L_PSD<180)    //只有左侧有墙的情况
    {
      err = L_PSD - DIST_wall_rl;
      PID_AL();
    }
    else if (20<R_PSD && R_PSD<180)    //只有右侧有墙的情况
    {
      err = DIST_wall_rl - R_PSD;
      PID_AL();
    }
    else
    {
      L_PSD = L_PSD % DIST_cell;
      R_PSD = R_PSD % DIST_cell;
      err = L_PSD - R_PSD;
      PID_AL();
    }
    
    VWGetPosition(&x_2, &y_2, &phi_2);
    DIST_move = sqrt(pow(x_2-x_1,2) + pow(y_2-y_1,2));
    
    //~ DIST_move_x2 = F_PSD;
    //~ DIST_move = DIST_move_x1 - DIST_move_x2;
    
    cout << "move: " << DIST_move << endl;
    
    
    // if (DIST_move > (DIST_cell -50))    //当移动距离只剩下最后的50mm的时候降点速度
    // {
    //   speed1 = speed2;
    // }
    
  }while (DIST_move < DIST_cell  &&  F_PSD > DIST_wall_f);    //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
  //~ }while (F_PSD > DIST_wall_f);    //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
  //~ MOTORDriveRaw(1,-15);
  //~ MOTORDriveRaw(2,-15);
  MOTORDriveRaw(1,0);
  MOTORDriveRaw(2,0);
  //VWSetSpeed(0,0);
}