代码在这里~只用了VEX的那个直流电机、两个限位开关和两个FC-04的超声波……
#include <Servo.h>
#define MOTOR_PIN 7 // define the PWM range for the VEX speed controll
#define MIN_POS 1000 // 0
#define MAX_POS 2000 // 255
#define CENTER_POS 1500 // 127
Servo myservo;
const int TrigPin = 2;
const int EchoPin = 3;
const int SecondTPin = 4;
const int SecondEPin = 5;
const int bumperup = 8;
const int bumperdown = 9;
bool motor;
int uval,dval;
float cm1,cm2;
void setup()
{
Serial.begin(9600);
pinMode(TrigPin, OUTPUT);
pinMode(EchoPin, INPUT);
pinMode(SecondTPin, OUTPUT);
pinMode(SecondEPin, INPUT);
pinMode(7, OUTPUT);
pinMode(8, OUTPUT);
pinMode(bumperup,INPUT);
digitalWrite(bumperup,HIGH);
pinMode(bumperdown,INPUT);
digitalWrite(bumperdown,HIGH);
myservo.attach(MOTOR_PIN);
motor = false;
}
void loop()
{
myservo.attach(MOTOR_PIN);
digitalWrite(TrigPin, LOW); //低高低电平发一个短时间脉冲去TrigPin
delayMicroseconds(2);
digitalWrite(TrigPin, HIGH);
delayMicroseconds(10);
digitalWrite(TrigPin, LOW);
cm1 = pulseIn(EchoPin, HIGH) / 58.0; //将回波时间换算成cm
cm1 = (int(cm1 * 100.0)) / 100.0; //保留两位小数
delay(100);
digitalWrite(SecondTPin, LOW); //低高低电平发一个短时间脉冲去TrigPin
delayMicroseconds(2);
digitalWrite(SecondTPin, HIGH);
delayMicroseconds(10);
digitalWrite(SecondTPin, LOW);
cm2 = pulseIn(SecondEPin, HIGH) / 58.0; //将回波时间换算成cm
cm2 = (int(cm2 * 100.0)) / 100.0; //保留两位小数
delay(100);
uval = digitalRead(bumperup);
Serial.print(cm1);
Serial.print("cm1");
Serial.println();
Serial.print(cm2);
Serial.print("cm2");
Serial.println();
uval = digitalRead(bumperup);
dval = digitalRead(bumperdown);
delay(10);
if(0<cm1<=30)
{
uval = digitalRead(bumperup);
while(uval == HIGH && motor == false)
{
myservo.writeMicroseconds(MIN_POS);
uval = digitalRead(bumperup);
}
myservo.writeMicroseconds(CENTER_POS);
motor = true;
}
delay(10);
dval = digitalRead(bumperdown);
myservo.writeMicroseconds(CENTER_POS);
if(cm1>=30 && cm2>=30)
{
while(dval == HIGH && motor == true)
{
myservo.writeMicroseconds(MAX_POS);
dval = digitalRead(bumperdown);
}
myservo.writeMicroseconds(CENTER_POS);
motor = false;
}
}
然而症状是……无论超声波的值是虾米，道闸只会一直向上和向下……求救QAQ

pc大神又卖萌？