找回密碼
 立即注冊(cè)

QQ登錄

只需一步，快速開(kāi)始

搜索
查看: 6109|回復(fù): 2
打印 上一主題 下一主題
收起左側(cè)

我的機(jī)械手臂設(shè)想（附喲代碼~�。┐蠹疫M(jìn)來(lái)討論討論~

[復(fù)制鏈接]
跳轉(zhuǎn)到指定樓層
樓主
這兩篇文章，是自己很久前寫(xiě)的了，09年，呵呵 那時(shí)候剛接觸arduino，對(duì)電子、機(jī)械什么的，也是完全的外行，突然想起這兩篇文章，去機(jī)器人天空翻出來(lái)看了看 哈哈 還是挺有意思的，就轉(zhuǎn)過(guò)來(lái)啦
最近我在弄一個(gè)小程序，就是用機(jī)械手臂來(lái)寫(xiě)字和畫(huà)畫(huà)
本來(lái)是打算做六足的，但是在制作的時(shí)候一不小心電壓接高了！
結(jié)果，只見(jiàn)伺服控制板火花四濺，一縷青煙裊裊升起！哎
200在一瞬間燒了��！
六足也就只能擱一邊了，慢慢攢錢買個(gè)新的吧
不過(guò)另一個(gè)想法就在我頭腦里出現(xiàn)了
用一只腿來(lái)實(shí)現(xiàn)寫(xiě)字
經(jīng)過(guò)一個(gè)星期的思考和n個(gè)晚上的時(shí)間，我終于寫(xiě)出了核心代碼
先是在DEV上模擬實(shí)現(xiàn)，然后再移植到arduino里
原理就是，把每個(gè)字（目前只考慮英文與數(shù)字，簡(jiǎn)單點(diǎn)）用坐標(biāo)表示，存在eeprom里，
于是每個(gè)字就可以用一根根直線畫(huà)出來(lái)
核心程序就是實(shí)時(shí)計(jì)算3個(gè)伺服的角度，如此讓機(jī)械手臂畫(huà)出一條條直線
經(jīng)過(guò)幾個(gè)測(cè)試，我寫(xiě)的程序能在arduino上運(yùn)行，但精確度不是很高
才85%~97%，希望能達(dá)到99%
因?yàn)樗欧刂破鞅粺�，所以無(wú)法進(jìn)行完全測(cè)試，而只是輸出每時(shí)刻三個(gè)伺服的角度
以后應(yīng)該還有很多要測(cè)試吧，但是高三了，可能沒(méi)那么多時(shí)間，會(huì)弄的很慢
公布出源碼，與大家討論討論，代碼寫(xiě)的沒(méi)什么水平，見(jiàn)笑
不知道我這個(gè)思路對(duì)不對(duì)，如果大家有更好的方法，盡情提出來(lái)~！
源碼已經(jīng)過(guò)測(cè)試，可以直接編譯運(yùn)行
dev版，可以自己修改printf來(lái)看實(shí)現(xiàn)過(guò)程
/*************************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,擴(kuò)大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.             }
復(fù)制代碼
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.     //因?yàn)樽约簩?duì)processing和arduino通信不是很了解，所以干脆
  94.     //把測(cè)試點(diǎn)直接存在arduino里，測(cè)試的時(shí)候發(fā)送0~3進(jìn)行選擇
  95.     //以后在改進(jìn)了
  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.             }
復(fù)制代碼

接下來(lái)有張?jiān)聿輬D，所用的“機(jī)械手臂”大家可以去看我的另張?zhí)印芭c大家分享我的六足”

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


分享到:  QQ好友和群QQ好友和群 QQ空間QQ空間 騰訊微博騰訊微博 騰訊朋友騰訊朋友
收藏收藏 分享淘帖 頂頂 踩踩

相關(guān)帖子
回復(fù)

使用道具 舉報(bào)

沙發(fā)
ID:137687 發(fā)表于 2016-8-27 09:55 | 只看該作者
你寫(xiě)的東西非常好！可惜我目前看不懂！很想搞關(guān)節(jié)型機(jī)械臂，但沒(méi)有這方面軟件，grbl只能用在直角坐標(biāo)系的機(jī)器上。
回復(fù)

使用道具 舉報(bào)

板凳
ID:137696 發(fā)表于 2016-8-27 11:22 | 只看該作者
學(xué)習(xí)了  謝謝樓主~~~~
回復(fù)

使用道具 舉報(bào)

本版積分規(guī)則

手機(jī)版|小黑屋|51黑電子論壇 |51黑電子論壇6群 QQ 管理員QQ:125739409;技術(shù)交流QQ群281945664

Powered by 單片機(jī)教程網(wǎng)

快速回復(fù) 返回頂部 返回列表