asdf

Dependencies:   stepper mbed

Files at this revision

API Documentation at this revision

Comitter:
Bilybill
Date:
Fri May 11 06:06:10 2018 +0000
Commit message:
afd

Changed in this revision

Stepper_motor.cpp Show annotated file Show diff for this revision Revisions of this file
Stepper_motor.h Show annotated file Show diff for this revision Revisions of this file
a4998.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
robo.h Show annotated file Show diff for this revision Revisions of this file
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;
+    }
+}