Keegan Hu
/
finalwarehouse
ddd
Fork of whouse by
robo.h
- Committer:
- glintligo
- Date:
- 2018-05-11
- Revision:
- 3:09f6061e6e5d
- Parent:
- 0:2e00b81c9137
File content as of revision 3:09f6061e6e5d:
#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; } }