//开沟：
// 串口屏设置开沟参数并且按下启动-进行一次静态开沟-震动传感器感应有震动-开始开沟-震动传感器感应未振动/按下结束开沟-停止开沟
// 传感器数据离谱-报警-闪光+蜂鸣器
// 测距深度实时反馈到串口屏
//开沟板子读取主控板的三个引脚，启动引脚 开沟深度引脚 误差阈值引脚
//#include <FlexiTimer2.h>    //定时器2中断库
//#include <PWM.h>            //输出PWM库
#include <math.h> //数学函数库

#define V 10
#define x 210
#define y 180
#define m 445
#define pi 3.1415926
#define rad
//超声波引脚
#define wave_E_1 4        //传感器管脚
#define wave_T_1 5
#define wave_E_2 14
#define wave_T_2 15
#define wave_E_3 16
#define wave_T_3 17
#define wave_E_4 18
#define wave_T_4 19

#define shaking_pin 6   //接震动传感器A0引脚
#define end_pin 3     //结束开沟引脚
#define start_pin 21        //启动读取引脚
#define setDepth_pin 22     //开沟深度读取引脚
#define errorRange_pin 23   //误差阈值读取引脚
#define L298N_in1 8//引脚8控制l298N模块的in1 
#define L298N_in2 9//引脚9控制l298N模块的in2
#define L298N_in3 12//引脚12控制l298N模块的in3
#define L298N_in4 13//引脚13控制l298N模块的in4
#define stop_num 3//中断编号
#define stop_pin 20 //中断引脚

int shaking_value;
float measuredDistance; //测量距离
float setDistance; //设定的距离=开沟深度+地面距离传感器距离
float t1;
float t2;
float ditcherFlag = 1; //开沟器抬起标志位
float first_flag = 1; //首次开沟标志位
float start_flag = 1; //启动开沟标志位
float setDepth_flag = 1;
float errorRange_flag = 1;
float stop_flag = 1; //结束开沟标志位
float errorRange;     //误差阈值
float setDepth = 20;  //设定的开沟深度
float distanceAdjust;   //需要调整的深度
float i = 0;
float d1;
float d2;
float d3; //传感器测的距离
float d0 = 460; //开沟器的高度
float h1;
float h2;
float L1;
float L2;
float dL;
//滤波参数****************************************************************
float P = 1; //状态协方差
float P_;//预测的位置量
float X = 0;
float X_;
float K;
float Q = 0.1; //状态转移协方差矩阵
float R = 1; //观测噪声的协方差矩阵

//声明的变量部分
char jieshoushuzi[10];//默认为'\0'
float shuzi[10];//默认为0
float chuandideshu;
float zhengshubufen;
float zhengshuguodu;
float xiaoshubufen;
float xiaoshuguodu;
int p, j, k;
//声明的变量部分
/*
  //PID constants****************************************************************
  float time;
  float setPoint=19;                       //开沟器底部到测量模块的距离
  float error;                             //当前误差
  float cumError;                          //历史误差
  float previous_error;                    //上一时刻误差，用来计算D
  float kp=0;                              //10
  float ki=0;                              //0.05
  float kd=0;                              //200
  int   dt=50;                             //每50毫秒进行一次计算
  float P_param,I_param,D_param,PID;
  //*****************************************************************************
*/

//函数申明
float kaerman(float Z);
float ultrasonicRanging();
void motorAdjust();
void forward();//电机正转
void forback();//电机反转
void forstop();//电机停转
void adjust();
void stable_data();//稳定测距数据
void qingling();
void jieshou();

float distance;
void setup() {

  // InitTimersSafe();           //初始化定时器
  //初始化各管脚
  Serial.begin(9600);
  //Serial3.begin(9600);
  pinMode(shaking_pin, INPUT);
  pinMode(stop_pin, INPUT_PULLUP); //上拉输入
  pinMode(start_pin, INPUT_PULLUP); //上拉输入
  pinMode(setDepth_pin, INPUT_PULLUP); //上拉输入
  pinMode(errorRange_pin, INPUT_PULLUP); //上拉输入
  pinMode(wave_E_1, INPUT);
  pinMode(wave_T_1, OUTPUT);
  pinMode(wave_E_2, INPUT);
  pinMode(wave_T_2, OUTPUT);
  pinMode(wave_E_3, INPUT);
  pinMode(wave_T_3, OUTPUT);
  pinMode(wave_E_4, INPUT);
  pinMode(wave_T_4, OUTPUT);
  //推杆电机设置初始化
  pinMode(L298N_in1, OUTPUT); //对应的四个连接L298N的输出口设置与一个接收PMW信号设置
  pinMode(L298N_in2, OUTPUT);
  pinMode(L298N_in3, OUTPUT);
  pinMode(L298N_in4, OUTPUT);
  pinMode(stop_pin, INPUT_PULLUP);
  //注册中断函数
  attachInterrupt(stop_pin, stop, FALLING);
}

void loop() {
  Serial.println('a');
  ultrasonicRanging();
  Serial.println(distance);
}
//   delay(100);//延时改短点
//   jieshou();//与qingling()一起用
//   setDepth = chuandideshu;
//   //输出显示：其中chuandideshu，zhengshubufen，xiaoshubufen均为float型数据
// // if(zhengshubufen!=0)
// // {
// //   Serial.println("chuandideshu:");
// //   Serial.println(chuandideshu);
// //   Serial.println("zhengshubufen:");
// //   Serial.println(zhengshubufen);
// //   Serial.println("xiaoshubufen:");
// //   Serial.println(xiaoshubufen);
// // }
//   qingling();//注意用了jieshou()就要用qingling()，不然第二轮loop会出问题
//   delay(100);  ///延时改短
//   jieshou();
//   errorRange = chuandideshu;

//     stop_flag = digitalRead(stop_pin);
//     shaking_value = digitalRead(shaking_pin); //震动传感器的D0引脚输出为0时表示为 震动
//     start_flag = digitalRead(start_pin);
//     setDepth_flag = digitalRead(setDepth_pin);
//     errorRange_flag = digitalRead(errorRange_pin);

//   if(stop_flag == 0) //结束开沟，开沟器自动抬起
//    {
//   //   stable_data();
//   //   if((d3>0)&&(d3<500))
//   //   {
//       forward();
//       delay(2000);//随便设置的 驱动2秒
//       forstop();
//     // }
//   }

//   if((start_flag == 0)&&(setDepth_flag == 0)&&(errorRange_flag == 0))
//   {

//     if((first_flag == 1)&&(ultrasonicRanging() > 400)&&(ultrasonicRanging() < 500))//测量数据正常则启动首次开沟
//       {
//         first_flag = 0;
//         stable_data();
//         adjust(d3,setDepth);
//         if((dL>0)&&(dL<100))
//         {
//           forback();
//           delay(dL/V);
//           forstop();
//         }
//       //首次开沟，开沟深度setDepth
//       }

//     stable_data();
//     distanceAdjust = d0-d3;//数据的稳定

//     if((shaking_value == 0)&&(first_flag == 0)&&(d3<500)&&(d3>400)&&(abs(distanceAdjust)>errorRange))//如果震动传感器有反应且大于误差阈值则开始开沟
//     {
//         if(distanceAdjust>0)//需调整的深度大于0，开沟器上升
//         {
//             adjust(d3,setDepth);
//             if((dL>0)&&(dL<30))
//             {
//               forward();
//               delay(dL*1000/V);
//               forstop();
//               delay(200);
//             }
//         }

//         else if(distanceAdjust<0) //调整的深度小于0，开沟器下降
//         {
//             adjust(d3,setDepth);
//             if((dL>0)&&(dL<30))
//             {
//               forback();
//               delay(dL*1000/V);
//               forstop();
//               delay(200);
//             }
//         }

//     }
// }
//}
//   }
//    /*
//     // PID Calculation, PID计算*************************************************
//     error = measuredDistance -setPoint;             //计算误差
//     //P项
//     P_param = kp*error;
//     //I项
//     cumError += error * dt;
//     I_param =  ki*cumError;
//     //D项
//     D_param = kd*((error - previous_error)/dt);
//     //PID
//     PID=P_param + I_param + D_param ;
//     previous_error=error;                  //更新上一时刻误差
//     // Serial Display,在串口监视器显示每个项，有助于调试*****************************
//     Serial.print("Distance: "); Serial.print(measuredDistance); Serial.print(" cm  ");
//     Serial.print("Error: ");    Serial.print(error);    Serial.print(" cm  ");
//     Serial.print("   P: ");     Serial.print(P_param);
//     Serial.print("   D: ");     Serial.print(D_param);
//     Serial.print("   I: ");     Serial.print(I_param);
//     Serial.print("   PID: ");   Serial.println(PID);
//     */
//     //控制电机移动相对距离



//卡尔曼滤波
float kaerman(float Z)
{
  X_ = X; //一维滤波，F=1，同时，下面的H也直接取1
  P_ = P + Q;
  K = P_ / (P_ + R);
  X = X_ + K * (Z - X_);
  P = P_ - K * P_;
  return X;
}

//超声波测距函数
float ultrasonicRanging()
{
  digitalWrite(wave_T_1, HIGH);
  digitalWrite(wave_T_2, HIGH);
  digitalWrite(wave_T_3, HIGH);
  digitalWrite(wave_T_4, HIGH);
  delayMicroseconds(10);
  digitalWrite(wave_T_1, LOW);
  digitalWrite(wave_T_2, LOW);
  digitalWrite(wave_T_3, LOW);
  digitalWrite(wave_T_4, LOW);
  distance = (pulseIn(wave_E_1, HIGH) + pulseIn(wave_E_2, HIGH) + pulseIn(wave_E_3, HIGH) + pulseIn(wave_E_4, HIGH)) / 4;
  Serial.println(distance);
  distance = distance * 340 / 2 / 1000; //单位mm
  delay(100);//延时，防止测到余波且定时输出数据
  distance = kaerman(distance); //滤波
  //Serial.println(distance);//要打开串口绘图器，只能是println，不能用print。
  //return distance;
}


//开沟器上升,推杆电机伸出
void forward()
{
  digitalWrite(L298N_in1, HIGH);
  digitalWrite(L298N_in2, LOW);
  digitalWrite(L298N_in3, HIGH);
  digitalWrite(L298N_in4, LOW);
}

//开沟器下降，推杆电机回收
void forback()
{
  digitalWrite(L298N_in1, LOW); //！！！！！测试正反转，正转？？
  digitalWrite(L298N_in2, HIGH);
  digitalWrite(L298N_in3, LOW);
  digitalWrite(L298N_in4, HIGH);
}

//推杆电机停转
void forstop()
{
  digitalWrite(L298N_in1, LOW);
  digitalWrite(L298N_in2, LOW);
  digitalWrite(L298N_in3, LOW);
  digitalWrite(L298N_in4, LOW);
}

void adjust(float d3, float setDepth)
{
  h1 = d3 - 215 + 65;
  h2 = d0 - setDepth - 215 + 65;
  L1 = sqrt(x * x + y * y - 2 * x * y * (x - (h1)) / m);
  L2 = sqrt(x * x + y * y - 2 * x * y * (x - (h2)) / m);
  dL = abs(L2 - L1);
}

void stable_data()
{
  for (i = 0; i < 10; i++)
  {
    d1 = ultrasonicRanging();
    d2 = ultrasonicRanging();
    if (abs(d1 - d2) < 20)
    {
      break;
    }
  }
  d3 = (d1 + d2) / 2;
}


void jieshou()
{

  if (Serial3.available() > 0)
  {
    for (p = 0; Serial3.available() > 0; p++)
    {
      jieshoushuzi[p] = Serial3.read();
    }
  }

  if (jieshoushuzi[0] != '\0')
  {
    for (p = 0; jieshoushuzi[p] != 0; p++)
    {
      switch (jieshoushuzi[p])
      {
        case '0' : {
            shuzi[p] = 0.0;
          } break;
        case '1' : {
            shuzi[p] = 1.0;
          } break;
        case '2' : {
            shuzi[p] = 2.0;
          } break;
        case '3' : {
            shuzi[p] = 3.0;
          } break;
        case '4' : {
            shuzi[p] = 4.0;
          } break;
        case '5' : {
            shuzi[p] = 5.0;
          } break;
        case '6' : {
            shuzi[p] = 6.0;
          } break;
        case '7' : {
            shuzi[p] = 7.0;
          } break;
        case '8' : {
            shuzi[p] = 8.0;
          } break;
        case '9' : {
            shuzi[p] = 9.0;
          } break;
        case '.' : {
            shuzi[p] = 10.0;
          } break; //用10.0代表'.'
      }
    }


    for (p = 0; shuzi[p] != 10.0; p++)
    {
      zhengshuguodu = zhengshuguodu + shuzi[p];
      zhengshubufen = zhengshuguodu;
      zhengshuguodu = zhengshuguodu * 10;
    }

    for (p = 9; shuzi[p] == 0.0; p--)
    {
      ;
    }

    for (; shuzi[p] != 10.0; p--)
    {
      xiaoshuguodu = xiaoshuguodu + shuzi[p] / 10;
      xiaoshubufen = xiaoshuguodu;
      xiaoshuguodu = xiaoshuguodu / 10;
    }
    i = 0;
    chuandideshu = zhengshubufen + xiaoshubufen;
  }
}

void qingling()
{
  for (p = 0; p < 10; p++)
  {
    jieshoushuzi[p] = '\0';
  }
  for (p = 0; p < 10; i++)
  {
    shuzi[p] = 0;
  }
  zhengshubufen = 0;
  xiaoshubufen = 0;
  chuandideshu = 0;
  zhengshuguodu = 0;
  xiaoshuguodu = 0;
  p = 0;
}

void stop()
{
  forstop();
}