Revision 0:5d1c1999d61d, committed 2018-05-07

Comitter:

himarsmty

Date:

Mon May 07 06:58:54 2018 +0000

Commit message:

s

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor_3/Motor_3.cpp	Mon May 07 06:58:54 2018 +0000
@@ -0,0 +1,60 @@
+#include "Motor_3.h"
+#include "mbed.h"
+#define HIGH 1
+#define LOW 0
+
+extern Serial pc;
+Motor_3::Motor_3(PinName dia1,PinName dia2,PinName pwa):
+    _dia1(dia1),_dia2(dia2),_pwa(pwa),_pwmout(0)
+{
+    //init
+    DigitalOut pda1(_dia1,LOW);
+    DigitalOut pda2(_dia2,LOW);
+}
+
+void Motor_3::mv(double speed)
+{
+//    speed=speed/100*20/1000;
+//    pc.printf("speed=%lf\n",speed);
+
+    if(speed>0)
+    {
+         double temp_pwmout=6*pow(10.0,-10.0)*pow(speed,4.0)-4*pow(10.0,-7.0)*pow(speed,3.0)+3*pow(10.0,-5.0)*pow(speed,2.0)+0.0241*speed+0.0794;//拟合
+//         pc.printf("&&&temp_pwmout:%lf\n",temp_pwmout);
+        int pwmout=(int)temp_pwmout;
+         DigitalOut mydia1(_dia1,HIGH);
+         DigitalOut mydia2(_dia2,LOW);
+//        _pwmout=pwmout;
+        _pwmout=20;
+        PwmOut mypwa(_pwa);
+        mypwa.period_ms(20);
+        mypwa.pulsewidth_ms(_pwmout);
+    }
+    else if (speed<0)
+    {
+       
+        double temp_pwmout=6*pow(10.0,-10.0)*pow(speed,4.0)-4*pow(10.0,-7.0)*pow(speed,3.0)+3*pow(10.0,-5.0)*pow(speed,2.0)+0.0241*speed+0.0794;//拟合
+//        pc.printf("temp_pwmout:%lf\n",temp_pwmout);
+        int pwmout=(int)temp_pwmout;        
+        speed=abs(speed);
+        DigitalOut mydia1(_dia1,LOW);
+        DigitalOut mydia2(_dia2,HIGH);
+//        _pwmout=pwmout;
+        _pwmout=10;
+        pc.printf("%d\n",_pwmout);
+        PwmOut mypwa(_pwa);
+        mypwa.period_ms(20);
+        mypwa.pulsewidth_ms(_pwmout); 
+    }
+    else 
+    {
+        speed=abs(speed);
+        DigitalOut mydia1(_dia1,LOW);
+        DigitalOut mydia2(_dia2,LOW);
+        _pwmout=0.0;
+        PwmOut mypwa(_pwa);
+        mypwa.period_ms(20);
+        mypwa.pulsewidth(_pwmout); 
+    }
+        
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor_3/Motor_3.h	Mon May 07 06:58:54 2018 +0000
@@ -0,0 +1,20 @@
+#ifndef MOTOR_3_H_
+#define MOTOR_3_H_
+#include "mbed.h"
+
+DigitalOut mydia1(PinName a1,int b1);
+DigitalOut mydia2(PinName a2,int b2);
+PwmOut mypwa(PinName pa);
+
+
+class Motor_3{
+public:
+    Motor_3(PinName dia1,PinName dia2,PinName pwa);
+    void mv(double speed);
+    int _pwmout;
+protected:
+    PinName _dia1;
+    PinName _dia2;
+    PinName _pwa;
+};
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Omni/Omni.cpp	Mon May 07 06:58:54 2018 +0000
@@ -0,0 +1,59 @@
+#include "Omni.h"
+
+extern Serial pc;
+
+double AFA=60.0;//轮间夹角为120度
+double L=10.0; //轮距离中心的半径
+double theta=0.0;//相对坐标系和绝对坐标系的夹角为0
+
+double costheta1=(AFA + theta) / 180.0*3.141592;
+double sintheta1=(theta + AFA) / 180.0*3.141592;
+double costheta2=theta / 180.0*3.141592;
+double sintheta2=theta /180.0*3.141592;
+double costheta3=(AFA - theta) / 180.0*3.141592;
+double sintheta3=(AFA - theta) / 180.0*3.141592;
+//Motor_3 motor1(PinName di1,PinName di2,PinName pwmout);
+Motor_3 motor1(PB_15,PB_14,PA_7);
+Motor_3 motor2(PA_12,PA_15,PB_0);
+Motor_3 motor3(PB_3,PB_4,PB_1);
+typedef struct 
+{
+    double v1;
+    double v2;
+    double v3;
+}Wheel;
+
+Wheel Omni3_Control(double Vx,double Vy,double argular)
+{
+    Wheel tempwheel;
+    tempwheel.v1 = (-cos(costheta1) * Vx - sin(sintheta1) * Vy + L *argular);
+    tempwheel.v2 = (cos(costheta2) * Vx + sin(sintheta2) * Vy + L * argular);
+    tempwheel.v3 = (-cos(costheta3) * Vx + sin(sintheta3) * Vy + L * argular);
+    return tempwheel;
+}
+
+void Omni::mv_x(double speed)
+{
+    this->_Vx=speed;
+    this->_Vy=0;
+    this->_argular=0;
+    Wheel wheel_group;
+    wheel_group=Omni3_Control(this->_Vx,this->_Vy,this->_argular);
+    pc.printf("wheel.v1:%f\n",wheel_group.v1);
+    pc.printf("wheel.v2:%f\n",wheel_group.v2);
+    pc.printf("wheel.v3:%f\n",wheel_group.v3);
+    motor1.mv(wheel_group.v1);//脉宽
+    motor2.mv(wheel_group.v2);
+    motor3.mv(wheel_group.v3);
+}
+void Omni::mv_y(double speed)
+{
+    this->_Vx=0;
+    this->_Vy=speed;
+    this->_argular=0;
+    Wheel wheel_group;
+    wheel_group=Omni3_Control(this->_Vx,this->_Vy,this->_argular);
+    motor1.mv(wheel_group.v1);//脉宽
+    motor2.mv(wheel_group.v2);
+    motor3.mv(wheel_group.v3);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Omni/Omni.h	Mon May 07 06:58:54 2018 +0000
@@ -0,0 +1,16 @@
+#ifndef OMNI_H_
+#define OMNI_H_
+#include "Motor_3.h"
+#include "mbed.h"
+#include "math.h"
+class Omni{
+public:
+    Omni():_Vx(0),_Vy(0),_argular(0){}
+    void mv_x(double speed);
+    void mv_y(double speed);
+protected:
+    double _Vx;
+    double _Vy;
+    double _argular;
+};
+#endif;
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Mon May 07 06:58:54 2018 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon May 07 06:58:54 2018 +0000
@@ -0,0 +1,32 @@
+#include "Omni.h"
+#include "QEI.h"
+Serial pc(PB_10,PB_11);
+
+QEI wheel1(PA_2, PA_3, NC, 11, QEI::X4_ENCODING);
+QEI wheel2(PA_9, PA_10, NC, 11, QEI::X4_ENCODING);
+QEI wheel3(PA_0, PA_1, NC, 11, QEI::X4_ENCODING);
+Ticker toggle_time_ticker;
+int get_v1,get_v2,get_v3;
+void time_ticker();
+int main()
+{
+    Omni my_omni;
+    my_omni.mv_x(600);
+    wheel1.reset();
+      wheel2.reset();
+        wheel3.reset();
+//    w1 = 0;
+//  w2 = 1;
+           toggle_time_ticker.attach(&time_ticker, 0.2);
+
+
+}
+void time_ticker(){
+    get_v1 = wheel1.getPulses();
+    get_v2=wheel2.getPulses();
+    get_v3=wheel3.getPulses();
+        wheel1.reset();
+      wheel2.reset();
+        wheel3.reset();
+  pc.printf("%d      %d         %d\n ",get_v1,get_v2,get_v3);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon May 07 06:58:54 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/5aab5a7997ee
\ No newline at end of file