Diff: robo.h

Revision:

4:d66b6f331b74

Parent:

3:09f6061e6e5d

--- 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&&deg1<0.00000001)
-    {
-        deg1=0;
-        if(deg2>-0.00000001&&deg2<0.00000001)
-        {
-            deg2=0;
-        }
-    }
-    else if (deg2>-0.00000001&&deg2<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;
-    }
-}