Servo A,B,C,D,E;

#include <Arduino.h>

#include <Wire.h>

int pos=0 ;

int La, Ra;

#define ECHOPIN A0;

#define TRIGPIN A1;

int RA, LB,RC;

#include <Wire.h>

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 =700;

void advance();

void cease();

void righe();

void left();

void scram();

void setup() {

// put your setup code here, to run once:

pinMode(xstepPin, OUTPUT);

pinMode(xdirPin, OUTPUT);

pinMode(ystepPin, OUTPUT);

pinMode(ydirPin, OUTPUT);

pinMode(zstepPin, OUTPUT);

pinMode(zdirPin, OUTPUT);

pinMode(astepPin, OUTPUT);

pinMode(adirPin, OUTPUT);

Serial.begin(9600);

delayMicroseconds(10000);

A.attach(2); // Servo 对象连接在 9 号引脚

delay(10);B.attach(3); // Servo 对象连接在 9 号引脚

delay(10);

C.attach(3); // Servo 对象连接在 9 号引脚

delay(10);

D.attach(3); // Servo 对象连接在 9 号引脚

delay(10);

E.attach(3); // Servo 对象连接在 9 号引脚

delay(10);

Serial.begin(9600);

}

void loop() {

cm=ceju(aa);

RA = analogRead(A2);

LB = analogRead(A4);

RC = analogRead(A6);

//cease();

//delay(100);

int shang;

shang=RA-LB;

Serial.print("zuo");

Serial.print(RA);

Serial.print("CM");

Serial.print("

");

Serial.print("you");

Serial.print(LB);

Serial.print("DISAN");

Serial.print(shang);

Serial.print("\n");

if (RA >= 500 && LB >= 400) {

//Serial.print("停止\n");

cease(); //调用停止函数

} else if (shang>=(-200)&&shang<=200) {

// Serial.print("前进\n");

advance(); //调用前进函数

} else if (shang>=200 ) {

// Serial.print("you 转\n");

//scram();

righe(); //右转} else if (shang < (-200)) {

//Serial.print("zuo 转\n");

//scram();

left(); //左转

}

else if(cm>=0&&cm<50)

{

jiaqu();

fanhui();

fuyuan();

}

}

void jiaqu()

{

for(int pos=130;pos>=0;pos--)

{

A.write(pos);

}

delay(2000);

for(int pos=30;pos<=120;os++)

{

B.write(pos);

}

delay(2000);

for(int pos=50;pos<=120;pos++)

{

C.write(pos);

}

delay(2000);

for(int pos=30pos<=30;pos++)

{

D.write(pos);

}

delay(2000);for(int pos=85;pos>=60;pos--)

{

E.write(pos);

}

delay(2000);

}

void huilai(){

for(int pos=170;pos>=40;pos--)

{

A.write(pos);

}

delay(2000);

for(int pos=30;pos=30;)

{

B.write(pos);

}

delay(2000);

for(int pos=140;pos>=45;pos--)

{

C.write(pos);

}

delay(2000);

for(int pos=150;pos>=30;pos--)

{

D.write(pos);

}

delay(2000);

for(int pos=85;pos>=60;pos--)

{

E.write(pos);

}

delay(2000);

}

void fuyuan(){

for(int pos=0;pos<=130;pos++)

{

A.write(pos);

}delay(2000);

for(int pos=120;pos>=30;pos--)

{

B.write(pos);

}

delay(2000);

for(int pos=120;pos>=50;pos--)

{

C.write(pos);

}

delay(2000);

for(int pos=30;pos>=30;pos--)

{

D.write(pos);

}

delay(2000);

for(int pos=60;pos<=85;pos--)

{

E.write(pos);

}

delay(2000);

}

viod ceju(){

digitalWrite(TRIGPIN, LOW); //给 Trigpin 发送一个低电平（初始

化）

delayMicroseconds(5000);

//等待 2 微妙

digitalWrite(TRIGPIN, HIGH);//给 Trigpin 发送一个高电平

delayMicroseconds(5000);

//等待 10 微妙

digitalWrite(TRIGPIN, LOW); //给 Trigpin 发送一个低电平

float distance = pulseIn(ECHOPIN, HIGH);//存储回波等待时间,

float aa = (distance * 17 )/1000; //把回波时间换算成 cm

return aa;

Serial.print("ECHOPIN =");

Serial.print(distance);//串口输出等待时间的原始数据

Serial.print(" | | Distance = ");

Serial.print(aa);//串口输出距离换算成 cm 的结果

Serial.println("cm");

}

void advance() { //前进函数digitalWrite(xdirPin, HIGH);

digitalWrite(ydirPin, LOW);

digitalWrite(zdirPin, HIGH);

digitalWrite(adirPin, LOW);

for (int x = 0; x <500; 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);

}

}

void cease() { //停止函数

digitalWrite(xstepPin, HIGH);

digitalWrite(ystepPin, LOW);

digitalWrite(zstepPin, HIGH);

digitalWrite(astepPin, LOW);

delayMicroseconds(1000);

}

void righe() { //右转函数

digitalWrite(xdirPin, HIGH);

digitalWrite(ydirPin, HIGH);

digitalWrite(zdirPin, HIGH);

digitalWrite(adirPin, HIGH);

for (int x = 0; x <600; x++) {

digitalWrite(xstepPin, HIGH);

digitalWrite(ystepPin, HIGH);

digitalWrite(zstepPin, HIGH);

digitalWrite(astepPin, HIGH);

delayMicroseconds(800);

digitalWrite(xstepPin, LOW);

digitalWrite(ystepPin, LOW);

digitalWrite(zstepPin, LOW);

digitalWrite(astepPin, LOW);

delayMicroseconds(800);

}

}void left() { //左转函数

digitalWrite(xdirPin, LOW);

digitalWrite(ydirPin, LOW);

digitalWrite(zdirPin, LOW);

digitalWrite(adirPin, LOW);

for (int x = 0; x < 600; 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);

}

}

void scram()

{

digitalWrite(xstepPin, HIGH);

digitalWrite(ystepPin, HIGH);

digitalWrite(zstepPin, HIGH);

digitalWrite(astepPin, HIGH);

delayMicroseconds(800);

digitalWrite(xstepPin, LOW);

digitalWrite(ystepPin, LOW);

digitalWrite(zstepPin, LOW);

digitalWrite(astepPin, LOW);

}