tianyun ma / Mbed 2 deprecated omni

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   QEI mbed

Omni/Omni.cpp@0:5d1c1999d61d, 2018-05-07 (annotated)

Committer:
himarsmty
Date:
Mon May 07 06:58:54 2018 +0000
Revision:
0:5d1c1999d61d
s

Who changed what in which revision?

UserRevisionLine numberNew contents of line
himarsmty 0:5d1c1999d61d 1 #include "Omni.h"
himarsmty 0:5d1c1999d61d 2
himarsmty 0:5d1c1999d61d 3 extern Serial pc;
himarsmty 0:5d1c1999d61d 4
himarsmty 0:5d1c1999d61d 5 double AFA=60.0;//轮间夹角为120度
himarsmty 0:5d1c1999d61d 6 double L=10.0; //轮距离中心的半径
himarsmty 0:5d1c1999d61d 7 double theta=0.0;//相对坐标系和绝对坐标系的夹角为0
himarsmty 0:5d1c1999d61d 8
himarsmty 0:5d1c1999d61d 9 double costheta1=(AFA + theta) / 180.0*3.141592;
himarsmty 0:5d1c1999d61d 10 double sintheta1=(theta + AFA) / 180.0*3.141592;
himarsmty 0:5d1c1999d61d 11 double costheta2=theta / 180.0*3.141592;
himarsmty 0:5d1c1999d61d 12 double sintheta2=theta /180.0*3.141592;
himarsmty 0:5d1c1999d61d 13 double costheta3=(AFA - theta) / 180.0*3.141592;
himarsmty 0:5d1c1999d61d 14 double sintheta3=(AFA - theta) / 180.0*3.141592;
himarsmty 0:5d1c1999d61d 15 //Motor_3 motor1(PinName di1,PinName di2,PinName pwmout);
himarsmty 0:5d1c1999d61d 16 Motor_3 motor1(PB_15,PB_14,PA_7);
himarsmty 0:5d1c1999d61d 17 Motor_3 motor2(PA_12,PA_15,PB_0);
himarsmty 0:5d1c1999d61d 18 Motor_3 motor3(PB_3,PB_4,PB_1);
himarsmty 0:5d1c1999d61d 19 typedef struct
himarsmty 0:5d1c1999d61d 20 {
himarsmty 0:5d1c1999d61d 21 double v1;
himarsmty 0:5d1c1999d61d 22 double v2;
himarsmty 0:5d1c1999d61d 23 double v3;
himarsmty 0:5d1c1999d61d 24 }Wheel;
himarsmty 0:5d1c1999d61d 25
himarsmty 0:5d1c1999d61d 26 Wheel Omni3_Control(double Vx,double Vy,double argular)
himarsmty 0:5d1c1999d61d 27 {
himarsmty 0:5d1c1999d61d 28 Wheel tempwheel;
himarsmty 0:5d1c1999d61d 29 tempwheel.v1 = (-cos(costheta1) * Vx - sin(sintheta1) * Vy + L *argular);
himarsmty 0:5d1c1999d61d 30 tempwheel.v2 = (cos(costheta2) * Vx + sin(sintheta2) * Vy + L * argular);
himarsmty 0:5d1c1999d61d 31 tempwheel.v3 = (-cos(costheta3) * Vx + sin(sintheta3) * Vy + L * argular);
himarsmty 0:5d1c1999d61d 32 return tempwheel;
himarsmty 0:5d1c1999d61d 33 }
himarsmty 0:5d1c1999d61d 34
himarsmty 0:5d1c1999d61d 35 void Omni::mv_x(double speed)
himarsmty 0:5d1c1999d61d 36 {
himarsmty 0:5d1c1999d61d 37 this->_Vx=speed;
himarsmty 0:5d1c1999d61d 38 this->_Vy=0;
himarsmty 0:5d1c1999d61d 39 this->_argular=0;
himarsmty 0:5d1c1999d61d 40 Wheel wheel_group;
himarsmty 0:5d1c1999d61d 41 wheel_group=Omni3_Control(this->_Vx,this->_Vy,this->_argular);
himarsmty 0:5d1c1999d61d 42 pc.printf("wheel.v1:%f\n",wheel_group.v1);
himarsmty 0:5d1c1999d61d 43 pc.printf("wheel.v2:%f\n",wheel_group.v2);
himarsmty 0:5d1c1999d61d 44 pc.printf("wheel.v3:%f\n",wheel_group.v3);
himarsmty 0:5d1c1999d61d 45 motor1.mv(wheel_group.v1);//脉宽
himarsmty 0:5d1c1999d61d 46 motor2.mv(wheel_group.v2);
himarsmty 0:5d1c1999d61d 47 motor3.mv(wheel_group.v3);
himarsmty 0:5d1c1999d61d 48 }
himarsmty 0:5d1c1999d61d 49 void Omni::mv_y(double speed)
himarsmty 0:5d1c1999d61d 50 {
himarsmty 0:5d1c1999d61d 51 this->_Vx=0;
himarsmty 0:5d1c1999d61d 52 this->_Vy=speed;
himarsmty 0:5d1c1999d61d 53 this->_argular=0;
himarsmty 0:5d1c1999d61d 54 Wheel wheel_group;
himarsmty 0:5d1c1999d61d 55 wheel_group=Omni3_Control(this->_Vx,this->_Vy,this->_argular);
himarsmty 0:5d1c1999d61d 56 motor1.mv(wheel_group.v1);//脉宽
himarsmty 0:5d1c1999d61d 57 motor2.mv(wheel_group.v2);
himarsmty 0:5d1c1999d61d 58 motor3.mv(wheel_group.v3);
himarsmty 0:5d1c1999d61d 59 }