#include<Servo.h>

#define SERVO_SPEED 40                  //定义舵机转动快慢的时间

#define SERVO_SPEED1 1

#define SERVO_SPEED_WalkForward_low 10   //设定前进的慢速动作

#define SERVO_SPEED_WalkForward_high   40 //设定前进的快速动作

#define SERVO_SPEED_WalkBack_low 10      //设定慢速的后退动作

#define SERVO_SPEED_WalkBack_high   5     //设定快速后退的动作

#define SERVO_SPEED_Right_low 6          //设定慢速右转动作时间

#define SERVO_SPEED_Right_high   4        //设定快速右转动作时间

Servo myServo[3];               //定义3个舵机

int f = 30;                     //定义舵机每个状态间转动的次数，以此来确定每个舵机每次转动的角度

int servo_port[3] = {3,4,7};                      //定义舵机引脚

int servo_num = sizeof(servo_port) / sizeof(servo_port[0]);     //定义舵机数量

float value_init[8] = {1650,1300,2400}; //定义舵机初始角度

int Judge=0;

int GetIntData=0;

const int xdirPin = 44;   // 方向引脚

const int xstepPin = 45; // 步进引脚

const int ydirPin = 32;   // 方向引脚

const int ystepPin = 33; // 步进引脚

const int zdirPin = 42;   // 方向引脚

const int zstepPin = 43; // 步进引脚

const int adirPin = 30;   // 方向引脚

const int astepPin = 31; // 步进引脚

const int STEPS_PER_REV =200;

#define ECHOPIN A0

#define TRIGPIN A1

void setup() {

//超声波传感器设置

  pinMode(TRIGPIN, OUTPUT);

  pinMode(ECHOPIN, INPUT);

//底盘设置

  pinMode(8,OUTPUT);

  pinMode(xstepPin,OUTPUT);

  pinMode(xdirPin,OUTPUT);

  pinMode(ystepPin,OUTPUT);

  pinMode(ydirPin,OUTPUT);

  pinMode(zstepPin,OUTPUT);

  pinMode(zdirPin,OUTPUT);

  pinMode(astepPin,OUTPUT);

  pinMode(adirPin,OUTPUT);

//执行器舵机设置

  for(int i=0;i<servo_num;i++)

  {

       ServoGo(i,value_init[i]);   //使能舵机并赋予初始角度

    }

}

void loop() {

  //超声波测距

  //以下代码为计算超声波测距传感器进行测距并计算，返回距离的单位为cm

  //Serial.begin(9600);

  digitalWrite(TRIGPIN,LOW);   

  delayMicroseconds(2);

  digitalWrite(TRIGPIN,HIGH);

  delayMicroseconds(10);

  digitalWrite(TRIGPIN,LOW);

  float distance=pulseIn(ECHOPIN,HIGH);

  distance=distance/58;

  //Serial.println(distance);

  delay(200);

  if(distance>10)     //10cm以内无障碍

  {

  //底盘的运动控制

  digitalWrite(xdirPin,LOW);

  digitalWrite(ydirPin,HIGH);

  digitalWrite(zdirPin,LOW);

  digitalWrite(adirPin,HIGH);

  for(int x = 0; x < STEPS_PER_REV; x++)

  {

       digitalWrite(xstepPin,HIGH);

       digitalWrite(ystepPin,HIGH);

       digitalWrite(zstepPin,HIGH);

       digitalWrite(astepPin,HIGH);

       delayMicroseconds(400);

       digitalWrite(xstepPin,LOW);

       digitalWrite(ystepPin,LOW);

       digitalWrite(zstepPin,LOW);  

       digitalWrite(astepPin,LOW);

       delayMicroseconds(400);

       }

  }

else

  {

    //停止，避障

    digitalWrite(xdirPin,LOW);

    digitalWrite(ydirPin,LOW);

    digitalWrite(zdirPin,LOW);

    digitalWrite(adirPin,LOW);

  }

  //舵机的运动控制

    servo_move(950,1300,1500, SERVO_SPEED_WalkForward_low);

    delay(5000);

    //               950,                              1300,                                   1500,                                 SERVO_SPEED_WalkForward_low

    //表示D3引脚连接的舵机要转到的角度     表示D4引脚连接的舵机要转到的角度        表示D7引脚连接的舵机要转到的角度                      表示舵机转动时的速度

    servo_move(2400,1800,2500, SERVO_SPEED_WalkForward_low);

    delay(5000);

    //               2400,                              1800,                                   2500,                                 SERVO_SPEED_WalkForward_low

    //表示D3引脚连接的舵机要转到的角度     表示D4引脚连接的舵机要转到的角度        表示D7引脚连接的舵机要转到的角度                      表示舵机转动时的速度   

}

  //自定义函数

  void ServoActionReset()

  {

      servo_move(1500,1000,2400, SERVO_SPEED); //初始化

  }

void ServoStart(int which)//使能舵机

{

  if(!myServo[which].attached()) myServo[which].attach(servo_port[which]);

  pinMode(servo_port[which], OUTPUT);

}

void ServoStop(int which)//释放舵机

{

  myServo[which].detach();

  digitalWrite(servo_port[which],LOW);

}

void ServoGo(int which , int where)//打开并给舵机写入相关角度

{

  if(where!=200)

  {

    if(where==201) ServoStop(which);

    else

    {

  ServoStart(which);

  myServo[which].write(where);

    }

  }

}

void servo_move(float value0, float value1, float value2, float t)

//   alue0, value1, value2, value3这四个参数时要写的舵机角度，float t这个参数是代表舵机运动的时间

{

  float value_arguments[] = {value0, value1, value2};

  float value_delta[servo_num];

  for(int i=0;i<servo_num;i++)

  {

    value_delta[i] = (value_arguments[i] - value_init[i]) / f;   //f=30，舵机初始角度定义为(1650,1300,2400)

  }

  for(int i=0;i<f;i++)

  {

    for(int k=0;k<servo_num;k++)

    {

       value_init[k] = value_delta[k] == 0 ? value_arguments[k] : value_init[k] + value_delta[k];

    }

    for(int j=0;j<servo_num;j++)

    {

        ServoGo(j,value_init[j]);

    }

    delay(t);

  }

}