硬件:
Arduino UNO
步进电机
9g舵机
* 需用外接电源 5-7V
代码:
#include <Stepper.h>
#include <Servo.h>
#define A1 8
#define B1 9
#define C1 10
#define D1 11
#define JDQ 12
#define TrigPin 4 //发送的信号
#define EchoPin 5 //回来的信号
int val=10;
int pulsewidth = 0; //高电平时间
float Value_cm; //距离
Servo myservo1;
Servo myservo2;
int pos1 =0;
int pos2 =0;
void setup() {
Serial.begin(9600); //串口
pinMode(A1,OUTPUT);pinMode(B1,OUTPUT);pinMode(C1,OUTPUT);pinMode(D1,OUTPUT);
pinMode(JDQ,OUTPUT);
pinMode(TrigPin, OUTPUT); //舵机信号输出
pinMode(EchoPin, INPUT); //舵机信号输出
myservo1.attach(2);
myservo1.write(0);
myservo2.attach(3);
myservo2.write(90);
}
void loop() {
for(int bs=0;bs<100;bs++){
digitalWrite(TrigPin, LOW);delayMicroseconds(2);
digitalWrite(TrigPin, HIGH);delayMicroseconds(10);
digitalWrite(TrigPin, LOW);
Value_cm = float( pulseIn(EchoPin, HIGH) * 17 )/1000; //计算距离
xsz();
if(Value_cm < 30){
stop();
digitalWrite(JDQ,HIGH);
for(int pos1=0;pos1<=90;pos1 +=1){myservo1.write(pos1);delay(20);}
for(int pos2=90;pos2>=0;pos2 -=1){myservo2.write(pos2);delay(20);}
for(int pos1=90;pos1>=0;pos1 -=1){myservo1.write(pos1);delay(20);}
for(int pos2=0;pos2<=90;pos2 +=1){myservo2.write(pos2);delay(20);}
}else{
digitalWrite(JDQ,LOW);
myservo1.write(0);
myservo2.write(90);
}
}
delay(1000);
for(int bs=0;bs<100;bs++){
digitalWrite(TrigPin, LOW);delayMicroseconds(2);
digitalWrite(TrigPin, HIGH);delayMicroseconds(10);
digitalWrite(TrigPin, LOW);
Value_cm = float( pulseIn(EchoPin, HIGH) * 17 )/1000;
fsz();
if(Value_cm < 30){
stop();
digitalWrite(JDQ,HIGH);
for(int pos1=0;pos1<=90;pos1 +=1){myservo1.write(pos1);delay(20);}
for(int pos2=90;pos2>=0;pos2 -=1){myservo2.write(pos2);delay(20);}
for(int pos1=90;pos1>=0;pos1 -=1){myservo1.write(pos1);delay(20);}
for(int pos2=0;pos2<=90;pos2 +=1){myservo2.write(pos2);delay(20);}
}else{
digitalWrite(JDQ,LOW);
myservo1.write(0);
myservo2.write(90);
}
}
delay(1000);
}
void xsz(){
Phase_A1(); //设置A相位
delay(val); //改变延时可改变旋转速度
Phase_B1(); //设置B相位
delay(val);
Phase_C1(); //设置C相位
delay(val);
Phase_D1(); //设置D相位
delay(val);
}
void fsz(){
Phase_A2(); //设置A相位
delay(val); //改变延时可改变旋转速度
Phase_B2(); //设置B相位
delay(val);
Phase_C2(); //设置C相位
delay(val);
Phase_D2(); //设置D相位
delay(val);
}
void stop(){
Phase_A3(); //设置A相位
delay(val); //改变延时可改变旋转速度
Phase_B3(); //设置B相位
delay(val);
Phase_C3(); //设置C相位
delay(val);
Phase_D3(); //设置D相位
delay(val);
}
void Phase_A1(){
digitalWrite(A1,HIGH); digitalWrite(B1,LOW);digitalWrite(C1,LOW);digitalWrite(D1,LOW);
}
void Phase_B1(){
digitalWrite(A1,LOW); digitalWrite(B1,HIGH); digitalWrite(C1,LOW);digitalWrite(D1,LOW);
}
void Phase_C1(){
digitalWrite(A1,LOW);digitalWrite(B1,LOW);digitalWrite(C1,HIGH);digitalWrite(D1,LOW);
}
void Phase_D1(){
digitalWrite(A1,LOW); digitalWrite(B1,LOW);digitalWrite(C1,LOW);digitalWrite(D1,HIGH);
}
void Phase_A2(){
digitalWrite(A1,LOW); digitalWrite(B1,LOW);digitalWrite(C1,LOW);digitalWrite(D1,HIGH);
}
void Phase_B2(){
digitalWrite(A1,LOW); digitalWrite(B1,LOW); digitalWrite(C1,HIGH);digitalWrite(D1,LOW);
}
void Phase_C2(){
digitalWrite(A1,LOW);digitalWrite(B1,HIGH);digitalWrite(C1,LOW);digitalWrite(D1,LOW);
}
void Phase_D2(){
digitalWrite(A1,HIGH); digitalWrite(B1,LOW);digitalWrite(C1,LOW);digitalWrite(D1,LOW);
}
void Phase_A3(){
digitalWrite(A1,LOW); digitalWrite(B1,LOW);digitalWrite(C1,LOW);digitalWrite(D1,LOW);
}
void Phase_B3(){
digitalWrite(A1,LOW); digitalWrite(B1,LOW); digitalWrite(C1,LOW);digitalWrite(D1,LOW);
}
void Phase_C3(){
digitalWrite(A1,LOW);digitalWrite(B1,LOW);digitalWrite(C1,LOW);digitalWrite(D1,LOW);
}
void Phase_D3(){
digitalWrite(A1,LOW); digitalWrite(B1,LOW);digitalWrite(C1,LOW);digitalWrite(D1,LOW);
}
视频:
|