Arduino DIY感应机器人

福州布衣

硬件:
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);
}
视频: