File content as of revision 0:5d1c1999d61d:

#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);
}