Revision 0:2e00b81c9137, committed 2018-05-11

Comitter:

Bilybill

Date:

Fri May 11 06:06:10 2018 +0000

Commit message:

afd

Changed in this revision

diff -r 000000000000 -r 2e00b81c9137 Stepper_motor.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Stepper_motor.cpp	Fri May 11 06:06:10 2018 +0000
@@ -0,0 +1,64 @@
+#include "Stepper_motor.h"
+
+Stepper_motor::Stepper_motor(PinName _en,PinName _stepPin,PinName _dirPin,PinName Inter,int _ratio,int _micorstep,int _dir,float _rec):Stepper(_en,_stepPin,_dirPin),InterruptIn(Inter)
+{
+    ratio = _ratio;
+    microstep = _micorstep;
+    dir = _dir;
+    Ori_rec = _rec;
+    pos_dir = 1;
+    this->enable();
+}
+
+void Stepper_motor::Config(float rec_rate,float rec)
+{
+    if(rec < 0 && rec != -1)
+    {
+        rec_rate = 0 - rec_rate;
+        rec = 0 - rec;
+    }
+    long long int frequency = rec_rate * ratio * microstep / Ori_rec;
+    if(frequency < 0)
+    {
+        dir = 1 - pos_dir;
+        frequency = 0 - frequency;
+    }
+    else if(frequency == 0)
+    {
+        this->disable();
+        return;
+    }
+    else if(frequency > 0)
+    {
+        dir = pos_dir;
+    }
+    long long int remain = rec * ratio * microstep / Ori_rec;
+    this->enable();
+    this->step(dir,frequency,remain);
+    
+}
+
+int Stepper_motor::getDir()
+{
+    return this->dir;
+}
+void Stepper_motor::reConfig(float _rec)
+{   
+    dir = 1 - dir;
+    long long int frequency = 5 * ratio * microstep / Ori_rec;
+    long long int remain = _rec * ratio * microstep / Ori_rec;
+    this->enable();
+    this->step(dir,frequency,remain);
+}
+void Stepper_motor::set()
+{
+    this -> Config(0,0);
+    wait(0.01);
+    this -> enable();
+    this -> Config(-5,7);
+}
+void Stepper_motor::Init()
+{
+    this->Config(10,-1);
+    this->fall(this,&Stepper_motor::set); 
+}
\ No newline at end of file

diff -r 000000000000 -r 2e00b81c9137 Stepper_motor.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Stepper_motor.h	Fri May 11 06:06:10 2018 +0000
@@ -0,0 +1,23 @@
+#ifndef _Stepper_motor_H
+#define _Stepper_motor_H
+#include "a4988.h"
+#include "mbed.h"
+class Stepper_motor:public Stepper,public InterruptIn
+{
+public:
+    Stepper_motor(PinName _en,PinName _stepPin,PinName _dirPin,PinName Inter,int _ratio,int _micorstep,int _dir,float _rec=1.8);
+    void Config(float rec_rate,float rec);
+    void reConfig(float _rec);
+    int getDir();
+    int pos_dir;
+    int neg_dir;
+    void Init();
+private:
+    void set();
+    int ratio;
+    int microstep;
+    int dir;
+    float Ori_rec;
+};
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r 2e00b81c9137 a4998.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/a4998.lib	Fri May 11 06:06:10 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Bilybill/code/stepper/#88f743f3e7a7

diff -r 000000000000 -r 2e00b81c9137 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri May 11 06:06:10 2018 +0000
@@ -0,0 +1,14 @@
+#include "mbed.h"
+#include "Stepper_motor.h"
+#include "robo.h"
+Serial Info(PB_10,PB_11);
+Ticker t1;
+Stepper_motor s1(PB_9,PB_0,PB_7,PA_11,10,16,1);
+Stepper_motor s2(PB_6,PA_7,PB_4,PA_15,10,16,1);
+Stepper_motor s3(PB_3,PA_6,PA_12,PB_1,10,16,1);
+int main()
+{
+    s1.Init();
+    s2.Init();
+    s3.Init();//电机初始化
+}

diff -r 000000000000 -r 2e00b81c9137 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri May 11 06:06:10 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/5aab5a7997ee
\ No newline at end of file

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&&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;
+    }
+}