首页 › Arduino › Arduino DIY感应机器人

福州布衣 发表于 2019-11-22 17:30:21

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 JDQ12
#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);
}
视频:
https://pan.baidu.com/s/1rFTBsKG0HAw4r8BGFpnUDw

查看完整版本: Arduino DIY感应机器人