![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
gggg
Fork of Nucleo_pwm by
Diff: robo.h
- Revision:
- 0:2e00b81c9137
diff -r 000000000000 -r 2e00b81c9137 robo.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/robo.h Fri May 11 06:06:10 2018 +0000 @@ -0,0 +1,386 @@ +#include<iostream> +#include<cmath> + +#include "mbed.h" + +const double pi=3.1415; +class robo +{ +public: + robo(double a, double b,double x1,double y1,double z1); + void setrobo(double *dest,int c); + void syschange();//坐标变换 + void degcalculate();//三舵机角度计算 + void droute(double a);//直线路径 + void croute(double a);//曲线路径 + void dexcute(double *p);//直线执行 + void cexcute(double *p);//曲线执行 + double argcalculate(double x, double y);//求反正切对应角度 +private://两组六个原始坐标,两组六个柱坐标下参数,两个机械臂常数,两组六个个机械臂转角,三个转角差,中间过程点直角坐标与柱坐标 + double X1; + double Y1; + double Z1; + double X2; + double Y2; + double Z2; + + double P1; + double H1; + double Fai1; + double P2; + double H2; + double Fai2; + + double A; + double B; + + double faia1; + double faia2; + double faia3; + double faib1; + double faib2; + double faib3; + double fai1; + double fai2; + double fai3; + + double delta1; + double delta2; + double delta3; + + double P; + double fai; + double H; + + double x; + double y; + double z; + + +}; + +robo::robo(double a, double b,double x1,double y1,double z1) +{ + A=a; + B=b; + X1=x1; + Y1=y1; + Z1=z1; + X2=x1; + Y2=y1; + Z2=z1; +/* cout<<"please input the first parameter A:"; + cin>>A; + cout<<"please input the second parameter B:"; + cin>>B; + */ +}; +void robo::setrobo(double *dest,int c) +{ + X1 = X2; + Y1 = Y2; + Z1 = Z2; + X2=dest[0]; + Y2=dest[1]; + Z2=dest[2]; + syschange(); + degcalculate(); + if(c==1) + { + dexcute(dest); + } + else cexcute(dest); +} +double robo::argcalculate(double x, double y) +{ + double a; + if(x>0.000001||x<-0.000001) + { + if(x>0&&y>=0) + { + a=atan(y/x); + } + else if (x>0&&y<0) + { + a=2*pi+atan(y/x); + } + else + { + a=pi+atan(y/x); + } + } + else + { + if(y>0) + { + a=pi/2; + } + else + { + a=3*pi/2; + } + } + /* cout<<"输入坐标为:"<<x<<","<<y<<endl; + cout<<"对应柱坐标弧度为:"<<a<<endl; + cout<<"对应角度为:"<<a/pi*180<<endl; + cout<<endl; + */return a; +} +void robo::syschange()//Fai1、Fai2范围0-360° +{ + P1=sqrt(X1*X1+Y1*Y1); + P2=sqrt(X2*X2+Y2*Y2); + H1=Z1; + H2=Z2; + Fai1=argcalculate(X1,Y1); + Fai2=argcalculate(X2,Y2); + /* cout<<"起始点对应柱坐标为:"<<P1<<","<<Fai1/pi*180<<","<<H1<<endl; + cout<<endl; + cout<<"终止点对应柱坐标为:"<<P2<<","<<Fai2/pi*180<<","<<H2<<endl; + cout<<endl; +*/ +} +void robo::degcalculate() +{ + double p2_h2_1=P1*P1+H1*H1;//计算始末的角度参数 + double sqrT1=sqrt(p2_h2_1); + double a1=(A*A+P1*P1+H1*H1-B*B)/(2*A*sqrT1); + double a2=P1/sqrT1; + double b1=a1; + double b2=(A*A-B*B-P1*P1-H1*H1)/(2*B*sqrT1); + faia2=pi-asin(a1)-asin(a2); + faia3=asin(b1)-asin(b2); + faia1=Fai1; + double p2_h2_2=P2*P2+H2*H2; + double sqrT2=sqrt(p2_h2_2); + double c1=(A*A+P2*P2+H2*H2-B*B)/(2*A*sqrT2); + double c2=P2/sqrT2; + double d1=c1; + double d2=(A*A-B*B-P2*P2-H2*H2)/(2*B*sqrT2); + faib2=pi-asin(c1)-asin(c2); + faib3=asin(d1)-asin(d2); + faib1=Fai2; +/* cout<<"起始点对应姿态为:"<<faia1<<" "<<faia2<<" "<<faia3<<endl; + cout<<endl; + cout<<"终止点对应姿态为:"<<faib1<<" "<<faib2<<" "<<faib3<<endl; + cout<<endl; +*/ +} +void robo::droute(double a) +{ + if(a<0||a>1){ +// cout<<"WRONG!"<<endl; + } + else + { + // cout<<"开始直线规划!"<<endl; + P=sqrt(a*a*P2*P2+(1-a)*(1-a)*P1*P1+2*a*(1-a)*P1*P2*cos(Fai2-Fai1)); + H=H1+a*(H2-H1); + double deg1=a*P2*sin(Fai2-pi/2)+(1-a)*P1*sin(Fai1-pi/2); + double deg2=a*P2*cos(Fai2-pi/2)+(1-a)*P1*cos(Fai1-pi/2); + if(deg1>-0.00000001&°1<0.00000001) + { + deg1=0; + if(deg2>-0.00000001&°2<0.00000001) + { + deg2=0; + } + } + else if (deg2>-0.00000001&°2<0.00000001) + { + deg2=0; + } +// cout<<cos(Fai1)<<endl; 角度累计计算有误差 + if(deg1>=0) + { + if(deg2==0) + { + fai=pi/2; + } + else + { + if(deg2>0) + { + fai=atan(deg1/deg2); + } + else + { + fai=pi+atan(deg1/deg2); + } + } + } + else + { + if(deg2==0) + { + fai=3*pi/2; + } + else + { + if(deg2>0) + { + fai=2*pi+atan(deg1/deg2); + } + else + { + fai=pi+atan(deg1/deg2); + } + } + } + // cout<<"两个参数deg1、deg2分别为:"<<deg1<<" "<<deg2<<endl; + double p2_h2=P*P+H*H;//计算末端三个角度参数 + double sqrT=sqrt(p2_h2); + double a1=(A*A+P*P+H*H-B*B)/(2*A*sqrT); + double a2=P/sqrT; + double b1=(A*A+P*P+H*H-B*B)/(2*A*sqrT); + double b2=(A*A-B*B-P*P-H*H)/(2*B*sqrT); + faia1=fai1; + faia2=fai2; + faia3=fai3; + fai2=pi-asin(a1)-asin(a2); + fai3=asin(b1)-asin(b2); + fai1=fai; + delta1=fai1-faia1; + delta2=fai2-faia2; + delta3=fai3-faia3; +/* cout<<"运动百分比:"<<a*100<<"%"<<endl; + cout<<"末端位置为:"<<P<<" "<<fai<<" "<<H<<endl; + cout<<"末端姿态为:"<<fai1<<" "<<fai2<<" "<<fai3<<endl; + cout<<"转动弧度为:"<<delta1<<" "<<delta2<<" "<<delta3<<endl; + cout<<"转动角度为:"<<delta1*180/pi<<" "<<delta2*180/pi<<" "<<delta3*180/pi<<endl; + */ } + +} +void robo::croute(double a) +{ + if(a<0||a>1){ +// cout<<"WRONG!"<<endl; + } + else + { + cout<<endl; + cout<<endl; + // cout<<"开始曲线规划!"<<endl; + double alpha; + double r=sqrt((X2-X1)*(X2-X1)+(Y2-Y1)*(Y2-Y1))/2; + if((X1-X2)!=0) + { + if(Y1>Y2) + { + if(X1>X2) + { + alpha=pi+atan((Y1-Y2)/(X1-X2)); + } + else + { + alpha=2*pi+atan((Y1-Y2)/(X1-X2)); + } + } + else + { + if(X1<X2) + { + alpha=atan((Y1-Y2)/(X1-X2)); + } + else + { + alpha=pi+atan((Y1-Y2)/(X1-X2)); + } + } + } + else + { + if(Y1>Y2) + { + alpha=3*pi/2; + } + else + { + alpha=pi/2; + } + } + // cout<<"夹角参数为:"<<alpha<<endl; + double xita=a*pi; + if(alpha>=0&&alpha<=(pi/2)) + { + x=(X1+X2)/2-r*cos(xita-alpha); + y=(Y1+Y2)/2+r*sin(xita-alpha); + z=Z1; + } + else + { + x=(X1+X2)/2-r*cos(xita+alpha); + y=(Y1+Y2)/2-r*sin(xita+alpha); + z=Z1; + } + if(x>-0.00000001&&x<0.00000001) + { + x=0; + } + // cout<<"圆弧直角坐标为:"<<x<<" "<<y<<" "<<z<<" "<<endl; + fai=argcalculate(x,y); + double p2=x*x+y*y; + P=sqrt(p2); + H=z; + double p2_h2=P*P+H*H;//计算末端三个角度参数 + double sqrT=sqrt(p2_h2); + double a1=(A*A+P*P+H*H-B*B)/(2*A*sqrT); + double a2=P/sqrT; + double b1=(A*A+P*P+H*H-B*B)/(2*A*sqrT); + double b2=(A*A-B*B-P*P-H*H)/(2*B*sqrT); + fai2=pi-asin(a1)-asin(a2); + fai3=asin(b1)-asin(b2); + fai1=fai; + delta1=fai1-faia1; + delta2=fai2-faia2; + delta3=fai3-faia3; + /* cout<<"运动百分比:"<<a*100<<"%"<<endl; + cout<<"中间圆弧位置为:"<<P<<" "<<fai<<" "<<H<<endl; + cout<<"中间姿态为:"<<fai1<<" "<<fai2<<" "<<fai3<<endl; + cout<<"转动角度为:"<<delta1<<" "<<delta2<<" "<<delta3<<endl; + */ } +} + +void robo::dexcute(double *p) +{ + double i=0; + int j = 0; + while(i<=1) + { + droute(i); + //myServo1.rotate(fai1/pi*180); + //myServo2.rotate(fai2/pi*180); + //myServo3.rotate(fai3/pi*180); + //wait(0.5); + p[j]=delta1; + p[j+1]=delta2; + p[j+2]=delta3; + j = j + 3; + + i=i+0.1; + // cout<<"正在执行步数:"<<i*10<<endl; + } +} +void robo::cexcute(double *p) +{ + double i=0; + int j = 0; + while(i<=1) + { + croute(i); + // double deg1=fai1/pi*180; + //myServo1.rotate(delta/pi*180); + //myServo2.rotate(fai2/pi*180); + //myServo3.rotate(fai3/pi*180); + //wait(0.5); +// cout<<"正在执行步数:"<<i*10<<endl; + p[j]=delta1; + p[j+1]=delta2; + p[j+2]=delta3; + j = j + 3; + + i=i+0.1; + +// cout<<"已旋转角度:"<<deg1<<endl; + } +}