麦克纳姆轮控制算法

麦克纳伦控制原理和方法详解及C语言代码实现控制车底盘。

//时间：2015-5-3
//作者：0721的
//描述：RM战车2号程序
//QQ  :825470791
#include “main.h“
extern unsigned char sbus_rx_buffer[25];
extern volatile  int sbus_rx_buffer_2[25];
extern unsigned int ANGLE_L; 
extern struct RC_Ctl_t RC_Ctl;
int current_v_201=0target_v_201=0current_v_203=0target_v_203=0;  
int current_p_201=0target_p_201=0current_p_203=0target_p_203=0;  
int xian_wei_y=0xian_wei_p=0;
int32_t L=250W=200R=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 tiao_bian;
char ALLOW_LAUNCH=0;
char ALLOW_DBUS=0; 
char motor1_Direction;            //0或者1       四个轮子方向
char motor2_Direction;            //0或者1
char motor3_Direction;            //0或者1
char motor4_Direction;            //0或者1 
int pwm_data1=0pwm_data2=0;               //电机1输出     pwm_data_nint pwm_data3=0pwm_data4=0;               //电机2的输出  
int pwm_launch=20;                             //发射电机
int32_t W1=0W2=0W3=0W4=0;                   //四个电机的转速单位mm/s

int CH0=1024CH1=1024CH2=1024CH3=1024S1=0S2=0XX=0YY=0ZZ=0;    //通道数据寄存
void MOTOR_GPIO_Configuration（void）;
void Encoder_Configuration（void）;
void CH_TO_SPEED（void）;
void CH_TO_CLOUD（void）;
int main（void）
{
  MOTOR_GPIO_Configuration（）;
  //PWM_Configuration（）;      /*-PWM1---（PA9---TIM1_CH2）--PWM2---（PA8---TIM1_CH1）--*/
	TIM6_Configuration（）;
	CAN1_Configuration（）;     /*----CAN1_TX-----PA12----CAN1_RX-----PA11----*/
	//CAN2_Configuration（）;     /*----CAN2_TX-----PB13----CAN2_RX-----PB12----*/
	USART2_Configuration（）;   /*-----USART2_RX-----PA3-*/ //for D-BUS
	USART3_Configuration（）;   /*-----USART3_TX-----PB10----USART3_RX-----PB11-----*/
	//Encoder_Configuration（）;  //TIM3 TIM5 connect to encodera-b
														//TIM3_CH1 ----- PC6//TIM3_CH2 ----- PC7
														//TIM5_CH1 ----- PA0//TIM5_CH2 ----- PA1
	TIM6_Start（）;              

    while（1）
    { 
			
			if（ALLOW_DBUS==0）        //若采集完成进行使用，不许采集
			{  
				CH_TO_CLOUD（）;
				CH_TO_SPEED（）;  
				ALLOW_DBUS=1;
			} 
			
    }
}


void CH_TO_SPEED（void）
	{			


	
	       if（CH0<364）CH0=364;if（CH0>1684）CH0=1684;      //检验
	       if（CH1<364）CH1=364;if（CH1>1684）CH1=1684;
	       if（CH2<364）CH2=364;if（CH2>1684）CH2=1684;
	       if（CH3<364）CH3=364;if（CH3>1684）CH3=1684; 
	
	       if（CH1<=1684&&CH1>=1024）        //CH2战车右直走
				 {  		 
					 Y_speed=CH1-1024;
					 right_or_left=1;
				 }
				 
	       else if（CH1<1024&&CH1>=364）         //CH2战车左直走
				 {  
					 Y_speed=1024-CH1; 
					 right_or_left=-1;
				 }				 
				 
	       if（CH0<=1684&&CH0>=1024）         //CH3战车前进
				 {  
					 X_speed=CH0-1024; 
					 forward_or_back=1;
				 }				 
					
	       else if（CH0<1024&&CH0>=364）         //CH3战车后退
				 {   
					 X_speed=1024-CH0;
					 forward_or_back=-1;
				 }
				 
				 if（（CH2<824）||（CH2>1224））     //YAW轴角度定义    更改区
				 { 
					 

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2019-01-14 19:45  51ºÚÂÛ̳_Âó¿ËÄÉÄ·ÂÖ×î¼òµ¥¿ØÖÆ×ÊÁÏ\
     目录           0  2018-05-20 03:24  51ºÚÂÛ̳_Âó¿ËÄÉÄ·ÂÖ×î¼òµ¥¿ØÖÆ×ÊÁÏ\麦克纳姆轮最简单控制资料\
     文件     1528540  2018-05-20 03:06  51ºÚÂÛ̳_Âó¿ËÄÉÄ·ÂÖ×î¼òµ¥¿ØÖÆ×ÊÁÏ\麦克纳姆轮最简单控制资料\IMG_20150528_225403_LI.jpg
     文件        5931  2018-05-20 03:12  51ºÚÂÛ̳_Âó¿ËÄÉÄ·ÂÖ×î¼òµ¥¿ØÖÆ×ÊÁÏ\麦克纳姆轮最简单控制资料\main.c
     文件     1020882  2015-04-14 00:46  51ºÚÂÛ̳_Âó¿ËÄÉÄ·ÂÖ×î¼òµ¥¿ØÖÆ×ÊÁÏ\麦克纳姆轮最简单控制资料\Mecanum轮逆矩阵研究_刘磊.pdf
     文件        2051  2018-05-20 03:26  51ºÚÂÛ̳_Âó¿ËÄÉÄ·ÂÖ×î¼òµ¥¿ØÖÆ×ÊÁÏ\麦克纳姆轮最简单控制资料\程序中的重要部分截取.txt
     目录           0  2018-05-20 03:24  麦克纳姆轮最简单控制资料\
     文件     1528540  2018-05-20 03:06  麦克纳姆轮最简单控制资料\IMG_20150528_225403_LI.jpg
     文件        5931  2018-05-20 03:12  麦克纳姆轮最简单控制资料\main.c
     文件     1020882  2015-04-14 00:46  麦克纳姆轮最简单控制资料\Mecanum轮逆矩阵研究_刘磊.pdf
     文件        2051  2018-05-20 03:26  麦克纳姆轮最简单控制资料\程序中的重要部分截取.txt