|
|
今天我对数据和程序进行了一些小修改,还测试了单个马达的工作状况。以后还会对程序进行简化。
请看测试视频
#include <Servo.h>
Servo myservo;
const int echo = 12;
const int trig = 13;
const int sw = 3;
const int th = 4;
void setup()
{
Serial.begin(9600);
pinMode(echo,INPUT);
pinMode(trig,OUTPUT);
int sensorValue = digitalRead(echo);
int swValue = digitalRead(sw);
int thValue = digitalRead(th);
myservo.attach(9);
}
void loop()
{
long duration, cm ,hold ,power;
hold = pulseIn(sw,HIGH);
if (hold>1500)
{
digitalWrite(trig,LOW);
delayMicroseconds(2);
digitalWrite(trig,HIGH);
delayMicroseconds(12);
digitalWrite(trig,LOW);
duration = pulseIn(echo,HIGH);
// convert the time into a distance
cm = microsecondsToCentimeters(duration);
Serial.print(cm);
Serial.print("cm");
Serial.print(duration);
Serial.print("-");
Serial.print(hold);
Serial.println();
duration = map(duration, 0, 1000, 135, 45);
myservo.write(duration);
}
else {
power = pulseIn(th,HIGH);
Serial.print(power);
power = map (power, 1000, 2000, 135, 45);
myservo.write(power);
Serial.print("th=");
Serial.print(power);
Serial.println();
delay(15);
}
}
long microsecondsToCentimeters(long microseconds)
{
return microseconds / 29 / 2;
} |
|