#include

#include //引用超声波库文件

Ultrasonic Ultrasonic1(ULTRASONIC_ADDR_1);//将Ultrasonic实例化

Motor MotorLeft(MOTOR0_PINA, MOTOR0_PINB);

Motor MotorRight(MOTOR1_PINA, MOTOR1_PINB);

void setup()

{

Serial.begin(115200); //串口初始化

Serial.begin(9600);

Ultrasonic1.begin(); //超声波初始化

MotorLeft.begin(); //电机MotorLeft初始化

MotorRight.begin(); //电机MotorLeft初始化

}

void loop()

{

uint16_t Dis; //定义变量

int dist;

Dis = Ultrasonic1.getDistance(); //获取超声波测得的距离

Serial.println(Dis);

dist = Dis/200;

switch(dist){

case 0:

fr(1);

back(100);

break;

case 1:

right(30);

slow(10);

left(30);

break;

case 2:

slow(1);

break;

default:

front();

break;

}

}

void front(){

int i;

MotorLeft.setSpeed(100);

MotorRight.setSpeed(-100);

delay(100);

}

void slow(int a){

int i;

for(i=0;i

MotorLeft.setSpeed(50);

MotorRight.setSpeed(-50);

}

delay(100);

}

void shut_down(int a){ //停几秒

int i;

for(i=0;i

MotorLeft.Brake(); //电机MotorLeft刹车

MotorRight.Brake(); //电机MotorRight刹车

delay(100);

}

}

void right(int a){

int i;

for(i=0;i

MotorLeft.setSpeed(100);

MotorRight.setSpeed(100);

}

delay(100);

}

void left(int a){

int i;

for(i=0;i

MotorLeft.setSpeed(100);

MotorRight.setSpeed(100);

}

delay(100);

}

void back(int a){

int i;

for(i=0;i

MotorLeft.setSpeed(-100); //设置电机MotorLeft速度为-100

MotorRight.setSpeed(100); //设置电机MotorRight速度为-100

}

delay(100);

}

void fr(int a){

int i;

for(i=0;i

MotorLeft.setSpeed(FREE); //设置电机MotorLeft为释放状态，即速度为0

MotorRight.setSpeed(FREE); //设置电机MotorRight为释放状态，即速度为0

delay(200);

}

}