Biy Bill
asdf
Diff: robo.h
- Revision:
- 0:2e00b81c9137
--- /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;
+ }
+}