//地点：武夷学院机器人实验室

//作者：0721的

//描述：RM战车2号程序

//交流QQ:825470791

//轮子的安装请参考图片！！！！

int32_t L=250,W=200,R=75;                 //车尺寸，单位mm

int32_t X_speed=0;

int32_t Y_speed=0;

int32_t W_speed=0;

int32_t right_or_left=0;

int32_t forward_or_back=0;

int32_t W_right_or_left=0;

char motor1_Direction;            //0或者1       四个轮子方向

char motor2_Direction;            //0或者1

char motor3_Direction;            //0或者1

char motor4_Direction;            //0或者1

int32_t W1=0,W2=0,W3=0,W4=0;                   //四个电机的转速

int pwm_data1=0,pwm_data2=0;               //电机1、2输出

int pwm_data3=0,pwm_data4=0;               //电机3、4输出

W1=((forward_or_back)*X_speed*2-(right_or_left)*Y_speed*2-W*W_right_or_left*W_speed-L*W_speed)/R;        //矩阵运算得出W1,W2,W3,W4

W2=((forward_or_back)*X_speed*2+(right_or_left)*Y_speed*2-W*W_right_or_left*W_speed-L*W_speed)/R;

W3=((forward_or_back)*X_speed*2-(right_or_left)*Y_speed*2+W*W_right_or_left*W_speed+L*W_speed)/R;         //此处强烈建议参考文件夹中的论文中的

W4=((forward_or_back)*X_speed*2+(right_or_left)*Y_speed*2+W*W_right_or_left*W_speed+L*W_speed)/R;          //我打红圈的逆运动学公式，可能加减符号有问题！

if(W1>0)motor1_Direction=0;else motor1_Direction=1;//0为正转，1为反转

if(W2>0)motor2_Direction=0;else motor2_Direction=1;

if(W3>0)motor3_Direction=0;else motor3_Direction=1;

if(W4>0)motor4_Direction=0;else motor4_Direction=1;

pwm_data1=2*abs(W1);  //pwm为算出的占空比值

pwm_data2=2*abs(W2);

pwm_data3=2*abs(W3);

pwm_data4=2*abs(W4);

if(pwm_data1<0)pwm_data1=0;if(pwm_data1>75)pwm_data1=75;  //限值，万一算出来的值超过了你最大PWM允许的值需要限值的

if(pwm_data2<0)pwm_data2=0;if(pwm_data2>75)pwm_data2=75;  //我的是75，你的根据你的PWM思路自己掌握

if(pwm_data3<0)pwm_data3=0;if(pwm_data3>75)pwm_data3=75;

if(pwm_data4<0)pwm_data4=0;if(pwm_data4>75)pwm_data4=75;

//电机的正反转接线能否正确，靠你自己啦！！加油！！