Code for Mecanum Wheel car
最终目的为了完成工厂运输钢瓶的目的，现阶段想要实现一辆由手柄随动控制的AGV小车。
全方位运动平台通常装有全向轮：omni wheels（全向轮) 或 mecanum wheels（麦克纳姆轮)。借助于横向移动和原地回旋的特性，全方位运动平台可方便的穿梭于狭窄拥挤空间中，灵活完成各种任务，相比传统移动平台有明显优势。
全方位运动系统中，Mecanum轮应用最广泛。Mecanum轮(又称瑞典轮)是瑞典Mecanum AB公司工程师Bengt Ilron提出的特殊轮系，其特点为沿轮毅圆周排布着与轮子成一定角度且可绕自身轴线进行旋转的辊子。由三个或以上Mecanum 轮按照一定方式排列组成的移动平台具有平面内三个自由度，可同时独立的前后、左右和原地旋转运动，可在不改变自身姿态的情况下向任意方向移动。
其运动学本质是：轮毂外围安装一周与轮毂轴线呈一定角度的无动力辊子作为轮胎，该辊子不仅可绕轮毂轴公转，也能在地面摩擦力作用下绕各自的支撑芯轴自转，两种运动的合成使得接触地面的辊子中心合速度与轮毂轴有一定的夹角，通过调节轮毂速度可改变辊子中心合速度的大小和方向。由同样结构的若干Mecanum 轮按一定规则组成的轮组系统，通过改变各轮毂速度的线性组合，进而控制运动系统中心合速度大小和方向，使机器人实现平面3自由度全方位运动。由于其外观上与斜齿轮相似，麦克纳姆轮也有齿轮啮合时相类似的问题：为了保证运动的平稳性，当前一个辊子与地面即将分离时，后一个辊子必须与地面接触。
麦克纳姆轮优点和缺点都是非常明确。采用麦克纳姆轮的车子大都移动异常灵活，机动性能非常好；但是这种轮胎的越野性能却非常差，跨越障碍的能力甚至不如普通轮胎，特别是当坡度较大的时候，甚至还会溜坡。所以配备这种车胎的设备大多是一些场地竞技机器人和室内仓储机器人等。
单位：mm
系统初始化主要分为两个部分：
系统时钟初始化
系统时钟设置为72M.
延时初始化
SysTick->LOAD为24位寄存器，所以，最大延时为：nms<=0xffffff*8*1000/SYSCLK，SYSCLK 单位为Hz，nms 单位为 ms。对 72M 条件下，nms <= 1864ms。
由于板载设计接口的限制，小车禁用 JTAG 调试接口，使能 SWD 调试接口。小车可以利用主板的 SWD 接口调试。
蓝色LED灯连接 PB13 引脚， 红色LED灯常亮。红色电源指示灯亮起，表示系统供电正常。蓝灯闪烁，表示单片机工作正常。
小车一共有三个按键。
复位按键 RESET
复位单片机。
模式选择开关
对应引脚 PB.12，小车启动后，会检查开关的状态，判断进入速度模式还是位置模式。小车启动后，改变模式选择开关，需要重新启动后才能生效。
参数读取按键
对应引脚 PB.14，小车启动后，会根据参数读取按键的状态选择是否读取FLASH中的PID参数。如果在启动时长按参数读取按键两秒，小车将会读取FLASH中的PID参数。
OLED显示屏可以显示小车的信息，如下图所示：
小车上一共使用了三个串口，USART1, USART2 和 USART3。其中 USART2 引出了蓝牙接口，默认接收蓝牙遥控指令。USART1 和 USART3 用于接收里程计数据或者发送控制指令，这两个串口具备同样的功能，我们可以任意选择一个串口使用，波特率都为115200。不能同时向这两个发送控制指令，不然会有异常，但是可以同时从这两个串口接收里程计数据。虽然这两个串口功能一样，但是 USART1 和 USART3 有一些细微的差别：
默认小车通过 USART2 接收蓝牙遥控指令，如果其他的设备要通过串口对小车进行控制，需要先使能串口控制。为了降低使用难度，串口使能机制比较简单，小车只要超过10次进入串口接收中断即可使能串口接收标志位，也就是说，向小车发送10个任意字节即可使能串口控制。此时，我们就可以使用其他的单片机，比如Arduino，通过串口发送指令对小车进行控制了，波特率是115200。判断串口已经使能成功的一个标志是如果小车不再接收 USART2 连接的蓝牙的控制指令，即可说明串口已经使能。2个串口对任意-一个使能，另外一个也会跟着使能。但是串口（包括 CAN）接收里程计数据不需要使能，上电就可以接收。
Mode=1 时，可对小车进行坐标控制，只需输入每个坐标轴的速度量，小车可以自行进行运动学分析。
tx[1] 和 tx[2] 合成16位无符号数控制 X 轴的速度大小
tx[3] 和 tx[4] 合成16位无符号数控制 Y 轴的速度大小
tx[5] 和 tx[6] 合成16位无符号数控制 Z 轴的速度大小
tx[7] 是方向控制位，由低3位分别控制三个轴的方向
Mode=2 时，可对每个电机单独进行速度闭环控制。
tx[1] 控制A电机速度的大小
tx[2] 控制B电机速度的大小
tx[3] 控制C电机速度的大小
tx[4] 控制D电机速度的大小
tx[7] 是方向控制位，由低4位分别控制四个电机的方向
Mode=1 时，可对小车进行坐标控制，只需输入每个坐标轴的位移量，小车可以自行进行运动学分析。
tx[1] 和 tx[2] 合成16位无符号数控制 X 轴位移量
tx[3] 和 tx[4] 合成16位无符号数控制 Y 轴位移量
tx[5] 和 tx[6] 合成16位无符号数控制 Z 轴位移量
tx[7] 是方向控制位，由低3位分别控制三个轴的方向
我们可以用 MicroUSB 的数据线将小车连接到电脑上，通过 USART1 给小车发送控制指令。可以很方便地通过 XCOM 串口调试助手进行测试：
这样相当于控制小车以 18(0x12) 的速度运动。下面解释一下这个 18 的单位，10ms 转 18(0x12) 个脉冲，1s 转1800个脉冲，车轮转一圈， 输出(编码器线数500 x 减速比27 x 4倍频) = 54000个脉冲，实际上因为光电编码器精度非常高，为了方便信号处理与传输，在代码里面已经除以了20 的，所以相当于车轮转一圈输出2700个脉冲，也就是每秒转2/3圈，再结合轮胎的直径信息，就可以得到小车的运行速度。
另外也可以通过 TTL 电平的 USART3 控制小车。比如我们使用另外一个 STM32 控制器对小车进行控制，发送10个字节就可以构成一个完整的控制命令：
Usart_Send(0xff);Usart_Send(0xfe);Usart_Send(0x01);Usart_Send(0x00);Usart_Send(0x00);Usart_Send(0x00);Usart_Send(0x12);Usart_Send(0x00);Usart_Send(0x00);Usart_Send(0x00);
这个指令的控制效果和刚才用电脑的发送指令一致。需要注意的是，发送的是10个字节，不是字符串。
通过小车的 USART1 和 USART3 可以采集IMU和里程计。可以两个串口同时接收，或者任选一个进行接收。一帧数据共有12个字节，其中0xff和0xfe是帧头，数据格式如下：
其中，速度大小的单位解释参考上一节（控制指令例程说明）。电机速度方向 tx[1], tx[3], tx[5], tx[7] 的数据解释为：
| 数据 | 含义 |
|---|---|
| 0 | 正方向 |
| 1 | 停止 |
| 2 | 负方向 |
Z轴陀螺仪的数据是+32768后再发过来的，所以，处理之后的真实Z轴陀螺仪的数据为：
\text { Gyro } Z=\operatorname{tx}[8] * 256+\operatorname{tx}[9]-32768
编码器接线说明（500线AB相输出）：
红色：VCC（3.3 ~ 5V）
黑色：GND
绿色：编码器A相输出（内置电阻上拉到VCC）
白色：编码器B相输出（内置电阻上拉到VCC）
我们只需给编码器电源5V供电，在电机转动的时候即可通过AB相输出方波信号，故可判断旋转方向。编码器自带了上拉电阻，所以无需外部上拉，可以直接连接到单片机IO通过外部中断读取。比如把编码器A相输出接到单片机的外部中断输入口，这样就可以通过跳变沿触发中断，然后在对应的外部中断服务函数里面，通过B相的电平来确定正反转
初始化代码：
/**************************************************************************函数功能：把TIM2初始化为编码器接口模式入口参数：无返 回 值：无**************************************************************************/void Encoder_Init_TIM2(void){// RCC->APB2ENR|=1<<0; //开启辅助时钟AFIO->MAPR|=1<<8; //01部分重映射RCC->APB1ENR|=1<<0; //TIM2时钟使能RCC->APB2ENR|=1<<2; //使能PORTA时钟RCC->APB2ENR|=1<<3; //使能PORTB时钟GPIOA->CRH&=0X0FFFFFFF; //PA15GPIOA->CRH|=0X40000000; //浮空输入GPIOB->CRL&=0XFFFF0FFF; //PB3GPIOB->CRL|=0X00004000; //浮空输入TIM2->DIER|=1<<0; //允许更新中断TIM2->DIER|=1<<6; //允许触发中断MY_NVIC_Init(1,3,TIM2_IRQn,1);TIM2->PSC = 0x0; //预分频器TIM2->ARR = ENCODER_TIM_PERIOD;//设定计数器自动重装值TIM2->CR1 &=~(3<<8); //选择时钟分频：不分频TIM2->CR1 &=~(3<<5); //选择计数模式：边沿对齐模式TIM2->CCMR1 |= 1<<0; //CC1S='01' IC1FP1映射到TI1TIM2->CCMR1 |= 1<<8; //CC2S='01' IC2FP2映射到TI2TIM2->CCER &= ~(1<<1); //CC1P='0' IC1FP1不反相，IC1FP1=TI1TIM2->CCER &= ~(1<<5); //CC2P='0' IC2FP2不反相，IC2FP2=TI2// TIM2->CCMR1 |= 3<<4; // IC1F='1000' 输入捕获1滤波器TIM2->SMCR |= 3<<0; //SMS='011' 所有的输入均在上升沿和下降沿有效TIM2->CR1 |= 0x01; //CEN=1，使能定时器}void Encoder_Init_TIM3(void);void Encoder_Init_TIM4(void);void Encoder_Init_TIM5(void); // 四个轮子对应四个编码器
参数读取函数：
/**************************************************************************函数功能：单位时间读取编码器计数入口参数：定时器返 回 值：速度值**************************************************************************/int Read_Encoder(u8 TIMX){int Encoder_TIM;switch(TIMX){case 2: Encoder_TIM= (short)TIM2 -> CNT; TIM2 -> CNT=0; break;case 3: Encoder_TIM= (short)TIM3 -> CNT; TIM3 -> CNT=0; break;case 4: Encoder_TIM= (short)TIM4 -> CNT; TIM4 -> CNT=0; break;case 5: Encoder_TIM= (short)TIM5 -> CNT; TIM5 -> CNT=0; break;default: Encoder_TIM=0;}return Encoder_TIM;}
中断函数：
/**************************************************************************函数功能：TIM2中断服务函数入口参数：无返 回 值：无**************************************************************************/void TIM2_IRQHandler(void){if(TIM2->SR&0X0001)//溢出中断{}TIM2->SR&=~(1<<0);//清除中断标志位}void TIM3_IRQHandler(void);void TIM4_IRQHandler(void);void TIM5_IRQHandler(void);
使用 PA.5 引脚，进行AD采样，读取电池电压，以便进行最低电压保护。
本车电机采用行星减速机，其优点是结构比较紧凑，回程间隙小、精度较高，使用寿命很长，额定输出扭矩可以做的很大，但价格略贵。
电机本身可直接通过调整电机两极的直流电压大小和极性实现调试和换向，但单片机IO的带负载能力较弱，而直流电机是大电流感性负载，所以我们需要功率放大器件。
电机初始化代码：
/**************************************************************************函数功能：电机控制所需IO初始化入口参数：无返 回 值：无**************************************************************************/void MiniBalance_Motor_Init(void){RCC->APB2ENR|=1<<3; //PORTB时钟使能RCC->APB2ENR|=1<<4; //PORTC时钟使能GPIOB->CRL&=0XFFFFFF00; //推挽输出GPIOB->CRL|=0X00000022; //推挽输出//GPIOB->ODR|=0<<0;//GPIOB->ODR|=0<<1;GPIOC->CRL&=0XFF00FFFF; //推挽输出GPIOC->CRL|=0X00220000; //推挽输出//GPIOC->ODR|=0<<4;//GPIOC->ODR|=0<<5;
PWM输出初始化代码：
/**************************************************************************函数功能：PWM波输出初始化入口参数：自动重装载值、预分频系数返 回 值：无**************************************************************************/void MiniBalance_PWM_Init(u16 arr,u16 psc){MiniBalance_Motor_Init(); //初始化电机控制所需IORCC->APB2ENR|=1<<13; //使能TIM8时钟RCC->APB2ENR|=1<<4; //PORTC时钟使能GPIOC->CRH&=0XFFFFFF00; //PORTC8复用输出GPIOC->CRH|=0X000000BB; //PORTC8复用输出GPIOC->CRL&=0X00FFFFFF; //PORTC6 7复用输出GPIOC->CRL|=0XBB000000; //PORTC6 7复用输出TIM8->ARR=arr; //设定计数器自动重装值TIM8->PSC=psc; //设定预分频器系数TIM8->CCMR1|=6<<4; //CH1 PWM1模式TIM8->CCMR1|=6<<12; //CH2 PWM1模式TIM8->CCMR2|=6<<4; //CH3 PWM1模式TIM8->CCMR2|=6<<12; //CH4 PWM1模式TIM8->CCMR1|=1<<3; //CH1预装载使能TIM8->CCMR1|=1<<11; //CH2预装载使能TIM8->CCMR2|=1<<3; //CH3预装载使能TIM8->CCMR2|=1<<11; //CH4预装载使能TIM8->CCER|=1<<0; //CH1输出使能TIM8->CCER|=1<<4; //CH2输出使能TIM8->CCER|=1<<8; //CH3输出使能TIM8->CCER|=1<<12; //CH4输出使能TIM8->BDTR |= 1<<15; //TIM必须要这句话才能输出PWM, main output enableTIM8->CR1=0x8000; //ARPE使能//stm32chinese p233 -->TIM8->CR1=0x0080??TIM8->CR1|=0x01; //使能定时器1}
当有按键按下时，对应位为0， 其他位为1， 例如当键 SELECT 被按下时，Data【3】= 11111110B。
在设置了震动模式后，就可以通过发送WW，YY 来控制震动电机。WW控制右侧的小震动电机，0x00关，其他值为开；YY用来控制左侧的大震动电机，0x40~0xFF电机开，值越大电机转动越快，震动越明显。
红灯模式时：左右摇杆发送模拟值，0x40~0xFF之间，且摇杆按下的键值L3、R3有效；
绿灯模式时：左右摇杆模拟值无效，推到极限时，对应发送UP、RIGHT，DOWN，LEFT，▲，⚪，×，口，按键L3，R3 无效。
接收器以软件模拟IIC的方式将手柄发来的数据传递给主机。
PS2手柄初始化代码：
/********************************************************函数功能：手柄接口初始化入口参数：无返回值：无*********************************************************/void PS2_Init(void){RCC->APB2ENR|=1<<2; //使能PORTA时钟GPIOA->CRL&=0XFFFF000F; //PA1 2 3推挽输出GPIOA->CRL|=0X00003330;GPIOA->CRL&=0XFFFFFFF0;GPIOA->CRL|=0X00000008;//PA0 设置成输入 默认下拉
主机向手柄发送命令：
/************************************************************函数功能：主机向手柄发送命令入口参数：CMD：命令数组返回值：无************************************************************/void PS2_Cmd(u8 CMD){volatile u16 ref=0x01;Data[1] = 0;for(ref=0x01;ref<0x0100;ref<<=1){if(ref&CMD){DO_H; //输出一位控制位}else DO_L;CLK_H; //时钟拉高DELAY_TIME;CLK_L;DELAY_TIME;CLK_H;if(DI)Data[1] = ref|Data[1];}delay_us(16);}
主机读取手柄数据：
/*********************************************************************函数功能：读取手柄数据入口参数：无返回值：无void PS2_ReadData(void){volatile u8 byte=0;volatile u16 ref=0x01;CS_L;PS2_Cmd(Comd[0]); //开始命令PS2_Cmd(Comd[1]); //请求数据for(byte=2;byte<9;byte++) //开始接受数据{for(ref=0x01;ref<0x100;ref<<=1){CLK_H;DELAY_TIME;CLK_L;DELAY_TIME;CLK_H;if(DI)Data[byte] = ref|Data[byte];}delay_us(16);}CS_H;}
CAN总线采用不归零码位填充技术，也就是说CAN总线上的信号有两种不同的信号状态，分别是显性的(Dominant)逻辑0和隐形的(recessive)逻辑1，信号每一次传输完后不需要返回到逻辑0(显性)的电平。
CAN收发器有TXD，RXD是与CAN控制器连接的。发送器接到网络的是CL和CH。CL与CH是差分电路。CAN网络上是用CL于CH的电压差来表示逻辑“0”和逻辑“1”。所以CAN网络中只能单向传输。
CAN控制器初始化：
/********************************************************函数功能：CAN1初始化入口参数：tsjw:重新同步跳跃时间单元.范围:1~3;tbs2:时间段2的时间单元.范围:1~8;tbs1:时间段1的时间单元.范围:1~16;brp :波特率分频器.范围:1~1024;(实际要加1,也就是1~1024) tq=(brp)*tpclk1注意以上参数任何一个都不能设为0,否则会乱.波特率=Fpclk1/((tbs1+tbs2+1)*brp);mode:0,普通模式;1,回环模式;Fpclk1的时钟在初始化的时候设置为36M,如果设置CAN1_Normal_Init(1,2,3,12,1);则波特率为: 36/(1+3+2)/12=500K *返回值:0,初始化OK;其他,初始化失败;********************************************************/u8 CAN1_Mode_Init(u8 tsjw,u8 tbs2,u8 tbs1,u16 brp,u8 mode){u16 i=0;if(tsjw==0||tbs2==0||tbs1==0||brp==0)return 1;tsjw-=1;//先减去1.再用于设置tbs2-=1;tbs1-=1;brp-=1;RCC->APB2ENR|=1<<2; //使能PORTA时钟GPIOA->CRH&=0XFFF00FFF;GPIOA->CRH|=0X000B8000;//PA11 RX,PA12 TX推挽输出GPIOA->ODR|=3<<11;RCC->APB1ENR|=1<<25;//使能CAN1时钟 CAN1使用的是APB1的时钟(max:36M)CAN1->MCR=0x0000; //退出睡眠模式(同时设置所有位为0)CAN1->MCR|=1<<0; //请求CAN1进入初始化模式while((CAN1->MSR&1<<0)==0){i++;if(i>100)return 2;//进入初始化模式失败}CAN1->MCR|=0<<7; //非时间触发通信模式CAN1->MCR|=0<<6; //软件自动离线管理CAN1->MCR|=0<<5; //睡眠模式通过软件唤醒(清除CAN1->MCR的SLEEP位)CAN1->MCR|=1<<4; //禁止报文自动传送CAN1->MCR|=0<<3; //报文不锁定,新的覆盖旧的CAN1->MCR|=0<<2; //优先级由报文标识符决定CAN1->BTR=0x00000000;//清除原来的设置.CAN1->BTR|=mode<<30; //模式设置 0,普通模式;1,回环模式;CAN1->BTR|=tsjw<<24; //重新同步跳跃宽度(Tsjw)为tsjw+1个时间单位CAN1->BTR|=tbs2<<20; //Tbs2=tbs2+1个时间单位CAN1->BTR|=tbs1<<16; //Tbs1=tbs1+1个时间单位CAN1->BTR|=brp<<0; //分频系数(Fdiv)为brp+1//波特率:Fpclk1/((Tbs1+Tbs2+1)*Fdiv)CAN1->MCR&=~(1<<0); //请求CAN1退出初始化模式while((CAN1->MSR&1<<0)==1){i++;if(i>0XFFF0)return 3;//退出初始化模式失败}//过滤器初始化CAN1->FMR|=1<<0; //过滤器组工作在初始化模式CAN1->FA1R&=~(1<<0); //过滤器0不激活CAN1->FS1R|=1<<0; //过滤器位宽为32位.CAN1->FM1R|=0<<0; //过滤器0工作在标识符屏蔽位模式CAN1->FFA1R|=0<<0; //过滤器0关联到FIFO0CAN1->sFilterRegister[0].FR1=0X00000000;//32位IDCAN1->sFilterRegister[0].FR2=0X00000000;//32位MASKCAN1->FA1R|=1<<0; //激活过滤器0CAN1->FMR&=0<<0; //过滤器组进入正常模式#if CAN1_RX0_INT_ENABLE//使用中断接收CAN1->IER|=1<<1; //FIFO0消息挂号中断允许.MY_NVIC_Init(1,0,USB_LP_CAN1_RX0_IRQn,2);//组2#endifreturn 0;}
CAN 发送数据：
/****************************************************************函数功能：通过CAN发送数据输入参数：id:标准ID(11位)/扩展ID(11位+18位)ide:0,标准帧;1,扩展帧rtr:0,数据帧;1,远程帧len:要发送的数据长度(固定为8个字节,在时间触发模式下,有效数据为6个字节)*dat:数据指针.返回值:0~3,邮箱编号.0XFF,无有效邮箱.******************************************************************/u8 CAN1_Tx_Msg(u32 id,u8 ide,u8 rtr,u8 len,u8 *dat){u8 mbox;if(CAN1->TSR&(1<<26))mbox=0; //邮箱0为空else if(CAN1->TSR&(1<<27))mbox=1; //邮箱1为空else if(CAN1->TSR&(1<<28))mbox=2; //邮箱2为空else return 0XFF; //无空邮箱,无法发送CAN1->sTxMailBox[mbox].TIR=0; //清除之前的设置if(ide==0) //标准帧{id&=0x7ff;//取低11位stdidid<<=21;}else //扩展帧{id&=0X1FFFFFFF;//取低32位extidid<<=3;}CAN1->sTxMailBox[mbox].TIR|=id;CAN1->sTxMailBox[mbox].TIR|=ide<<2;CAN1->sTxMailBox[mbox].TIR|=rtr<<1;len&=0X0F;//得到低四位CAN1->sTxMailBox[mbox].TDTR&=~(0X0000000F);CAN1->sTxMailBox[mbox].TDTR|=len; //设置DLC.//待发送数据存入邮箱.CAN1->sTxMailBox[mbox].TDHR=(((u32)dat[7]<<24)|((u32)dat[6]<<16)|((u32)dat[5]<<8)|((u32)dat[4]));CAN1->sTxMailBox[mbox].TDLR=(((u32)dat[3]<<24)|((u32)dat[2]<<16)|((u32)dat[1]<<8)|((u32)dat[0]));CAN1->sTxMailBox[mbox].TIR|=1<<0; //请求发送邮箱数据return mbox;}
CAN接收数据：
/***************************************************************函数功能：通过CAN接收数据输入参数：fifox:邮箱号id:标准ID(11位)/扩展ID(11位+18位)ide:0,标准帧;1,扩展帧rtr:0,数据帧;1,远程帧len:接收到的数据长度(固定为8个字节,在时间触发模式下,有效数据为6个字节)dat:数据缓存区*****************************************************************/void CAN1_Rx_Msg(u8 fifox,u32 *id,u8 *ide,u8 *rtr,u8 *len,u8 *dat){*ide=CAN1->sFIFOMailBox[fifox].RIR&0x04;//得到标识符选择位的值if(*ide==0)//标准标识符{*id=CAN1->sFIFOMailBox[fifox].RIR>>21;}else //扩展标识符{*id=CAN1->sFIFOMailBox[fifox].RIR>>3;}*rtr=CAN1->sFIFOMailBox[fifox].RIR&0x02; //得到远程发送请求值.*len=CAN1->sFIFOMailBox[fifox].RDTR&0x0F;//得到DLC//*fmi=(CAN1->sFIFOMailBox[FIFONumber].RDTR>>8)&0xFF;//得到FMI//接收数据dat[0]=CAN1->sFIFOMailBox[fifox].RDLR&0XFF;dat[1]=(CAN1->sFIFOMailBox[fifox].RDLR>>8)&0XFF;dat[2]=(CAN1->sFIFOMailBox[fifox].RDLR>>16)&0XFF;dat[3]=(CAN1->sFIFOMailBox[fifox].RDLR>>24)&0XFF;dat[4]=CAN1->sFIFOMailBox[fifox].RDHR&0XFF;dat[5]=(CAN1->sFIFOMailBox[fifox].RDHR>>8)&0XFF;dat[6]=(CAN1->sFIFOMailBox[fifox].RDHR>>16)&0XFF;dat[7]=(CAN1->sFIFOMailBox[fifox].RDHR>>24)&0XFF;if(fifox==0)CAN1->RF0R|=0X20;//释放FIFO0邮箱else if(fifox==1)CAN1->RF1R|=0X20;//释放FIFO1邮箱}
中断服务函数：
#if CAN1_RX0_INT_ENABLE //使能RX0中断//中断服务函数void USB_LP_CAN1_RX0_IRQHandler(void){u8 i;u32 id;u8 ide,rtr,len;CAN1_Rx_Msg(0,&id,&ide,&rtr,&len,rxbuf);if(id==0x100){for(i=0;i<8;i++){Rxbuf[i]=rxbuf[i];}}else if(id==0x101){for(i=8;i<16;i++){Rxbuf[i]=rxbuf[i-8];}}}#endif
机器人控制的基础是运动学分析，利用运动学分析可以得出机器人运动过程中各类参数的变化规律和相互之间的关系，然后通过C语言在控制器里面进行验证。在运动建模之前，为简化运动学数学模型，做下列几种理想化假设：
麦克纳姆轮不与地面打滑，同时地面有足够摩擦力。
4个轮子分布在长方形或者正方形的4个角上，轮子之间相互平行。
假定机体坐标系和地理坐标系重合，机器人运动方向做如下图所述的规定;
刚体运动可以线性分解为三个分量。那么只需计算输出麦轮底盘在沿X+，Y+方向平移，Z+方向转动时四个轮子的速度，就可以通过公式的合并，计算出这三种简单运动所合成的“平动+旋转”运动时所需要的四个轮子的转速。
其中 $V{A}, V{B}, V{C}, V{D}$ 分别为 $A, B, C, D$ 四个轮子的转速，也就是电机的转速。$V{x}$ 为小车沿着 $X$ 轴平移速度, $V{y}$ 为小车沿着 $Y$ 轴平移速度，$\omega$ 为小车沿着 $Z$ 轴旋转速度。
当小车沿着X轴平移时：
\begin{array}{l}{V{A}=-V{x}} \ {V{B}=+V{x}} \ {V{C}=-V{x}} \ {V{D}=+V{x}}\end{array}
当小车沿着Y轴平移时：
\begin{array}{l}{V{A}=+V{y}} \ {V{B}=+V{y}} \ {V{C}=+V{y}} \ {V{D}=+V{y}}\end{array}
当小车绕几何中心自转时：
\begin{array}{l}{V{A}=-\omega(\mathrm{a}+b)} \ {V{B}=-\omega(\mathrm{a}+b)} \ {V{C}=+\omega(\mathrm{a}+b)} \ {V{D}=+\omega(\mathrm{a}+b)}\end{array}
将以上三个方程组合并，即可根据小车的运动状态解算出四个轮子的转速：
\begin{array}{l}{V{A}=-V{x}+V{y}-\omega(y+b)} \ {V{B}=+V{x}+V{y}-\omega(a+b)} \ {V{C}=-V{x}+V{y}+\omega(a+b)} \ {V{D}=+V{x}+V{y}+\omega(a+b)}\end{array}
根据上面的建模结果，我们很容易用C语言进行实现：
#define a_PARAMETER (0.25f)#define b_PARAMETER (0.225f)/**************************************************************************函数功能：小车运动数学模型入口参数：X Y Z 三轴速度或者位置返回 值：无**************************************************************************/void Kinematic_Analysis(float Vx,float Vy,float Vz){Target_A = -Vx+Vy-Vz*(a_PARAMETER+b_PARAMETER);Target_B = +Vx+Vy-Vz*(a_PARAMETER+b_PARAMETER);Target_C = -Vx+Vy+Vz*(a_PARAMETER+b_PARAMETER);Target_D = +Vx+Vy+Vz*(a_PARAMETER+b_PARAMETER);}/**************************************************************************
小车分为速度模式与位置模式，用到的控制算法为 PID 控制，控制方块图如下：
