asdf
Embed:
(wiki syntax)
Show/hide line numbers
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&°1<0.00000001) 00182 { 00183 deg1=0; 00184 if(deg2>-0.00000001&°2<0.00000001) 00185 { 00186 deg2=0; 00187 } 00188 } 00189 else if (deg2>-0.00000001&°2<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 }
Generated on Mon Jul 18 2022 03:14:21 by
1.7.2