arduinouno组装无人机_新人求助，，arduino uno的板子

arduino uno的板子，电机驱动板，还有一个舵机驱动板。小车由两个步进电机驱动，前置一个hcsr04超声波传感器。电机驱动板电源电压24v，uno和舵机驱动板由单独电源供电，5到6v。想实现的功能是，当前方障碍物距离大于40cm时，前行。小于或等于40cm时，停止，然后舵机转动，复位，再退后。目前只控制一个舵机，以后可能要同时控制6个，程序如下：

发现启动后，小车前方障碍物距离大于40cm时小车长时间后退，

#include

Servo myservo1;

Servo myservo;

int Left_motor_P=3;

int Left_motor_N=6;

int Right_motor_P=5;

int Right_motor_N=11;

int Ultrasonic_Echo = A0;

int Ultrasonic_Trig =A1;

int Front_Distance = 0;

void setup()

{

Serial.begin(9600);

pinMode(Left_motor_P,OUTPUT);

pinMode(Left_motor_N,OUTPUT);

pinMode(Right_motor_P,OUTPUT);

pinMode(Right_motor_N,OUTPUT);

pinMode(Ultrasonic_Echo, INPUT);

pinMode(Ultrasonic_Trig, OUTPUT);

myservo1.attach(2);

myservo.attach(12);

}

void go(int a)

{

digitalWrite(Right_motor_P,HIGH);

digitalWrite(Right_motor_N,LOW);

analogWrite(Right_motor_P,80);

analogWrite(Right_motor_N,0);

digitalWrite(Left_motor_P,LOW);

digitalWrite(Left_motor_N,HIGH);

analogWrite(Left_motor_P,0);

analogWrite(Left_motor_N,90);

delay(a * 100);

}

void Stop (int f)

{

digitalWrite(Right_motor_P,LOW);

digitalWrite(Right_motor_N,LOW);

digitalWrite(Left_motor_P,LOW);

digitalWrite(Left_motor_N,LOW);

delay(f * 100);

}

void Reverse(int g)

{

digitalWrite(Right_motor_P,LOW);

digitalWrite(Right_motor_N,HIGH);

analogWrite(Right_motor_P,0);

analogWrite(Right_motor_N,80);

digitalWrite(Left_motor_P,HIGH);

digitalWrite(Left_motor_N,LOW);

analogWrite(Left_motor_P,90);

analogWrite(Left_motor_N,0);

delay(g * 100);

}

void ask_pin_F()

{

myservo.write(90);

digitalWrite(Ultrasonic_Trig, LOW);

delayMicroseconds(2);

digitalWrite(Ultrasonic_Trig, HIGH);

delayMicroseconds(10);

digitalWrite(Ultrasonic_Trig, LOW);

float Fdistance = pulseIn(Ultrasonic_Echo, HIGH);

Fdistance= Fdistance/59;

Front_Distance = Fdistance;

}

void loop()

{

ask_pin_F();

if(Front_Distance>=40)

{

go(1);

}

else

{

Stop(1);

myservo1.attach(45);

myservo1.attach(90);

Reverse(1);

}

}