//IDG330 Mannual, 2mv= 1度/s 的角速率,ad读数1024,3.3v,那么每读数对应 3.223mV,所以每读数对应3.223/2/180*PI= 0.028123弧度/秒
static const double SEMICIRCLE_ARC = 57.29578;  /*半圆对应的弧度值*/
static const double GYRO_OPERATOR = 0.028123;  /*AD读取的陀螺仪数值对应的弧度算子*/
/*kalman*/
static const double C_0 = 1;
static const double Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.005;//注意：dt的取值为kalman滤波器采样时间
double P[2][2] = {{ 1, 0 },
                  { 0, 1 }};
double Pdot[4] ={ 0,0,0,0};
double q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
double angle, angle_dot;

/*sensor*/
double sensorPort[6] = { 0, 1, 2, 3, 4, 5};/*传感器地址,电路决定.accZ,gyroX, AD1, AD2, AD3, AD4*/
double sensorValue[6];/*传感器的返回值*/
/*传感器零点 0:Z轴(平行轴) 1:陀螺仪中点*/
double sensorZero[2] = {499,505};
double sensorAdjusted[2];/*传感器的返回值重整*/
//double provAngle;
/*moto*/
int E1 = 6;
int E2 = 9;
int M1 = 7;
int M2 = 8;
double deadAreaCompensation1 = 45,deadAreaCompensation2 = 35;
/*balance*/
double RATE[4] = {  0,0,0,0};/*公式中的4个变量*/
double  K[4] = {  60.45, 1.27, 125, 0.75};/*公式中的4个常量*/
double  K_AD[4] = {  1, 1, 1, 1};/*公式中的4个常量*/
double wheel_ls[7];/*左轮:0:编码器 1:位移(position) 2:position_dot 3:position_dot_filter 4:speedNeed 5:turnNeed 6:speedOutPut */
double wheel_rs[7];/*右轮:0:编码器 1:位移(position) 2:position_dot 3:position_dot_filter 4:speedNeed 5:turnNeed 6:speedOutPut */

double Torque;/*扭矩*/
unsigned int count,count2;
boolean OK=false;//这个是误差达到一定程度后的系统关闭开关.
int bf,X,Y;//从无线端发来的命令

/*kalman*/
/*angle_m:经过atan2(ax,ay)方法计算的偏角,弧度值
 gyro_m:经过初步减去零点的陀螺仪角速度值,弧度值
 */
void Kalman_Filter(double angle_m,double gyro_m)
{
  angle+=(gyro_m-q_bias) * dt;
  angle_err = angle_m - angle;
  Pdot[0]=Q_angle - P[0][1] - P[1][0];
  Pdot[1]=- P[1][1];
  Pdot[2]=- P[1][1];
  Pdot[3]=Q_gyro;
  P[0][0] += Pdot[0] * dt;
  P[0][1] += Pdot[1] * dt;
  P[1][0] += Pdot[2] * dt;
  P[1][1] += Pdot[3] * dt;
  PCt_0 = C_0 * P[0][0];
  PCt_1 = C_0 * P[1][0];
  E = R_angle + C_0 * PCt_0;
  K_0 = PCt_0 / E;
  K_1 = PCt_1 / E;
  t_0 = PCt_0;
  t_1 = C_0 * P[0][1];
  P[0][0] -= K_0 * t_0;
  P[0][1] -= K_0 * t_1;
  P[1][0] -= K_1 * t_0;
  P[1][1] -= K_1 * t_1;
  angle	+= K_0 * angle_err;
  q_bias += K_1 * angle_err;
  angle_dot = gyro_m-q_bias;//也许应该用last_angle-angle
}

void do_balance() {
  while(Serial.available() > 0){
    bf = Serial.read();
  }
  if(bf!=-1){//计算命令速度
      Y = bf/16;//bf最大255
      X = bf%16;//
  }
  
  wheel_ls[4] = (8>X)?1:(8==X?0:-1);//前进后退
  wheel_rs[4] = (8>X)?1:(8==X?0:-1);
  
  wheel_ls[5] = (8-Y)*16;//转弯
  wheel_rs[5] = (Y-8)*16;
  
  
  sensorValue[0] = analogRead(sensorPort[0]);//Z轴加速度
  sensorValue[1] = analogRead(sensorPort[1]);//X轴旋转(陀螺)
  sensorValue[2] = analogRead(sensorPort[2]);//旋钮1
  sensorValue[3] = analogRead(sensorPort[3]);//旋钮2
  sensorValue[4] = analogRead(sensorPort[4]);//旋钮3
  sensorValue[5] = analogRead(sensorPort[5]);//旋钮4

  //算出整理过的零点值
  sensorAdjusted[0] = sensorValue[0] - sensorZero[0];
  if(abs(sensorAdjusted[0]) > 70)//如果倾斜到一定程度,就停机
  {
    OK = false;
    sensorAdjusted[0] =sensorAdjusted[0]>0?70:-70;
  }  
  sensorAdjusted[1] = sensorZero[1] - sensorValue[1];/*减去初始零点*/

  
  /*根据可变电阻改变K的倍数*/
  //deadAreaCompensation = (sensorValue[4]+1)/10.24;
  //K_AD[0] = (sensorValue[2]+1)/256;//102.4;/*值在0.001~10之间变化.在AD=100的时候接近1 */
  //K_AD[1] = (sensorValue[3]+1)/256;//102.4;
  //K_AD[2] = (sensorValue[4]+1)/256;//102.4;
  //K_AD[3] = (sensorValue[5]+1)/256;//102.4;
  
  /*根据上一周期Torqu的值统计轮子转动积分*/
  wheel_ls[2] = Torque > 0?wheel_ls[0]:-wheel_ls[0];
  wheel_rs[2] = Torque > 0?wheel_rs[0]:-wheel_rs[0];
  wheel_ls[0] = wheel_rs[0] = 0;
/*************** balance *********************************************/

  /*小车初始状态*/
  {
    if(!OK)
    {
      if(abs(sensorAdjusted[0]) <= 3)
      {
        count=0;
        OK = true;
      }
      for(int i = 0; i <7;i++)
      {
        wheel_ls[i] = 0;
        wheel_rs[i] = 0;
      }
    }
  }

  /*balance*/
  {
    /*GYRO_OPERATOR = 0.028123 AD读取的陀螺仪数值对应的弧度算子*/
    Kalman_Filter(atan2(sensorAdjusted[0], sqrt(6400-sensorAdjusted[0]*sensorAdjusted[0])), sensorAdjusted[1] * GYRO_OPERATOR);//6400:因为只有一轴加速度计,所以虚拟一个斜边.这可能是小车晃动的原因之一
    RATE[0] = angle * SEMICIRCLE_ARC;//SEMICIRCLE_ARC=57.29578;  /*半圆对应的弧度值*/
    RATE[1] = angle_dot * SEMICIRCLE_ARC;
    //RATE[1] = RATE[0] - provAngle;
    //provAngle = RATE[0];
  }

  /*计算速度  double wheel_ls[8];  0:编码器累加 1:位移(position) 2:position_dot 3:速度(position_dot_filter) 4:speedNeed 5:turnNeed 6:speedOutPut 
  */
  {
    wheel_ls[3] *= 0.95;              /*车轮速度滤波,wheel_ls[3] : position_dot_filter*/
    wheel_ls[3] += wheel_ls[2]*0.05;  /*wheel_ls[2] : position_dot*/
    wheel_ls[1] += wheel_ls[3];       /*wheel_ls[1] : position*/
    wheel_ls[1] += wheel_ls[4];       /*wheel_ls[4] : speedNeed*/
    wheel_ls[1] = max(-50, wheel_ls[1]);
    wheel_ls[1] = min(50, wheel_ls[1]);
    /*
    wheel_rs[3] *= 0.85;
    wheel_rs[3] += wheel_rs[2]*0.15;
    wheel_rs[1] += wheel_rs[3];
    wheel_rs[1] += wheel_rs[5];
    wheel_rs[1] = max(-50, wheel_rs[1]);
    wheel_rs[1] = min(50, wheel_rs[1]);
*/
    RATE[2] = wheel_ls[3];//速度--滤波过了
    RATE[3] = wheel_ls[1];//位置
  }
 
  /*Torque 综合所有参数算出扭矩*/
  {
    Torque = (RATE[0]+(8-X)) * K[0] * K_AD[0] + RATE[1] * K[1] * K_AD[1] + RATE[2] * K[2] * K_AD[2] + RATE[3] * K[3]* K_AD[3];
   
    //根据扭矩算轮子的命令值
    wheel_ls[6] = abs(Torque+wheel_ls[5]) + deadAreaCompensation1;//wheel_ls[6]:扭矩输出   wheel_ls[5]:Turn_Need   deadAreaCompensation1:左轮的死区补偿
    wheel_ls[6] = min(255, wheel_ls[6]);//限制最大扭矩255
    wheel_ls[6] = OK?wheel_ls[6]:0;//当机体倾斜角大于60度的时候停止

    wheel_rs[6] = abs(Torque+wheel_rs[5]) + deadAreaCompensation2;
    wheel_rs[6] =  min(255, wheel_rs[6]);
    wheel_rs[6] = OK?wheel_rs[6]:0;
  }

  if(Torque > 0)
  {
    digitalWrite(M2, HIGH);
    digitalWrite(M1, LOW);
  }
  else
  {
    digitalWrite(M2, LOW);
    digitalWrite(M1, HIGH);
  }
  analogWrite(E1, wheel_ls[6]); //PWM调速a==0-255
  analogWrite(E2, wheel_rs[6]);
}
void do_msg(){
  if(count%100==0)
  {
    Serial.print("$CLEAR\r\n");
    Serial.print("$GO 1 1\r\n");
    Serial.print("$PRINT ");

    Serial.print(wheel_ls[1]);
    Serial.print(" ");
    Serial.print(wheel_ls[3]);
    //Serial.print(" ");
    //Serial.print(RATE[2]);
    //Serial.print(K[0]*K_AD[0]);
    //Serial.print(" ");
    //Serial.print(K[1]*K_AD[1]);
    //Serial.print(" ");
    Serial.print("\r\n");

    Serial.print("$GO 2 1\r\n");
    Serial.print("$PRINT ");
    //Serial.print(K[0]*K_AD[0]);
    //Serial.print(" ");
    //Serial.print(K[1]*K_AD[1]);
    //Serial.print(" ");
    //Serial.print(K[2]*K_AD[2]);
    //Serial.print(" ");
    //Serial.print(K[3]*K_AD[3]);
    //Serial.print(count);
    Serial.print(wheel_ls[5]);
    Serial.print(" ");
    Serial.print(wheel_rs[5]);
    //Serial.print(count);
    //Serial.print(Torque);
    Serial.print("\r\n");
  }
}
/***************** setup-loop *************************************************/
void setup() {
  count=0;
  X=Y=8;
  bf=-1;
  //init motos
  for (int i = 6; i <= 9; i++) {
    pinMode(i, OUTPUT);
  }
  Serial.begin(9600);//115200);
  analogReference(EXTERNAL); //设置模拟输入为外部参考3.3V

  attachInterrupt(0, blinkone, CHANGE); //设置为0号中断，中断函数blink，触发方式为RISING
  attachInterrupt(1, blinktwo, CHANGE); //设置为0号中断，中断函数blink，触发方式为RISING
}
void blinkone()//中断函数
{
  wheel_ls[0] ++;
}
void blinktwo()//中断函数
{
  wheel_rs[0] ++;
}

void loop() {
  do_balance();//计算平衡
  do_msg();
  delay(3);
  count++;
  if(count>=60000)
    count=0;
}