這兩篇文章，是自己很久前寫的了，09年，呵呵 那時候剛接觸arduino，對電子、機械什么的，也是完全的外行，突然想起這兩篇文章，去機器人天空翻出來看了看 哈哈 還是挺有意思的，就轉過來啦
最近我在弄一個小程序，就是用機械手臂來寫字和畫畫
本來是打算做六足的，但是在制作的時候一不小心電壓接高了！
結果，只見伺服控制板火花四濺，一縷青煙裊裊升起！哎
200在一瞬間燒了！！
六足也就只能擱一邊了，慢慢攢錢買個新的吧
不過另一個想法就在我頭腦里出現了
用一只腿來實現寫字
經過一個星期的思考和n個晚上的時間，我終于寫出了核心代碼
先是在DEV上模擬實現，然后再移植到arduino里
原理就是，把每個字（目前只考慮英文與數字，簡單點）用坐標表示，存在eeprom里，
于是每個字就可以用一根根直線畫出來
核心程序就是實時計算3個伺服的角度，如此讓機械手臂畫出一條條直線
經過幾個測試，我寫的程序能在arduino上運行，但精確度不是很高
才85%~97%，希望能達到99%
因為伺服控制器被燒了，所以無法進行完全測試，而只是輸出每時刻三個伺服的角度
以后應該還有很多要測試吧，但是高三了，可能沒那么多時間，會弄的很慢
公布出源碼，與大家討論討論，代碼寫的沒什么水平，見笑
不知道我這個思路對不對，如果大家有更好的方法，盡情提出來~！
源碼已經過測試，可以直接編譯運行
dev版，可以自己修改printf來看實現過程
/*************************Hand_v1_0_dev_test**********************************************
- #include
- #include
- #include
- int l2=60,l3=70,li;// 用mm做單位
- void Line(int xa,int ya,int xb,int yb)
- {
- //用int代替float,擴大100000倍，xa3位，cos5位，alpha為整，oa4位
- int cosin(int alpha);
- int arccos(int cos_alpha);
- int alpha=0,alphai=0,beta=0,sita=0,fai=0;
- int cos_alpha,cos_alphai,cos_fai,sin_fai,cos_sita,sin_sita;
- int oa,oa_2,ob,ob_2,xx_yy;
-
-
- //save the caculate result for reuse
- oa_2=xa*xa+ya*ya;
- oa=(int)sqrt(oa_2);
- ob_2=xb*xb+yb*yb;
- ob=(int)sqrt(ob_2);
- xx_yy=xa*xb+ya*yb;
- //printf("oa:%d,ob:%d,xx_yy:%d",oa,ob,xx_yy);
- //printf("Line 1");
- cos_alpha=100000*abs(xx_yy)/(oa*ob);
- cos_alphai=0;
- alpha=arccos(cos_alpha);
- alphai=0;
- //printf("cos_fai=(int)(100000*(%d-%d)/(sqrt(%d+%d-2*%d)*%d));",
- //oa_2,xx_yy,oa_2,ob_2,xx_yy,oa);
- cos_fai=(int)(100000*(oa_2-xx_yy)/(sqrt(oa_2+ob_2-2*xx_yy)*oa));
- //printf("cos_fai=100000*(%d-%d)/(sqrt(%d+%d-2*%d)*%d))",
- //oa_2,xx_yy,oa_2,ob_2,xx_yy,oa);
- sin_fai=cosin(arccos(cos_fai)-90);
- //printf("cos_alpha:%d",cos_alpha);
- //printf("alpha:%d,alphai:%d",arccos(cos_alpha),alphai);
- //printf("fai:=%d,cos_fai:%d,sin_fai:%d",arccos(cos_fai),cos_fai,sin_fai);
- //getchar();
- while(abs(alphai-alpha)>1)
- {
- //printf("alpha:%f,alphai:%f",alpha,alphai);
- //printf("oa:%f,oa_2:%f,ob:%f,ob_2:%f,",oa,oa_2,ob,ob_2);
- if(alphai>alpha)
- alphai-=1;
- else alphai+=1;
- //printf("alpha:%d,alphai:%d",alpha,alphai);
- //printf("li=(%d*%d)/(%d*%d+%d*%d);",
- //sin_fai,oa,cosin(alphai-90),cos_fai,cosin(alphai),sin_fai);
- li=(sin_fai*oa)/cosin(alphai+arccos(cos_fai)-90);
- printf("li=%d",li);
- //printf("cos_sita=100000*(%d*%d+%d*%d-%d*%d)/(2*%d*%d);",
- //l2,l2,l3,l3,li,li,l2,l3);
- cos_sita=100000*(l2*l2+l3*l3-li*li)/(2*l2*l3);
- sin_sita=cosin(arccos(cos_sita)-90);
- //printf("cos_sita:%d,sin_sita:%d",cos_sita,sin_sita);
- int t1=10000000,t2=0;
- //printf("Line 2");
- //printf("sita:%d",arccos(cos_sita));
- while(abs(t1-t2)>100000)
- {
- t1=l2*cosin(beta-90);
- t2=l3*cosin(beta+arccos(cos_sita)-90);
- //printf("cosin(beta-90):%d,cosin(beta+sita):%d",
- //cosin(beta-90),cosin(beta+arccos(cos_sita)-90));
- //printf("t1:%d,t2:%d",t1,t2);
- if(t1>t2)
- beta-=1;
- else beta+=1;
- }
- //printf("Line 3");
- //printf("alpha:%d,beta:%d,sita:%d,",
- //alpha,beta,arccos(cos_sita));
- //getchar();
- //servo(.....);
- //delay(100);
- printf("alpha:%d,beta:%d,sita:%d,",
- alpha,beta,arccos(cos_sita));
- }
- getchar();
- }
-
- void InitHand()
- {
- int beta=45,sita=45;
- printf("beta:%d,sita:%d,",beta,sita);
- }
- int cosin(int t)
- {
- return (int)(100000*cos(t*3.1415926/180));
- }
- int arccos(int t)
- {
- int left=0,mid,right=180,i=0;
- int cos_mid,delta;
- if(t==100000) return 0;
- while(i<9)
- {
- mid=(left+right)/2;
- cos_mid=(int)(100000*cos(mid*3.1415926/180));
- if(mid>90) delta=(181-mid)*29;
- else delta=mid*29;
- //printf("cos:%f,fabs:%f,delta:%f",cos(mid*3.1415926/180),
- // fabs(t-cos(mid*3.1415926/180)),delta);
- if(fabs(t-cos_mid)<=delta)
- break;
- else if(t<cos_mid)
- left=mid;
- else right=mid;
- i++;
- }
- return mid;
- }
- main()
- {
- int i,j,n;
- int xa,xb,ya,yb;
- n=20;
- InitHand();
- while(1){
- system("cls");
- printf("Input x1,y1,x2,y2(mm)");
- scanf("%d,%d,%d,%d",&xa,&ya,&xb,&yb);
- printf("InitHand() done");
- getchar();
- //printf("xa:%3.1f,ya:%3.1f,xb:%3.1f,yb:%3.1f,",1,2.8,7,6);
- Line(xa,ya,xb,yb);
- getchar();
- }
- }
/****************************Hand_v1_0*************************************************
- long int l2=60,l3=70,li;
- void Line(long int xa,long int ya,long int xb,long int yb)
- {
- long int cosin(long int alpha);
- long int arccos(long int cos_alpha);
- long int alpha=0,alphai=0,beta=0,sita=0,fai=0;
- long int cos_alpha,cos_alphai,cos_fai,sin_fai,cos_sita,sin_sita;
- long int oa,oa_2,ob,ob_2,xx_yy;
-
-
- //save the caculate result for reuse
- oa_2=xa*xa+ya*ya;
- oa=(long int)sqrt(oa_2);
- ob_2=xb*xb+yb*yb;
- ob=(long int)sqrt(ob_2);
- xx_yy=xa*xb+ya*yb;
- cos_alpha=100000*abs(xx_yy)/(oa*ob);
- cos_alphai=0;
- alpha=arccos(cos_alpha);
- alphai=0;
- cos_fai=(long int)(100000*(oa_2-xx_yy)/(sqrt(oa_2+ob_2-2*xx_yy)*oa));
- sin_fai=cosin(arccos(cos_fai)-90);
- while(abs(alphai-alpha)>1)
- {
- if(alphai>alpha)
- alphai-=1;
- else alphai+=1;
- li=(sin_fai*oa)/cosin(alphai+arccos(cos_fai)-90);
- printf("li=%d",li);
- cos_sita=100000*(l2*l2+l3*l3-li*li)/(2*l2*l3);
- sin_sita=cosin(arccos(cos_sita)-90);
- long int t1=10000000,t2=0;
- while(abs(t1-t2)>100000)
- {
- t1=l2*cosin(beta-90);
- t2=l3*cosin(beta+arccos(cos_sita)-90);
- if(t1>t2)
- beta-=1;
- else beta+=1;
- }
- Serial.print("alpha:");
- Serial.println(alpha);
- Serial.print("beta:");
- Serial.println(beta);
- Serial.print("sita:");
- Serial.println(arccos(cos_sita));
- Serial.print("li:");
- Serial.println(li);
- }
- Serial.println("Line() done!");
- }
-
- void InitHand()
- {
- long int beta=45,sita=45;
- Serial.print("beta:");
- Serial.println(beta);
- Serial.print("sita");
- Serial.println(sita);
- }
- long int cosin(long int t)
- {
- return (long int)(100000*cos(t*3.1415926/180));
- }
- long int arccos(long int t)
- {
- long int left=0,mid,right=180,i=0;
- long int cos_mid,delta;
- if(t==100000) return 0;
- while(i<9)
- {
- mid=(left+right)/2;
- cos_mid=(long int)(100000*cos(mid*3.1415926/180));
- if(mid>90) delta=(181-mid)*29;
- else delta=mid*29;
- if(fabs(t-cos_mid)<=delta)
- break;
- else if(t<cos_mid)
- left=mid;
- else right=mid;
- i++;
- }
- return mid;
- }
- void setup()
- {
- Serial.begin(9600);
- InitHand();
- Serial.println("InitHand done!");
- }
- int n;
- int xa,xb,ya,yb;
- //因為自己對processing和arduino通信不是很了解，所以干脆
- //把測試點直接存在arduino里，測試的時候發送0~3進行選擇
- //以后在改進了
- unsigned int Point[10][4]={ {0,30,40,30},
- {50,25,15,80},
- {30,10,30,100},
- {50,65,5,65}, };
- void loop()
- {
- if(Serial.available())
- {
- n=Serial.read()-48;
- Serial.print("The line-points group is");
- Serial.println(n);
- xa=Point[n][0];
- ya=Point[n][1];
- xb=Point[n][2];
- yb=Point[n][3];
- Serial.println("And the points are(");
- Serial.print(xa);Serial.print(",");Serial.print(ya);Serial.print("),(");
- Serial.print(xb);Serial.print(",");Serial.print(ya);Serial.println(")");
- Serial.println("Yes or N0 ?");
- delay(4000);
- Serial.println("begin in 3s");
- delay(1000);
- Serial.println("begin in 2s");
- delay(1000);
- Serial.println("begin in 1s");
- delay(1000);
- Serial.println("go!");
- Line(xa,ya,xb,yb);
- }
- }
接下來有張原理草圖，所用的“機械手臂”大家可以去看我的另張帖子“與大家分享我的六足”
025248gbmdb5zxu2tusu8b.jpg (25.37 KB, 下載次數: 91)
下載附件
2016-4-9 19:44 上傳
希望大家看得懂
更希望大家多交流！
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
