asdf

Dependencies:   stepper mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers robo.h Source File

robo.h

00001 #include<iostream>
00002 #include<cmath>
00003 
00004 #include "mbed.h"
00005 
00006 const double pi=3.1415;
00007 class robo
00008 {
00009 public:
00010     robo(double a, double b,double x1,double y1,double z1);
00011     void setrobo(double *dest,int c);
00012     void syschange();//坐标变换
00013     void degcalculate();//三舵机角度计算
00014     void droute(double a);//直线路径
00015     void croute(double a);//曲线路径
00016     void dexcute(double *p);//直线执行
00017     void cexcute(double *p);//曲线执行
00018     double argcalculate(double x, double y);//求反正切对应角度
00019 private://两组六个原始坐标,两组六个柱坐标下参数,两个机械臂常数,两组六个个机械臂转角,三个转角差,中间过程点直角坐标与柱坐标
00020     double X1;
00021     double Y1;
00022     double Z1;
00023     double X2;
00024     double Y2;
00025     double Z2;
00026     
00027     double P1;
00028     double H1;
00029     double Fai1;
00030     double P2;
00031     double H2;
00032     double Fai2;
00033     
00034     double A;
00035     double B;
00036     
00037     double faia1;
00038     double faia2;
00039     double faia3;
00040     double faib1;
00041     double faib2;
00042     double faib3;
00043     double fai1;
00044     double fai2;
00045     double fai3;
00046     
00047     double delta1;
00048     double delta2;
00049     double delta3;
00050     
00051     double P;
00052     double fai;
00053     double H;
00054     
00055     double x;
00056     double y;
00057     double z;
00058 
00059     
00060 };
00061 
00062 robo::robo(double a, double b,double x1,double y1,double z1)
00063 {
00064     A=a;
00065     B=b;
00066     X1=x1;
00067     Y1=y1;
00068     Z1=z1;
00069     X2=x1;
00070     Y2=y1;
00071     Z2=z1;
00072 /*    cout<<"please input the first parameter A:";
00073     cin>>A;
00074     cout<<"please input the second parameter B:";
00075     cin>>B;
00076     */
00077 };
00078 void robo::setrobo(double *dest,int c)
00079 {
00080     X1 = X2;
00081     Y1 = Y2;
00082     Z1 = Z2;    
00083     X2=dest[0];
00084     Y2=dest[1];
00085     Z2=dest[2];
00086     syschange();
00087     degcalculate();
00088     if(c==1)
00089     {
00090         dexcute(dest);
00091     }
00092     else cexcute(dest);
00093 }
00094 double robo::argcalculate(double x, double y)
00095 {
00096     double a;
00097     if(x>0.000001||x<-0.000001)
00098     {
00099         if(x>0&&y>=0)
00100         {
00101             a=atan(y/x);
00102         }
00103         else if (x>0&&y<0)
00104         {
00105             a=2*pi+atan(y/x);
00106         }
00107         else
00108         {
00109             a=pi+atan(y/x);
00110         }
00111     }
00112     else
00113     {
00114         if(y>0)
00115         {
00116             a=pi/2;
00117         }
00118         else
00119         {
00120             a=3*pi/2;
00121         }
00122     }
00123   /*  cout<<"输入坐标为:"<<x<<","<<y<<endl;
00124     cout<<"对应柱坐标弧度为:"<<a<<endl;
00125     cout<<"对应角度为:"<<a/pi*180<<endl;
00126     cout<<endl;
00127     */return a;
00128 }
00129 void robo::syschange()//Fai1、Fai2范围0-360°
00130 {
00131     P1=sqrt(X1*X1+Y1*Y1);
00132     P2=sqrt(X2*X2+Y2*Y2);
00133     H1=Z1;
00134     H2=Z2;
00135     Fai1=argcalculate(X1,Y1);
00136     Fai2=argcalculate(X2,Y2);
00137   /*  cout<<"起始点对应柱坐标为:"<<P1<<","<<Fai1/pi*180<<","<<H1<<endl;
00138     cout<<endl;
00139     cout<<"终止点对应柱坐标为:"<<P2<<","<<Fai2/pi*180<<","<<H2<<endl;
00140     cout<<endl;
00141 */
00142 }
00143 void robo::degcalculate()
00144 {
00145     double p2_h2_1=P1*P1+H1*H1;//计算始末的角度参数
00146     double sqrT1=sqrt(p2_h2_1);
00147     double a1=(A*A+P1*P1+H1*H1-B*B)/(2*A*sqrT1);
00148     double a2=P1/sqrT1;
00149     double b1=a1;
00150     double b2=(A*A-B*B-P1*P1-H1*H1)/(2*B*sqrT1);
00151     faia2=pi-asin(a1)-asin(a2);
00152     faia3=asin(b1)-asin(b2);
00153     faia1=Fai1;
00154     double p2_h2_2=P2*P2+H2*H2;
00155     double sqrT2=sqrt(p2_h2_2);
00156     double c1=(A*A+P2*P2+H2*H2-B*B)/(2*A*sqrT2);
00157     double c2=P2/sqrT2;
00158     double d1=c1;
00159     double d2=(A*A-B*B-P2*P2-H2*H2)/(2*B*sqrT2);
00160     faib2=pi-asin(c1)-asin(c2);
00161     faib3=asin(d1)-asin(d2);
00162     faib1=Fai2;
00163 /*    cout<<"起始点对应姿态为:"<<faia1<<" "<<faia2<<" "<<faia3<<endl;
00164     cout<<endl;
00165     cout<<"终止点对应姿态为:"<<faib1<<" "<<faib2<<" "<<faib3<<endl;
00166     cout<<endl;
00167 */
00168 }
00169 void robo::droute(double a)
00170 {
00171     if(a<0||a>1){
00172 //        cout<<"WRONG!"<<endl;
00173     }
00174     else
00175     {
00176   //      cout<<"开始直线规划!"<<endl;
00177     P=sqrt(a*a*P2*P2+(1-a)*(1-a)*P1*P1+2*a*(1-a)*P1*P2*cos(Fai2-Fai1));
00178     H=H1+a*(H2-H1);
00179     double deg1=a*P2*sin(Fai2-pi/2)+(1-a)*P1*sin(Fai1-pi/2);
00180     double deg2=a*P2*cos(Fai2-pi/2)+(1-a)*P1*cos(Fai1-pi/2);
00181     if(deg1>-0.00000001&&deg1<0.00000001)
00182     {
00183         deg1=0;
00184         if(deg2>-0.00000001&&deg2<0.00000001)
00185         {
00186             deg2=0;
00187         }
00188     }
00189     else if (deg2>-0.00000001&&deg2<0.00000001)
00190     {
00191         deg2=0;
00192     }
00193 //  cout<<cos(Fai1)<<endl; 角度累计计算有误差
00194     if(deg1>=0)
00195     {
00196         if(deg2==0)
00197         {
00198             fai=pi/2;
00199         }
00200         else
00201         {
00202             if(deg2>0)
00203             {
00204                 fai=atan(deg1/deg2);
00205             }
00206             else
00207             {
00208                 fai=pi+atan(deg1/deg2);
00209             }
00210         }
00211     }
00212     else
00213     {
00214         if(deg2==0)
00215         {
00216             fai=3*pi/2;
00217         }
00218         else
00219         {
00220             if(deg2>0)
00221             {
00222                 fai=2*pi+atan(deg1/deg2);
00223             }
00224             else
00225             {
00226                 fai=pi+atan(deg1/deg2);
00227             }
00228         }
00229     }
00230   //  cout<<"两个参数deg1、deg2分别为:"<<deg1<<" "<<deg2<<endl;
00231     double p2_h2=P*P+H*H;//计算末端三个角度参数
00232     double sqrT=sqrt(p2_h2);
00233     double a1=(A*A+P*P+H*H-B*B)/(2*A*sqrT);
00234     double a2=P/sqrT;
00235     double b1=(A*A+P*P+H*H-B*B)/(2*A*sqrT);
00236     double b2=(A*A-B*B-P*P-H*H)/(2*B*sqrT);
00237     faia1=fai1;
00238     faia2=fai2;
00239     faia3=fai3;
00240     fai2=pi-asin(a1)-asin(a2);
00241     fai3=asin(b1)-asin(b2);
00242     fai1=fai;
00243     delta1=fai1-faia1;
00244     delta2=fai2-faia2;
00245     delta3=fai3-faia3;
00246 /*    cout<<"运动百分比:"<<a*100<<"%"<<endl;
00247     cout<<"末端位置为:"<<P<<" "<<fai<<" "<<H<<endl;
00248     cout<<"末端姿态为:"<<fai1<<" "<<fai2<<" "<<fai3<<endl;
00249     cout<<"转动弧度为:"<<delta1<<" "<<delta2<<" "<<delta3<<endl;
00250     cout<<"转动角度为:"<<delta1*180/pi<<" "<<delta2*180/pi<<" "<<delta3*180/pi<<endl;
00251   */  }
00252     
00253 }
00254 void robo::croute(double a)
00255 {
00256     if(a<0||a>1){
00257 //        cout<<"WRONG!"<<endl;
00258     }
00259     else
00260     {
00261         cout<<endl;
00262         cout<<endl;
00263   //      cout<<"开始曲线规划!"<<endl;
00264         double alpha;
00265         double r=sqrt((X2-X1)*(X2-X1)+(Y2-Y1)*(Y2-Y1))/2;
00266         if((X1-X2)!=0)
00267         {
00268             if(Y1>Y2)
00269             {
00270                 if(X1>X2)
00271                 {
00272                     alpha=pi+atan((Y1-Y2)/(X1-X2));
00273                 }
00274                 else
00275                 {
00276                     alpha=2*pi+atan((Y1-Y2)/(X1-X2));
00277                 }
00278             }
00279             else
00280             {
00281                 if(X1<X2)
00282                 {
00283                     alpha=atan((Y1-Y2)/(X1-X2));
00284                 }
00285                 else
00286                 {
00287                     alpha=pi+atan((Y1-Y2)/(X1-X2));
00288                 }
00289             }
00290         }
00291         else
00292         {
00293             if(Y1>Y2)
00294             {
00295                 alpha=3*pi/2;
00296             }
00297             else
00298             {
00299                 alpha=pi/2;
00300             }
00301         }
00302   //      cout<<"夹角参数为:"<<alpha<<endl;
00303         double xita=a*pi;
00304         if(alpha>=0&&alpha<=(pi/2))
00305         {
00306             x=(X1+X2)/2-r*cos(xita-alpha);
00307             y=(Y1+Y2)/2+r*sin(xita-alpha);
00308             z=Z1;
00309         }
00310         else
00311         {
00312             x=(X1+X2)/2-r*cos(xita+alpha);
00313             y=(Y1+Y2)/2-r*sin(xita+alpha);
00314             z=Z1;
00315         }
00316         if(x>-0.00000001&&x<0.00000001)
00317         {
00318             x=0;
00319         }
00320    //     cout<<"圆弧直角坐标为:"<<x<<" "<<y<<" "<<z<<" "<<endl;
00321         fai=argcalculate(x,y);
00322         double p2=x*x+y*y;
00323         P=sqrt(p2);
00324         H=z;
00325         double p2_h2=P*P+H*H;//计算末端三个角度参数
00326         double sqrT=sqrt(p2_h2);
00327         double a1=(A*A+P*P+H*H-B*B)/(2*A*sqrT);
00328         double a2=P/sqrT;
00329         double b1=(A*A+P*P+H*H-B*B)/(2*A*sqrT);
00330         double b2=(A*A-B*B-P*P-H*H)/(2*B*sqrT);
00331         fai2=pi-asin(a1)-asin(a2);
00332         fai3=asin(b1)-asin(b2);
00333         fai1=fai;
00334         delta1=fai1-faia1;
00335         delta2=fai2-faia2;
00336         delta3=fai3-faia3;
00337  /*       cout<<"运动百分比:"<<a*100<<"%"<<endl;
00338         cout<<"中间圆弧位置为:"<<P<<" "<<fai<<" "<<H<<endl;
00339         cout<<"中间姿态为:"<<fai1<<" "<<fai2<<" "<<fai3<<endl;
00340         cout<<"转动角度为:"<<delta1<<" "<<delta2<<" "<<delta3<<endl;
00341  */       }
00342 }
00343 
00344 void robo::dexcute(double *p)
00345 {
00346     double i=0;
00347     int j = 0;
00348     while(i<=1)
00349     {
00350         droute(i);
00351         //myServo1.rotate(fai1/pi*180);
00352         //myServo2.rotate(fai2/pi*180);
00353         //myServo3.rotate(fai3/pi*180);
00354         //wait(0.5);
00355         p[j]=delta1;
00356         p[j+1]=delta2;
00357         p[j+2]=delta3;
00358         j = j + 3;
00359         
00360         i=i+0.1;
00361     //    cout<<"正在执行步数:"<<i*10<<endl;
00362     }
00363 }
00364 void robo::cexcute(double *p)
00365 {
00366     double i=0;
00367     int j = 0;
00368     while(i<=1)
00369     {
00370         croute(i);
00371        // double deg1=fai1/pi*180;
00372         //myServo1.rotate(delta/pi*180);
00373         //myServo2.rotate(fai2/pi*180);
00374         //myServo3.rotate(fai3/pi*180);
00375         //wait(0.5);
00376 //      cout<<"正在执行步数:"<<i*10<<endl;
00377         p[j]=delta1;
00378         p[j+1]=delta2;
00379         p[j+2]=delta3;
00380         j = j + 3;
00381         
00382         i=i+0.1;
00383         
00384 //      cout<<"已旋转角度:"<<deg1<<endl;
00385     }
00386 }