设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 嵌入式/单片机论坛 › Arduino › 我的机械手臂设想（附哟代码~！）大家进来讨论讨论~
返回列表 发新帖
查看: 5032|回复: 2
打印 上一主题 下一主题
收起左侧

我的机械手臂设想（附哟代码~！）大家进来讨论讨论~

[复制链接]
跳转到指定楼层
楼主
ID:112317 发表于 2016-4-9 19:24 | 只看该作者 |只看大图 回帖奖励 |倒序浏览 |阅读模式
这两篇文章，是自己很久前写的了，09年，呵呵 那时候刚接触arduino，对电子、机械什么的，也是完全的外行，突然想起这两篇文章，去机器人天空翻出来看了看 哈哈 还是挺有意思的，就转过来啦
最近我在弄一个小程序，就是用机械手臂来写字和画画
本来是打算做六足的，但是在制作的时候一不小心电压接高了！
结果，只见伺服控制板火花四溅，一缕青烟袅袅升起！哎
200在一瞬间烧了！！
六足也就只能搁一边了，慢慢攒钱买个新的吧
不过另一个想法就在我头脑里出现了
用一只腿来实现写字
经过一个星期的思考和n个晚上的时间，我终于写出了核心代码
先是在DEV上模拟实现，然后再移植到arduino里
原理就是，把每个字（目前只考虑英文与数字，简单点）用坐标表示，存在eeprom里，
于是每个字就可以用一根根直线画出来
核心程序就是实时计算3个伺服的角度，如此让机械手臂画出一条条直线
经过几个测试，我写的程序能在arduino上运行，但精确度不是很高
才85%~97%，希望能达到99%
因为伺服控制器被烧了，所以无法进行完全测试，而只是输出每时刻三个伺服的角度
以后应该还有很多要测试吧，但是高三了，可能没那么多时间，会弄的很慢
公布出源码，与大家讨论讨论，代码写的没什么水平，见笑
不知道我这个思路对不对，如果大家有更好的方法，尽情提出来~！
源码已经过测试，可以直接编译运行
dev版，可以自己修改printf来看实现过程
/*************************Hand_v1_0_dev_test**********************************************
  1.     #include
  2.     #include
  3.     #include
  4.     int l2=60,l3=70,li;// 用mm做单位
  5.     void Line(int xa,int ya,int xb,int yb)
  6.     {
  7.       //用int代替float,扩大100000倍，xa3位，cos5位，alpha为整，oa4位
  8.       int cosin(int alpha);
  9.       int arccos(int cos_alpha);
  10.       int alpha=0,alphai=0,beta=0,sita=0,fai=0;
  11.       int cos_alpha,cos_alphai,cos_fai,sin_fai,cos_sita,sin_sita;
  12.       int oa,oa_2,ob,ob_2,xx_yy;
  13.      
  14.      
  15.       //save the caculate result for reuse
  16.       oa_2=xa*xa+ya*ya;
  17.       oa=(int)sqrt(oa_2);
  18.       ob_2=xb*xb+yb*yb;
  19.       ob=(int)sqrt(ob_2);
  20.       xx_yy=xa*xb+ya*yb;
  21.       //printf("oa:%d,ob:%d,xx_yy:%d",oa,ob,xx_yy);
  22.       //printf("Line 1");
  23.       cos_alpha=100000*abs(xx_yy)/(oa*ob);
  24.       cos_alphai=0;
  25.       alpha=arccos(cos_alpha);
  26.       alphai=0;
  27.       //printf("cos_fai=(int)(100000*(%d-%d)/(sqrt(%d+%d-2*%d)*%d));",
  28.                 //oa_2,xx_yy,oa_2,ob_2,xx_yy,oa);
  29.       cos_fai=(int)(100000*(oa_2-xx_yy)/(sqrt(oa_2+ob_2-2*xx_yy)*oa));
  30.       //printf("cos_fai=100000*(%d-%d)/(sqrt(%d+%d-2*%d)*%d))",
  31.                 //oa_2,xx_yy,oa_2,ob_2,xx_yy,oa);
  32.       sin_fai=cosin(arccos(cos_fai)-90);
  33.       //printf("cos_alpha:%d",cos_alpha);
  34.       //printf("alpha:%d,alphai:%d",arccos(cos_alpha),alphai);
  35.       //printf("fai:=%d,cos_fai:%d,sin_fai:%d",arccos(cos_fai),cos_fai,sin_fai);
  36.       //getchar();
  37.       while(abs(alphai-alpha)>1)
  38.       {
  39.         //printf("alpha:%f,alphai:%f",alpha,alphai);
  40.         //printf("oa:%f,oa_2:%f,ob:%f,ob_2:%f,",oa,oa_2,ob,ob_2);
  41.         if(alphai>alpha)
  42.           alphai-=1;
  43.         else alphai+=1;
  44.         //printf("alpha:%d,alphai:%d",alpha,alphai);
  45.         //printf("li=(%d*%d)/(%d*%d+%d*%d);",
  46.             //sin_fai,oa,cosin(alphai-90),cos_fai,cosin(alphai),sin_fai);
  47.         li=(sin_fai*oa)/cosin(alphai+arccos(cos_fai)-90);
  48.         printf("li=%d",li);
  49.         //printf("cos_sita=100000*(%d*%d+%d*%d-%d*%d)/(2*%d*%d);",
  50.                  //l2,l2,l3,l3,li,li,l2,l3);
  51.         cos_sita=100000*(l2*l2+l3*l3-li*li)/(2*l2*l3);
  52.         sin_sita=cosin(arccos(cos_sita)-90);
  53.         //printf("cos_sita:%d,sin_sita:%d",cos_sita,sin_sita);
  54.         int t1=10000000,t2=0;
  55.         //printf("Line 2");
  56.         //printf("sita:%d",arccos(cos_sita));
  57.         while(abs(t1-t2)>100000)
  58.         {
  59.           t1=l2*cosin(beta-90);
  60.           t2=l3*cosin(beta+arccos(cos_sita)-90);
  61.           //printf("cosin(beta-90):%d,cosin(beta+sita):%d",
  62.                    //cosin(beta-90),cosin(beta+arccos(cos_sita)-90));
  63.           //printf("t1:%d,t2:%d",t1,t2);
  64.           if(t1>t2)
  65.             beta-=1;
  66.           else    beta+=1;
  67.         }
  68.         //printf("Line 3");
  69.         //printf("alpha:%d,beta:%d,sita:%d,",
  70.                    //alpha,beta,arccos(cos_sita));
  71.         //getchar();
  72.         //servo(.....);
  73.         //delay(100);
  74.         printf("alpha:%d,beta:%d,sita:%d,",
  75.                    alpha,beta,arccos(cos_sita));
  76.       }
  77.       getchar();
  78.     }
  79.      
  80.     void InitHand()
  81.     {
  82.       int beta=45,sita=45;
  83.       printf("beta:%d,sita:%d,",beta,sita);
  84.     }
  85.     int cosin(int t)
  86.     {
  87.           return (int)(100000*cos(t*3.1415926/180));
  88.     }
  89.     int arccos(int t)
  90.     {
  91.     int left=0,mid,right=180,i=0;
  92.     int cos_mid,delta;
  93.     if(t==100000) return 0;
  94.     while(i<9)
  95.     {
  96.        mid=(left+right)/2;
  97.        cos_mid=(int)(100000*cos(mid*3.1415926/180));
  98.        if(mid>90) delta=(181-mid)*29;
  99.        else delta=mid*29;
  100.        //printf("cos:%f,fabs:%f,delta:%f",cos(mid*3.1415926/180),
  101.        //          fabs(t-cos(mid*3.1415926/180)),delta);
  102.        if(fabs(t-cos_mid)<=delta)
  103.           break;
  104.        else if(t<cos_mid)
  105.          left=mid;
  106.        else right=mid;
  107.        i++;
  108.        }
  109.        return mid;
  110.     }
  111.     main()
  112.     {
  113.            int i,j,n;
  114.            int xa,xb,ya,yb;
  115.            n=20;
  116.            InitHand();
  117.            while(1){
  118.            system("cls");
  119.            printf("Input x1,y1,x2,y2(mm)");
  120.            scanf("%d,%d,%d,%d",&xa,&ya,&xb,&yb);
  121.            printf("InitHand() done");
  122.            getchar();
  123.            //printf("xa:%3.1f,ya:%3.1f,xb:%3.1f,yb:%3.1f,",1,2.8,7,6);
  124.            Line(xa,ya,xb,yb);
  125.             getchar();
  126.             }
  127.             }
复制代码
arduino版
/****************************Hand_v1_0*************************************************   
  1.     long int l2=60,l3=70,li;
  2.     void Line(long int xa,long int ya,long int xb,long int yb)
  3.     {
  4.       long int cosin(long int alpha);
  5.       long int arccos(long int cos_alpha);
  6.       long int alpha=0,alphai=0,beta=0,sita=0,fai=0;
  7.       long int cos_alpha,cos_alphai,cos_fai,sin_fai,cos_sita,sin_sita;
  8.       long int oa,oa_2,ob,ob_2,xx_yy;
  9.      
  10.      
  11.       //save the caculate result for reuse
  12.       oa_2=xa*xa+ya*ya;
  13.       oa=(long int)sqrt(oa_2);
  14.       ob_2=xb*xb+yb*yb;
  15.       ob=(long int)sqrt(ob_2);
  16.       xx_yy=xa*xb+ya*yb;
  17.       cos_alpha=100000*abs(xx_yy)/(oa*ob);
  18.       cos_alphai=0;
  19.       alpha=arccos(cos_alpha);
  20.       alphai=0;
  21.       cos_fai=(long int)(100000*(oa_2-xx_yy)/(sqrt(oa_2+ob_2-2*xx_yy)*oa));
  22.       sin_fai=cosin(arccos(cos_fai)-90);
  23.       while(abs(alphai-alpha)>1)
  24.       {
  25.         if(alphai>alpha)
  26.           alphai-=1;
  27.         else alphai+=1;
  28.         li=(sin_fai*oa)/cosin(alphai+arccos(cos_fai)-90);
  29.         printf("li=%d",li);
  30.         cos_sita=100000*(l2*l2+l3*l3-li*li)/(2*l2*l3);
  31.         sin_sita=cosin(arccos(cos_sita)-90);
  32.         long int t1=10000000,t2=0;
  33.         while(abs(t1-t2)>100000)
  34.         {
  35.           t1=l2*cosin(beta-90);
  36.           t2=l3*cosin(beta+arccos(cos_sita)-90);
  37.           if(t1>t2)
  38.             beta-=1;
  39.           else    beta+=1;
  40.         }
  41.         Serial.print("alpha:");
  42.         Serial.println(alpha);
  43.         Serial.print("beta:");
  44.         Serial.println(beta);
  45.         Serial.print("sita:");
  46.         Serial.println(arccos(cos_sita));
  47.         Serial.print("li:");
  48.         Serial.println(li);
  49.       }
  50.       Serial.println("Line() done!");
  51.     }
  52.      
  53.     void InitHand()
  54.     {
  55.       long int beta=45,sita=45;
  56.       Serial.print("beta:");
  57.       Serial.println(beta);
  58.       Serial.print("sita");
  59.       Serial.println(sita);
  60.     }
  61.     long int cosin(long int t)
  62.     {
  63.           return (long int)(100000*cos(t*3.1415926/180));
  64.     }
  65.     long int arccos(long int t)
  66.     {
  67.     long int left=0,mid,right=180,i=0;
  68.     long int cos_mid,delta;
  69.     if(t==100000) return 0;
  70.     while(i<9)
  71.     {
  72.        mid=(left+right)/2;
  73.        cos_mid=(long int)(100000*cos(mid*3.1415926/180));
  74.        if(mid>90) delta=(181-mid)*29;
  75.        else delta=mid*29;
  76.        if(fabs(t-cos_mid)<=delta)
  77.           break;
  78.        else if(t<cos_mid)
  79.          left=mid;
  80.        else right=mid;
  81.        i++;
  82.        }
  83.        return mid;
  84.     }
  85.     void setup()
  86.     {
  87.       Serial.begin(9600);
  88.       InitHand();
  89.       Serial.println("InitHand done!");
  90.     }
  91.     int n;
  92.     int xa,xb,ya,yb;
  93.     //因为自己对processing和arduino通信不是很了解，所以干脆
  94.     //把测试点直接存在arduino里，测试的时候发送0~3进行选择
  95.     //以后在改进了
  96.     unsigned int Point[10][4]={ {0,30,40,30},
  97.                                 {50,25,15,80},
  98.                                 {30,10,30,100},
  99.                                 {50,65,5,65},   };
  100.     void loop()
  101.     {
  102.            if(Serial.available())
  103.            {
  104.              n=Serial.read()-48;
  105.              Serial.print("The line-points group is");
  106.              Serial.println(n);
  107.              xa=Point[n][0];
  108.              ya=Point[n][1];
  109.              xb=Point[n][2];
  110.              yb=Point[n][3];
  111.              Serial.println("And the points are(");
  112.              Serial.print(xa);Serial.print(",");Serial.print(ya);Serial.print("),(");
  113.              Serial.print(xb);Serial.print(",");Serial.print(ya);Serial.println(")");
  114.              Serial.println("Yes or N0 ?");
  115.              delay(4000);
  116.              Serial.println("begin in 3s");
  117.              delay(1000);
  118.              Serial.println("begin in 2s");
  119.              delay(1000);
  120.              Serial.println("begin in 1s");
  121.              delay(1000);
  122.              Serial.println("go!");
  123.                Line(xa,ya,xb,yb);
  124.             }
  125.             }
复制代码

接下来有张原理草图，所用的“机械手臂”大家可以去看我的另张帖子“与大家分享我的六足”

希望大家看得懂
更希望大家多交流！


机械, 手臂
分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏 分享淘帖 顶顶 踩踩

相关帖子
回复

使用道具 举报

沙发
ID:137687 发表于 2016-8-27 09:55 | 只看该作者
你写的东西非常好！可惜我目前看不懂！很想搞关节型机械臂，但没有这方面软件，grbl只能用在直角坐标系的机器上。
回复

使用道具 举报

板凳
ID:137696 发表于 2016-8-27 11:22 | 只看该作者
学习了  谢谢楼主~~~~
回复

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表