一、使用步骤
1.引入库
代码如下(示例):
#include <Servo.h>//舵机库
//舵机定义
#include <SoftwareSerial.h>
//这是软串口通讯arduino和esp8266-Mode连接的库函数
#include <AccelStepper.h>
#include <MultiStepper.h>
//本示例程序使用AccelStepper库
Servo myServo1,myServo2;
2.读入数据
代码如下(示例):
#include <Servo.h>//舵机库
//舵机定义
#include <SoftwareSerial.h>
//这是软串口通讯arduino和esp8266-Mode连接的库函数
#include <AccelStepper.h>
#include <MultiStepper.h>
//本示例程序使用AccelStepper库
Servo myServo1,myServo2;
SoftwareSerial youSerial(50, 51); //RX=50,TX=51 需分配Mega 和 Mega 2560 上并非所有引脚都支持更改中断,因此只有以下引脚可用于 RX:10、11、12、13、14、15、50、51、52、53、A8 (62)、A9 (63)、A10 (64)、A11 (65)、A12 (66)、A13 (67)、A14 (68)、A15 (69)。
// 定义电机控制用常量
const int enablePin1 = 8; // 使能控制引脚
const int enablePin2 = 9; // 使能控制引脚
const int enablePin3 = 10; // 使能控制引脚
const int enablePin4 = 11; // 使能控制引脚
const int xdirPin = 5; // x方向控制引脚
const int xstepPin = 2; // x步进控制引脚
const int ydirPin = 6; // y方向控制引脚
const int ystepPin = 3; // y步进控制引脚
const int zdirPin = 7; // z方向控制引脚
const int zstepPin = 4; // z步进控制引脚
const int adirPin = 13; // a方向控制引脚
const int astepPin = 12; // a步进控制引脚
const int qled1 = 22; //红外控制引脚
const int qled2 = 24; //红外控制引脚
const int qled3 = 26; //红外控制引脚
const int qled4 = 28; //红外控制引脚
const int qled5 = 30; //红外控制引脚
const int qled6 = 32; //红外控制引脚
int n=1;
int b = 100;
int a,q,w,e,r,t;
int data[32];
int x,y,z,j,u,p,m=0;
const int z1led1 = 23; //红外控制引脚
const int z1led2 = 25; //红外控制引脚
const int z1led3= 27; //红外控制引脚
const int z1led4 = 29; //红外控制引脚
const int z1led5 = 31; //红外控制引脚
const int z1led6 = 33; //红外控制引脚
const int z2led1 = 38; //红外控制引脚
const int z2led2 = 40; //红外控制引脚
const int z2led3= 42; //红外控制引脚
const int z2led4 = 44; //红外控制引脚
const int z2led5 = 46; //红外控制引脚
const int z2led6 = 48; //红外控制引脚
const int hled1 = 39; //红外控制引脚
const int hled2 = 41; //红外控制引脚
const int hled3 = 43; //红外控制引脚
const int hled4 = 45; //红外控制引脚
const int hled5 = 47; //红外控制引脚
const int hled6 = 49; //红外控制引脚
const int moveSteps = 50; //测试电机运行使用的运行步数
int trac_x();
int trac_y();
int trac_z();
int trac_p();
void go();
void houtui();
void left();
void right();
int n_o1=1;
int n_o2=1;
int n_o3=1;
int n_o4=1;
AccelStepper stepper1(n_o1,xstepPin,xdirPin,enablePin1);//建立步进电机对象1
AccelStepper stepper2(n_o2,ystepPin,ydirPin,enablePin2);//建立步进电机对象2
AccelStepper stepper3(n_o3,zstepPin,zdirPin,enablePin3);//建立步进电机对象3
AccelStepper stepper4(n_o4,astepPin,adirPin,enablePin4);//建立步进电机对象4
// 定义电机控制用常量
// 电机每圈步数
const int vauel_0=6521;//调整左右处于中间
const int vauel_1=5210;//一格的步数5220,5454
const int vauel_2=5454;
const int vauel_3=3000;
const int vauel_4=27406;
const int vauel=200;
const int vauel_5=20000;
void setup() {
// Arduino控制A4988步进和方向的引脚为输出模式
Serial.begin(9600);
pinMode(xstepPin,OUTPUT);
pinMode(xdirPin,OUTPUT);
pinMode(enablePin1,OUTPUT);
pinMode(enablePin2,OUTPUT);
pinMode(enablePin3,OUTPUT);
pinMode(enablePin4,OUTPUT);
pinMode(ystepPin,OUTPUT);
pinMode(ydirPin,OUTPUT);
pinMode(zstepPin,OUTPUT);
pinMode(zdirPin,OUTPUT);
pinMode(astepPin,OUTPUT);
pinMode(adirPin,OUTPUT);
pinMode(qled1,INPUT);
pinMode(qled2,INPUT);
pinMode(qled3,INPUT);
pinMode(qled4,INPUT);
pinMode(qled5,INPUT);
pinMode(qled6,INPUT);
pinMode(z1led1,INPUT);
pinMode(z1led2,INPUT);
pinMode(z1led3,INPUT);
pinMode(z1led4,INPUT);
pinMode(z1led5,INPUT);
pinMode(z1led6,INPUT);
pinMode(z2led1,INPUT);
pinMode(z2led2,INPUT);
pinMode(z2led3,INPUT);
pinMode(z2led4,INPUT);
pinMode(z2led5,INPUT);
pinMode(z2led6,INPUT);
pinMode(hled1,INPUT);
pinMode(hled2,INPUT);
pinMode(hled3,INPUT);
pinMode(hled4,INPUT);
pinMode(hled5,INPUT);
pinMode(hled6,INPUT);
myServo1.attach(17);
myServo2.attach(18);//定义舵机引脚
}
void Servo12act1()//这个是开
{
myServo1.write(50);
myServo2.write(60);
}
void Servo12act2()//这个是旋转装配
{
myServo1.write(50);
myServo2.write(60);
}
void Servo12act3()//这个是关
{
myServo1.write(50);
myServo2.write(60);
}
int yun()
{
switch(n)
{
case 1:n++;e=vauel_5;
youSerial.listen();
while(1){
if (mySerial.available() > 0 )
{
char ml_a = youSerial.read();
}
if (ml_a==11)
{break;}
}
go();delay(1);go();delay(1);break;//上 刹车然后向左
case 2:n++; q=vauel_1*2;left();delay(1);break;//左
case 3:n++; q=vauel_1*2;left();delay(1);break;//左
case 4:n++; q=vauel_1*2;left();delay(1);break;//左
case 5:n++; q=vauel_1*2;left();delay(1);break;//左
case 6:n++; q=vauel_1*2;go();delay(1);break;//上
case 7:n++; q=vauel_1*2;go();delay(1);break;//上
case 8:n++; q=vauel_1*2;go();delay(1);break;//上
case 9:n++; q=vauel_1*2;
youSerial.listen();
while(1){
if (mySerial.available() > 0 )
{
char ml_a = youSerial.read();
}
if (ml_a==t2)
{break;}
}
Servo12act1();
while(1){
if (mySerial.available() > 0 )
{
char ml_a = youSerial.read();
}
if (ml_a==t2)
{break;}
}
Servo12act2();
while(1){
if (mySerial.available() > 0 )
{
char ml_a = youSerial.read();
}
if (ml_a==t2)
{break;}
}
Servo12act3();
right();delay(1);break;//下
// case 2:n=3; q=vauel_1*2;right();delay(1);break;
// case 3:n=4;e=vauel_1;go();delay(1);break;
// case 4:n=5;w=vauel_1*6;left();delay(1);break;
}
// switch(n)
// {
// case 1:n=2;e=vauel_4;go();delay(1);break;
// case 2:n=3; q=vauel_1*2;right();delay(1);break;
// case 3:n=4;e=vauel_1;go();delay(1);break;
// case 4:n=5;w=vauel_1*6;left();delay(1);break;
// }
return n;
}
void right(){
digitalWrite(enablePin2,LOW);
digitalWrite(enablePin3,LOW);
digitalWrite(enablePin1,HIGH);
digitalWrite(enablePin4,HIGH);
digitalWrite(ydirPin,LOW);
digitalWrite(zdirPin,LOW);
for(int u=0; u < q; u++) {
trac_z();
if(z!=20&&z!=30&&z!=3&&z!=2&&z!=50&&z!=5&&z!=60&&z!=40&&z!=6&&z!=4){
digitalWrite(zstepPin,HIGH);
digitalWrite(ystepPin,HIGH);
delayMicroseconds(100);
digitalWrite(zstepPin,LOW);
digitalWrite(ystepPin,LOW);
delayMicroseconds(100);
}
if(z==30||z==20)
{
int vauel = 200;
digitalWrite(ydirPin,LOW);
for(int a=0; a < (vauel*1); a++) {
digitalWrite(ystepPin,HIGH);
delayMicroseconds(100);
digitalWrite(ystepPin,LOW);
delayMicroseconds(100);
}
u=u+1;
}
if(z==3||z==2)
{
int vauel = 200;
digitalWrite(zdirPin,LOW);
for(int j=0; j < (vauel*1); j++) {
digitalWrite(zstepPin,HIGH);
delayMicroseconds(100);
digitalWrite(zstepPin,LOW);
delayMicroseconds(100);
}
u=u+1;
}
if(z==60||z==40)
{
int vauel = 200;
digitalWrite(zdirPin,LOW);
for(int j=0; j < (vauel*2); j++) {
digitalWrite(zstepPin,HIGH);
delayMicroseconds(100);
digitalWrite(zstepPin,LOW);
delayMicroseconds(100);
}
u=u+1;
}
if(z==6||z==4)
{
int vauel = 200;
digitalWrite(zdirPin,LOW);
for(int j=0; j < (vauel*2); j++) {
digitalWrite(zstepPin,HIGH);
delayMicroseconds(100);
digitalWrite(zstepPin,LOW);
delayMicroseconds(100);
}
u=u+1;
}
}
}
void left(){
digitalWrite(enablePin2,LOW);
digitalWrite(enablePin3,LOW);
digitalWrite(enablePin1,LOW);
digitalWrite(enablePin4,LOW);
digitalWrite(ydirPin,HIGH);
digitalWrite(zdirPin,HIGH);
for(int u=0; u < w; u++) {
trac_x();
// if(x==0||x==1||x==10||x==66||x==65||x==63||x==61){
if(x!=20&&x!=30&&x!=3&&x!=2&&x!=50&&x!=5&&x!=60&&x!=40&&x!=6&&x!=4){
digitalWrite(zstepPin,HIGH);
digitalWrite(ystepPin,HIGH);
delayMicroseconds(100);
digitalWrite(zstepPin,LOW);
digitalWrite(ystepPin,LOW);
delayMicroseconds(100);
}
if(x==30||x==20)
{
int vauel = 200;
digitalWrite(zdirPin,HIGH);
for(int a=0; a <vauel*1; a++) {
digitalWrite(zstepPin,HIGH);
delayMicroseconds(100);
digitalWrite(zstepPin,LOW);
delayMicroseconds(100);
}
u=u+1;
}
if(x==3||x==2)
{
int vauel = 200;
digitalWrite(ydirPin,HIGH);
for(int j=0; j < (vauel*1); j++) {
digitalWrite(ystepPin,HIGH);
delayMicroseconds(100);
digitalWrite(ystepPin,LOW);
delayMicroseconds(100);
}
u=u+1;
}
if(x==60||x==40)
{
int vauel = 200;
digitalWrite(ydirPin,HIGH);
for(int j=0; j < (vauel*2); j++) {
digitalWrite(ystepPin,HIGH);
delayMicroseconds(100);
digitalWrite(ystepPin,LOW);
delayMicroseconds(100);
}
u=u+1;
}
if(x==6||x==4)
{
int vauel = 200;
digitalWrite(ydirPin,HIGH);
for(int j=0; j < (vauel*2); j++) {
digitalWrite(ystepPin,HIGH);
delayMicroseconds(100);
digitalWrite(ystepPin,LOW);
delayMicroseconds(100);
}
u=u+1;
}
}
}
//void go(){
//digitalWrite(xdirPin,HIGH);
//digitalWrite(adirPin,HIGH);
//digitalWrite(ydirPin,LOW);
//digitalWrite(zdirPin,LOW);
// for(int u=0; u <e;u++) {
// trac_y();
// if(y!=20&&y!=30&&y!=3&&y!=2&&y!=50&&y!=5&&y!=60&&y!=40&&y!=6&&y!=4){
// digitalWrite(xstepPin,HIGH);
// digitalWrite(astepPin,HIGH);
// delayMicroseconds(100);
// digitalWrite(xstepPin,LOW);
// digitalWrite(astepPin,LOW);
// delayMicroseconds(100);
// }
// }
//}
void go(){
digitalWrite(enablePin1,LOW);
digitalWrite(enablePin4,LOW);
digitalWrite(enablePin2,HIGH);
digitalWrite(enablePin3,HIGH);
digitalWrite(xdirPin,HIGH);
digitalWrite(adirPin,HIGH);
for(int u=0; u <e;u++) {
trac_y();
if(y!=20&&y!=30&&y!=3&&y!=2&&y!=50&&y!=5&&y!=60&&y!=40&&y!=6&&y!=4){
digitalWrite(xstepPin,HIGH);
digitalWrite(astepPin,HIGH);
delayMicroseconds(100);
digitalWrite(xstepPin,LOW);
digitalWrite(astepPin,LOW);
delayMicroseconds(100);
}
if(y==30||y==20)
{
int vauel = 200;
digitalWrite(xdirPin,HIGH);
for(int a=0; a < (vauel*1); a++) {
digitalWrite(xstepPin,HIGH);
delayMicroseconds(100);
digitalWrite(xstepPin,LOW);
delayMicroseconds(100);
}
u=u+1;
}
if(y==3||y==2)
{
int vauel = 200;
digitalWrite(adirPin,HIGH);
for(int j=0; j < (vauel*1); j++) {
digitalWrite(astepPin,HIGH);
delayMicroseconds(100);
digitalWrite(astepPin,LOW);
delayMicroseconds(100);
}
u=u+1;
}
if(y==60||y==40)
{
int vauel = 200;
digitalWrite(adirPin,HIGH);
for(int j=0; j < (vauel*2); j++) {
digitalWrite(astepPin,HIGH);
delayMicroseconds(100);
digitalWrite(astepPin,LOW);
delayMicroseconds(100);
}
u=u+1;
}
if(y==6||y==4)
{
int vauel = 200;
digitalWrite(adirPin,HIGH);
for(int j=0; j < (vauel*2); j++) {
digitalWrite(astepPin,HIGH);
delayMicroseconds(100);
digitalWrite(astepPin,LOW);
delayMicroseconds(100);
}
u=u+1;
}
}
}
void houtui(){
digitalWrite(enablePin1,LOW);
digitalWrite(enablePin4,LOW);
digitalWrite(enablePin2,HIGH);
digitalWrite(enablePin3,HIGH);
digitalWrite(xdirPin,LOW);
digitalWrite(adirPin,LOW);
for(int u=0; u < r; u++) {
trac_p();
// if(p==0||p==1||p==10||p==66||p==65||p==63||p==61){
if(p!=20||p!=30||p!=3||p!=2||p!=50||p!=5||p!=60||p!=40||p!=6||p!=4){
digitalWrite(xstepPin,LOW);
digitalWrite(astepPin,LOW);
delayMicroseconds(100);
digitalWrite(xstepPin,HIGH);
digitalWrite(astepPin,HIGH);
delayMicroseconds(100);
}
if(p==30||p==20)
{
int vauel = 200;
digitalWrite(zdirPin,LOW);
for(int a=0; a < (vauel*1); a++) {
digitalWrite(zstepPin,HIGH);
delayMicroseconds(100);
digitalWrite(zstepPin,LOW);
delayMicroseconds(100);
}
u=u+1;
}
if(p==3||p==2)
{
int vauel = 200;
digitalWrite(xdirPin,LOW);
for(int j=0; j < (vauel*1); j++) {
digitalWrite(xstepPin,HIGH);
delayMicroseconds(100);
digitalWrite(xstepPin,LOW);
delayMicroseconds(100);
}
u=u+1;
}
if(p==60||p==40)
{
int vauel = 200;
digitalWrite(xdirPin,LOW);
for(int j=0; j < (vauel*2); j++) {
digitalWrite(xstepPin,HIGH);
delayMicroseconds(100);
digitalWrite(xstepPin,LOW);
delayMicroseconds(100);
}
u=u+1;
}
if(p==6||4)
{
int vauel = 200;
digitalWrite(xdirPin,LOW);
for(int j=0; j < (vauel*2); j++) {
digitalWrite(xstepPin,HIGH);
delayMicroseconds(100);
digitalWrite(xstepPin,LOW);
delayMicroseconds(100);
}
u=u+1;
}
}
}
void stop(){
digitalWrite(enablePin1,HIGH);
}
//int yun()
//{
// switch(n)
// {
case 1:n=2;e=vauel_5;go();break;
case 1:n=2;e=vauel_0+vauel_1*4;go();delay(1);break;
// case 1:n=2;e=vauel_4;go();delay(1);break;
// case 2:n=3; q=vauel_1*2;right();delay(1);break;
// case 3:n=4;e=vauel_1;go();delay(1);break;
// case 4:n=5;w=vauel_1*6;left();delay(1);break;
// }
// }
// else if(z==0||z==30||z==3||z==1||z==10){
// switch(n)
// {
// case 2:n=3; q=vauel_1*2;right();break;
// }
// }
// else if(x==0||x==30||x==3||x==1||x==10){
// switch(n)
// {
// case 4:n=5;w=vauel_1*6;left();break;
// }
// }
// return n;
//}
int trac_x()
{
data[0] = digitalRead(22);
data[1] = digitalRead(24);
data[2] = digitalRead(26);
data[3] = digitalRead(28);
data[4] = digitalRead(30);
data[5] = digitalRead(32);
x=data[0]*40+data[1]*20+data[2]*10+data[3]*1+data[4]*2+data[5]*4;
return x;
}
int trac_y(){
data[8] = digitalRead(23);
data[9] = digitalRead(25);
data[10] = digitalRead(27);
data[11] = digitalRead(29);
data[12] = digitalRead(31);
data[13] = digitalRead(33);
y=data[8]*40+data[9]*20+data[10]*10+data[11]*1+data[12]*2+data[13]*4;
return y;
}
int trac_z()
{
data[16] = digitalRead(38);
data[17] = digitalRead(40);
data[18] = digitalRead(42);
data[19] = digitalRead(44);
data[20] = digitalRead(46);
data[21] = digitalRead(48);
z=data[16]*40+data[17]*20+data[18]*10+data[19]+data[20]*2+data[21]*4;
return z;
}
int trac_p(){
data[24] = digitalRead(39);
data[25] = digitalRead(41);
data[26] = digitalRead(43);
data[27] = digitalRead(45);
data[28] = digitalRead(47);
data[29] = digitalRead(49);
p=data[24]*40+data[25]*20+data[26]*10+data[27]*1+data[28]*2+data[29]*4;
return p;
}
void loop(){
// e=vauel_1;
// go();
//delayMicroseconds(30000);
// left();
//delayMicroseconds(30000);
// stop();
//delayMicroseconds(30000);
// right();
//delayMicroseconds(30000);
yun();
Serial.print("n=");
Serial.println(n);
}
总结
仅仅分享一下代码,希望能对你有所帮助。后期再将细节说明清楚