时隔一年半重回论坛,想到原来在这里遇到问题时大神们热情的相助,觉得应该回报社会一下~
这是信息产品设计的大作业,利用两个步进电机和同步带,带动画笔在竖直平面完成作图
经过测试,基本实现功能,但有些抖动,以后可能也没机会完善了,发到论坛上来纪念一下吧
PS:一年半以前的作业,最近整理资料,发现了这个,自己留着也没用,放上来供大家完善
没获奖的兔斯基
视频地址:
http://v.youku.com/v_show/id_XNTU1ODc5MDQw.html
以及获了奖的兔斯基
视频地址:
http://v.youku.com/v_show/id_XNTU1ODI4ODky.html
使用了的库函数:
http://www.geek-workshop.com/for ... thread&tid=3266
我使用了了大连好人写的库函数,链接在上面,解决了两个步进电机分别在同一时间走不同步的问题
如果想立即找到解决方案,在下面的链接的答案中找,我已放过。
http://www.geek-workshop.com/forum.php?mod=viewthread&tid=4319&page=2#pid70901
这是以前写的一个像素游戏--雷电复古版的前传~,用红外遥控器控制,16*16LED点阵实现
有意戳进:
http://www.geek-workshop.com/thread-4335-1-1.html
竖直画图装置代码贴的如下: - //垂直画图装置
- //by:梁YJ 刘DY
- //点坐标移动版本
- //变量设定
- //步进电机部分
- #include <EasyStepper.h>
- #define STEP1_PIN 2
- #define STEP2_PIN 4
- #define DIR1_PIN 6
- #define DIR2_PIN 8
- #define EN1_PIN 12
- #define EN2_PIN 13//两个EN端悬空
- #define DIR1_PIN_INVERTED true
- #define DIR2_PIN_INVERTED true
- #define EN1_PIN_INVERTED true
- #define EN2_PIN_INVERTED true
- EasyStepper stepper1(STEP1_PIN, DIR1_PIN, EN1_PIN, DIR1_PIN_INVERTED, EN1_PIN_INVERTED);
- EasyStepper stepper2(STEP2_PIN, DIR2_PIN, EN2_PIN, DIR2_PIN_INVERTED, EN2_PIN_INVERTED);
- int times=0;//帮助电机动,每次开始需要初始化
- //坐标相关
- #define D 130 //两个电机间的距离(cm)
- #define base_x D/2 //定位点的坐标,为等边三角形
- #define base_y 0.866*D
- float init_a,init_b,init_x,init_y;//起始点
- float dest_a,dest_b,dest_x,dest_y;//目标点
- float da,db;//a,b的增减值
- int step_a=0,step_b=0;//步进电机走的步数
- float speed_a,speed_b;//步进电机走的速度
- #define step_per_cm 2000/50//每cm需要走多少步
- #define spd 600//步进电机最大速度
- //坐标的单位是cm,允许小数
- //得奖图片
- int dpaint_x[128]=
- { 3027,3713,2755,2660,2767,3015,3630,3725,3878,4091,
- 3796,3630,3666,3807,4221,4742,5238,5451,5640,5770,
- 5889,5841,5640,5451,5699,6019,6279,6456,6302,5865,
- 6078,6042,5900,5652,5285,5002,4824,4813,5439,5037,
- 5215,5652,5711,5640,5451,5167,5037,4765,4008,4327,
- 4623,5274,4931,4801,4907,4990,4919,3535,3394,3535,
- 3666,3547,2353,2459,2578,2471,2365,1076,934,1064,
- 1159,1064,402,568,709,532,627,414,709,579,461,639,
- 733,1206,1005,946,780,780,1171,1171,970,946,733,958,
- 1182,1313,1514,1514,1514,1514,1265,1584,1750,1750,
- 2034,1845,1703,1845,1608,1277,2081,1667,1573,1407,
- 1253,1655,1797,2081,2199,2909,2542,2542,2223,2814,
- 2660,2873,2128,2968};//x坐标值
-
- int dpaint_y[128]=
- { 4193,3385,2506,2340,2245,2316,2910,3088,3266,3385,
- 3160,2827,2447,2079,1805,1663,1758,1853,1615,1556,
- 1604,1782,2007,1877,2090,1853,1722,1841,2043,2269,
- 2637,3041,3361,3599,3373,3183,3207,3361,3718,3742,
- 3599,3575,3777,4050,4098,4074,3932,4205,2174,2304,
- 2387,2768,380,736,1117,736,380,950,1283,1675,1283,
- 926,1687,2209,1710,1330,1675,1128,1437,1900,1449,
- 1128,570,238,487,451,582,653,665,926,808,1033,273,
- 273,285,439,843,439,439,843,535,867,1033,855,1057,
- 297,439,214,713,523,606,463,226,333,344,582,428,606,
- 665,772,772,677,915,998,1021,760,950,1033,404,404,
- 202,630,962,962,748,1057,653,653}; //y坐标值
-
- int dpaint_bi[128]=
- { 2,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
- 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,2,0,0,0,0,0,1,2,1,
- 2,1,2,0,0,0,1,2,0,0,0,1,2,0,0,0,1,2,0,0,0,1,2,0,1,
- 2,1,2,0,0,0,1,2,0,0,1,2,0,0,1,2,0,1,2,1,2,0,0,0,0,
- 1,2,0,0,0,0,0,0,1,2,1,2,0,0,1,2,0,1,2,1,2,0,0,0,0,
- 1,2,1 };//控制抬笔和落笔
- //2代表落笔,0代表示维持现状,1代表抬笔
-
- //没得奖图片
- int npaint_x[115]=
- { 4103,4257,4801,4813,3914,3938,4801,5250,5049,5096,
- 5605,5593,5073,3961,3914,4056,3997,4115,4044,3027,
- 3204,3654,3985,3630,3382,3299,3488,3725,4008,3938,
- 4056,4221,4257,4446,4481,4635,4836,4765,5215,5664,
- 5877,5877,5427,5333,4505,4446,5380,5345,4623,4576,
- 4883,5120,5711,5238,5794,5309,6314,5865,6492,5640,
- 3571,2850,3441,497,627,473,603,544,485,662,828,828,
- 1088,1123,1242,1242,698,1206,650,1395,1395,2081,
- 2081,1395,1726,1726,2235,2353,2199,2400,2412,2412,
- 2684,2519,2613,2968,2779,2530,2578,2779,2188,3015,
- 2601,2471,2223,2590,2802,3003,3441,3441,3417,3417,
- 3512,3512,3429};//x坐标值
-
- int npaint_y[115]=
- { 1413,1271,1651,1924,1912,1615,1627,1508,1687,1912,
- 1924,1687,1699,1936,2364,1948,2364,1960,2387,4157,
- 3504,2922,3100,2910,2589,2138,1651,1366,1223,535,
- 404,463,1117,1128,594,392,499,1212,1318,1604,2079,
- 2601,2981,2114,2126,3433,3385,3243,3266,3409,3433,
- 4122, 226,701,986,1390,226,630,1045,1627,2839,3504,
- 3171,535,653,760,891,998,1318,808,713,594,594,784,
- 796,1342,903,903,1330,998,713,713,998,1010,523,1330,
- 582,701,903,831,535,998,535,784,630,630,891,962,
- 713,891,1069,1069,962,1235,1318,1069,1271,1318,582,
- 1057,1140,1247,1247,1140,1140}; //x坐标值
-
- int npaint_bi[115]=
- { 2,0,0,0,0,0,1,2,0,0,0,0,1,2,1,2,1,2,1,2,0,1,2,0,
- 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
- 0,0,0,1, 2,0,0,1,2,0,0,1,2,0,1,2,1,2,1,2,1,2,0,
- 0,0,0,1,2,0,0,1,2,0,0,0,1,2,1,2,1,2,1,2,1,2,1,2,
- 0,0,1,2,1,2,1,2,0,1,2,0,1,2,1,2,0,0,0,1};
- //控制抬笔和落笔
- //2代表落笔,0代表示维持现状,1代表抬笔
- //自定义函数
- void xyMove(float dest_x,float dest_y);
- //舵机部分
- #include <Servo.h>
- Servo dj;
- int angle_ding=30;//舵机顶出时角度,抬笔
- int angle_shou=180;//舵机收回时角度,落笔
- //前一组信号接收用
- int c,d;
- void setup()
- {
- Serial.begin(9600);
- //步进电机部分初始化
- stepper1.debugMode(false);
- stepper1.startup();
- stepper2.debugMode(false);
- stepper2.startup();
- init_x=base_x;
- init_y=base_y;
-
- //舵机部分初始化
- dj.attach(9);
- dj.write(angle_ding);
-
- //前一组信号接收端口,有三种状态
- //悬空,10号HIGH,和10,11号口均为HIGH
- pinMode(10,INPUT);
- pinMode(11,INPUT);
- }
- void loop()
- {
- c=digitalRead(10);
- d=digitalRead(11);
- Serial.println(c);
- Serial.println(d);
- if(c==HIGH&&d==HIGH)//判断为得奖
- {
- Serial.println("de");
- for(int i=0;i<128;i++)
- {
- //将int型数组的值变为float,这样做可以减少内存的使用量
- float x=dpaint_x[i]*1.0/100.0;
- float y=dpaint_y[i]*1.0/100.0;
-
- //调用函数,并通过增加数值的改变图像零点的位置
- xyMove(x+33,y+38);
-
- if(dpaint_bi[i]==1)//1代表抬笔
- {
- for(int degree=180;degree>=30;degree--)
- {
- dj.write(degree);
- delay(5);
- }
- }
- else if(dpaint_bi[i]==2)//2代表落笔
- {
- for(int degree=30;degree<=180;degree++)
- {
- dj.write(degree);
- delay(5);
- }
- }
- delay(200);//延时,减少笔的抖动
- }
-
- xyMove(base_x,base_y);//回到定位点
- }
- else if(c==HIGH&&d==LOW)//判断为没得奖
- {
- Serial.println("no");
- for(int i=0;i<115;i++)
- {
- //将int型数组的值变为float,这样做可以减少内存的使用量
- float x=npaint_x[i]*1.0/100.0;
- float y=npaint_y[i]*1.0/100.0;
-
- //调用函数,并通过增加数值的改变图像零点的位置
- xyMove(x+33,y+37);
-
- //舵机角度改变
- if(npaint_bi[i]==1)//1代表抬笔
- {
- for(int degree=180;degree>=30;degree--)
- {
- dj.write(degree);
- delay(5);
- }
- }
- else if(npaint_bi[i]==2)//2代表落笔
- {
- for(int degree=30;degree<=180;degree++)
- {
- dj.write(degree);
- delay(5);
- }
- }
- delay(200);//延时,减少笔的抖动
- }
-
- xyMove(base_x,base_y);//回到定位点
-
- }
- else//判断为没有信号输入
- {
- delay(20);//延时等待下一次
- }
-
-
- }
- //函数作用为从现在的点移动到目标点(dest_x,dest_y)
- void xyMove(float dest_x,float dest_y)
- {
- //a,b代表与步进a与步进b相连的同步带的长度
- init_a=float(sqrt(pow(init_x,2)+pow(init_y,2)));
- init_b=float(sqrt(pow(D-init_x,2)+pow(init_y,2)));
- dest_a=float(sqrt(pow(dest_x,2)+pow(dest_y,2)));
- dest_b=float(sqrt(pow(D-dest_x,2)+pow(dest_y,2)));
-
- da=dest_a-init_a;
- db=dest_b-init_b;
- //通过计算目的长度-初始长度得到步进电机移动的步数
- step_a=fabs(da*step_per_cm)+0.5;
- step_b=fabs(db*step_per_cm)+0.5;
-
- //通过步数的比例得到运动速度的比例
- //使得两个步进可以同时开始同时停止
- if(step_a>=step_b)
- {
- speed_a=spd;
- speed_b=(step_b*1.0/step_a)*spd;
- }
- else
- {
- speed_b=spd;
- speed_a=(step_a*1.0/step_b)*spd;
- }
-
- //步进电机运转方向设定
- if(da<0)
- { step_a=step_a*(-1);}
- if(db<0)
- { step_b=step_b*(-1);}
-
- //步进电机转速.步数设定
- stepper1.rotate(speed_a,-step_a);
- stepper2.rotate(speed_b,step_b);
-
- times=0;//需要初始化,以保证连续运动
- while (times < 1)
- {
- //判断设定的步数是否走完
- if (stepper1.isDone()==false)
- {
- stepper1.run();//走一步
- }
- //判断设定的步数是否走完
- if (stepper2.isDone()==false)
- {
- stepper2.run();
- }
- //都走完了,跳出死循环
- if(stepper1.isDone()&&stepper2.isDone())
- {
- times=1;
- }
- }
-
- init_x=dest_x;
- init_y=dest_y;
- //移动到目的坐标后,将目的坐标存在起始点中,等待下一次移动
-
-
- }
复制代码
|