驱动三个步进电机和四个直流电机。蓝牙控制步进电机前进与转向(和速度),直流电机方向和转速
#include <SoftwareSerial.h>
SoftwareSerial blue(10,11); //10接Tx,11接Rx
int SPEED = 30;//前进时速度,越低越快
int SPEEDL = 20;//转向速度
int SPEEDR = 200;//另一个步进电机速度
#define btl 9600 //波特率
int sp = 200;//直流电机速度,越低越慢(0~255)
#define dirin1 4
#define puin2 5
#define dirin3 6
#define puin4 7
#define dirin5 8
#define puin6 9
#define in7 28
#define in8 29 //定义各引脚
#define pwmin9 12
char receivedChar='5';
char ch = '5';
void qd(){//前进
Serial.println("执行qd");
blue.println("执行qd");
digitalWrite(dirin1,HIGH);
digitalWrite(dirin3,HIGH);
// for (int x=0;x<STEPS;x++){
while(1){
digitalWrite(puin2,HIGH);
digitalWrite(puin4,HIGH);
delayMicroseconds(SPEED);
digitalWrite(puin2,LOW);
digitalWrite(puin4,LOW);
delayMicroseconds(SPEED);
if(blue.available()>0){
break;
}
}
delay(20);
Serial.println("跳出qd");
blue.println("跳出qd");
}
void ht(){//后退
Serial.println("执行ht");
blue.println("执行ht");
digitalWrite(dirin1,LOW);
digitalWrite(dirin3,LOW);
// for (int x=0;x<STEPS;x++){
while(1){
digitalWrite(puin2,HIGH);
digitalWrite(puin4,HIGH);
delayMicroseconds(SPEED);
digitalWrite(puin2,LOW);
digitalWrite(puin4,LOW);
delayMicroseconds(SPEED);
if(blue.available()>0){
break;
}
}
Serial.println("跳出ht");
blue.println("跳出ht");
delay(20);
}
void gb(){//停车
Serial.println("执行gb");
blue.println("执行gb");
digitalWrite(dirin1,HIGH);
digitalWrite(dirin3,HIGH);
digitalWrite(puin2,LOW);
digitalWrite(puin2,LOW);
digitalWrite(puin4,LOW);
digitalWrite(puin4,LOW);
delay(20);
}
void zz(){//左转
Serial.println("执行zz");
blue.println("执行zz");
digitalWrite(dirin1,LOW);
digitalWrite(dirin3,HIGH);
// for (int x=0;x<2000;x++){
while(1){
digitalWrite(puin2,HIGH);
digitalWrite(puin4,HIGH);
delayMicroseconds(SPEEDL);
digitalWrite(puin2,LOW);
digitalWrite(puin4,LOW);
delayMicroseconds(SPEEDL);
if(blue.available()>0){
break;
}
}
Serial.println("跳出zz");
blue.println("跳出zz");
delay(20);
}
void yz(){//右转
Serial.println("执行yz");
blue.println("执行yz");
digitalWrite(dirin1,HIGH);
digitalWrite(dirin3,LOW);
// for (int x=0;x<2000;x++){
while(1){
digitalWrite(puin2,HIGH);
digitalWrite(puin4,HIGH);
delayMicroseconds(SPEEDL);
digitalWrite(puin2,LOW);
digitalWrite(puin4,LOW);
delayMicroseconds(SPEEDL);
if(blue.available()>0){
break;
}
}
Serial.println("跳出yz");
blue.println("跳出yz");
delay(20);
}
void ss(){//另一个步进电机转动方向,自己看
Serial.println("执行ss");
blue.println("执行ss");
digitalWrite(dirin5,LOW);
// for (int x=0;x<3000;x++){
while(1){
digitalWrite(puin6,HIGH);
delayMicroseconds(SPEEDR);
digitalWrite(puin6,LOW);
delayMicroseconds(SPEEDR);
if(blue.available()>0){
break;
}
}
Serial.println("跳出ss");
blue.println("跳出ss");
}
void xj(){
Serial.println("执行xj");
blue.println("执行xj");
digitalWrite(dirin5,HIGH);
// for (int x=0;x<3000;x++){
while(1){
digitalWrite(puin6,HIGH);
delayMicroseconds(SPEEDR);
digitalWrite(puin6,LOW);
delayMicroseconds(SPEEDR);
if(blue.available()>0){
break;
}
}
Serial.println("跳出xj");
blue.println("跳出xj");
}
void setup() {
Serial.begin(btl); //设置arduino的串口波特率与蓝牙模块的默认值相同为9600
blue.begin(btl); //设置虚拟输入输出串口波特率与蓝牙模块的默认值相同为9600
pinMode(dirin1, OUTPUT);
pinMode(puin2, OUTPUT);
pinMode(dirin3, OUTPUT);
pinMode(puin4, OUTPUT);
pinMode(dirin5, OUTPUT);
pinMode(puin6, OUTPUT);
pinMode(in7, OUTPUT);
pinMode(in8, OUTPUT);
pinMode(22, OUTPUT);
pinMode(23, OUTPUT);
pinMode(24, OUTPUT);
pinMode(25, OUTPUT);
pinMode(26, OUTPUT);
pinMode(27, OUTPUT);
delay(2000);
}
void loop() {
while(1){
if(blue.available()>0){
blue.flush();
Serial.flush();
receivedChar = blue.read(); //读取蓝牙模块获得的数据
Serial.println(receivedChar);
blue.print("data:");
blue.println(receivedChar);
switch(receivedChar){
case 'q': qd();break;
case 'h': ht();break;
case 'g': gb();break;
case 'z': zz();break;
case 'y': yz();break;
case 's': ss();break;
case 'x': xj();break;
case '2': sp-=10;analogWrite(pwmin9, sp);blue.println(sp);break;
case '3': sp+=10;analogWrite(pwmin9, sp);if(sp>=255){sp=250;};blue.println(sp);break;
case '1': digitalWrite(in7,HIGH);digitalWrite(in8,LOW);digitalWrite(22,HIGH);digitalWrite(23,LOW);digitalWrite(24,HIGH);digitalWrite(25,LOW);digitalWrite(26,HIGH);digitalWrite(27,LOW);analogWrite(pwmin9, sp);analogWrite(13, sp);analogWrite(2, sp);analogWrite(3, sp);break;
case '0': analogWrite(pwmin9, 0);analogWrite(13, 0);analogWrite(2, 0);analogWrite(3, 0);break;
case 'a': SPEED-=10;if(SPEED<0){SPEED=10;} blue.println(SPEED);;break;
case 'b': SPEED+=10;if(SPEED>2000){SPEED=1900;}blue.println(SPEED);break;
case 'c': SPEEDL-=10;if(SPEEDL<0){SPEEDL=10;}blue.println(SPEEDL);break;
case 'd': SPEEDL+=10;if(SPEEDL>2000){SPEEDL=1900;}blue.println(SPEEDL);break;
case 'e': SPEEDR-=10;if(SPEEDR<0){SPEEDR=30;}blue.println(SPEEDR);break;
case 'f': SPEEDR+=10;if(SPEEDR>2000){SPEEDR=1900;}blue.println(SPEEDR);break;
default :blue.println("输错了");break;
}
delay(10);
}
}
}
步进电机接线方式,PU+是脉冲,DIR+是方向,这两个接在板子引脚上,PU-,DIR-,GND,这三个可以合并接在板子上的GND