Diff: Omni/Omni.cpp

Revision:

0:5d1c1999d61d

diff -r 000000000000 -r 5d1c1999d61d Omni/Omni.cpp
--- /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