Keegan Hu
/
finalwarehouse
ddd
Fork of whouse by
Diff: robo.h
- Revision:
- 4:d66b6f331b74
- Parent:
- 3:09f6061e6e5d
diff -r 09f6061e6e5d -r d66b6f331b74 robo.h --- a/robo.h Fri May 11 10:42:09 2018 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,386 +0,0 @@ -#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; - } -}