#include <AccelStepper.h>
  #include <ArduinoJson.h>


//编码器
  int pinA = 3;   //CLK接 pin3
  int pinB = 4;   //DT接pin4
  int counter=0;
  int pinALast;
  int aVal;
  int bianmaqipiancha;

//限位开关
int key = 6;
int k;
int lastk;


//步进电机
  #define dirPin 8
  #define stepPin 7
  #define motorInterfaceType 1
  AccelStepper myStepper(motorInterfaceType, dirPin, stepPin);

    #define dirPin2 10
  #define stepPin2 9
  #define motorInterfaceType 1
  AccelStepper myStepper2(1, dirPin2, stepPin2);


//mpu6050
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 mpu;
int16_t ax, ay, az;
int16_t lastax=0, lastay=0, lastaz=0;
int16_t gx, gy, gz;

//串口输入解析
char str1;
int str;
String str2 = "";

int nameStr;
int nameq;
String dev ;




  
  void setup()
  {
    //编码器
    pinMode(pinA, INPUT);
    pinMode(pinB, INPUT);
    pinALast = digitalRead(pinA);

    //限位开关
     pinMode(key, INPUT_PULLUP);
     lastk=digitalRead(key);



    pinMode(12, OUTPUT);
    digitalWrite(12, LOW);
  
    
//步进电机
    Serial.begin(115200);
   
      myStepper.setMaxSpeed(500); 
    myStepper.setAcceleration(40000000000);
    myStepper.setSpeed(300);

      myStepper2.setMaxSpeed(500);
    myStepper2.setAcceleration(40000000000);
    myStepper2.setSpeed(300);

     Serial.println(1);
     //开机自检归0先关闭
    //zijian();


//mpu6050
 Wire.begin();
Serial.println("Initializing I2C devices...");
mpu.initialize();
Serial.println("Testing device connections...");
Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
    
//串口输入解析
    while (!Serial); //等待串口连接后执行
    // 发送介绍：
    Serial.println("wait for input");

  }

  void loop()
  {
       //编码器
    bianmaqi();
   

  //限位开关
  xianwei();
   
    
  
    // 步进电机
    bujin();

    //mpu6050
   //mpu60();

// 串口json数据解析
    jsonjiexi();

    
    
  }


//编码器
  void bianmaqi(){
     aVal = digitalRead(pinA);
    ///旋钮事件
    if (aVal != pinALast)
    {
      if (digitalRead(pinB) != aVal && abs(aVal-pinALast)<10)
      { // 顺时针旋转
        counter++;
        Serial.print("encoder1:");
        Serial.println(counter+bianmaqipiancha);
        
      }
      else
      { // 逆时针旋转
        counter--;
        Serial.print("encoder1:");
        Serial.println(counter+bianmaqipiancha);
      }
    }
    pinALast = aVal;
  }

  //限位开关
  void xianwei(){
    k = digitalRead(key);
    
    if(k != lastk){
  if(k== HIGH){
     Serial.println("xianwei1-Released");
    }
  if (k == LOW) {
    Serial.println("xianwei1-Pressed");
  }
 
  lastk=k;
  }
  //Serial.println(lastk);
  }

  //步进电机
  void bujin(){
if( digitalRead(key)==HIGH){
     myStepper.setAcceleration(50);
    //digitalWrite(12, HIGH);
     //myStepper.moveTo(myStepper.currentPosition()-30);
     //counter=0;//归0位，修正编码器，陀螺仪数据
   bianmaqipiancha=0;
   delay(2000);
   myStepper.runToNewPosition(myStepper.currentPosition()-40);
      delay(2000);
    }
    else{
      //Serial.println(myStepper.currentPosition());
      //    digitalWrite(12, LOW);

      myStepper.setAcceleration(50);
      myStepper.moveTo(nameStr);
      myStepper2.setAcceleration(50);
      myStepper2.moveTo(nameq);
      
    }

  myStepper.run();
  myStepper2.run();
  while(myStepper.distanceToGo() != 0 || myStepper2.distanceToGo() != 0) {
    myStepper.run();
    myStepper2.run();
    bianmaqi();
   }
  }


//自检测归0
void zijian(){
  int temp=0.01;
  while(digitalRead(key)==HIGH){
    temp--;
    //myStepper.runToNewPosition(temp);
    myStepper.moveTo(temp);
    delay(100);
   myStepper.run();
  }
delay(2000);
  //编码器，传感器修正,
bianmaqipiancha=digitalRead(key);
myStepper.runToNewPosition(myStepper.currentPosition()+40);

  delay(2000);
  //限位开关弹开
  
}

//mpu6050
void mpu60(){
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    //accelgyro.getAcceleration(&ax, &ay, &az);
    //accelgyro.getRotation(&gx, &gy, &gz);
   
//if(ax!=lastax ){
  if(ax!=lastax || ay!=lastay || az!=lastaz){

    Serial.print("a/g:\t");
    Serial.print(ax); Serial.print("\t");
    Serial.print(ay); Serial.print("\t");
    Serial.print(az); Serial.print("\t");
    Serial.print(gx); Serial.print("\t");
    Serial.print(gy); Serial.print("\t");
    Serial.println(gz);
    lastax=ax;lastay=ay;lastaz=az;
    delay(100);
    }
}


//串口输入解析
void jsonjiexi(){
   str2 = "";
    while (Serial.available() > 0) {

        str2 += char(Serial.read()); // read是剪切，而不是复制
        delay(10); // 延时
    }

    if (str2.length() > 0) {

        Serial.print(F("命令行发送内容："));
        Serial.print(str2);

        String json = str2;
        Serial.println(String("") + "JSON大小：" + json.length());
        // 重点2：解析的JSON数据大小
        DynamicJsonDocument doc(json.length() * 2); //解析的JSON数据大小

        // 重点3：反序列化数据
        deserializeJson(doc, json);

        // 重点4：获取解析后的数据信息
         nameStr = doc["c"].as < int > ();
          nameq = doc["d"].as<int>();
        String dev = doc["s"].as < String > ();

        // 通过串口监视器输出解析后的数据信息
        Serial.print("c = ");
        Serial.println(nameStr);
         Serial.print("d = ");
        Serial.println(nameq);
        Serial.print("s = ");
        Serial.println(dev);
    }
}