智能家居设计-智能扫地机器人-

1、智能家居设计-智能扫地机器人智能家居设计-智能扫地机器人摘要智能扫地机器人是目前科技争辩领域的大热门项目，争辩的成果是利用效率高的人工智能清扫代替以往效率低下而且时间成本高的手工劳动。本论文以国内外的路径规划算法为参考，以便捷的使用、低成本、良好的牢靠性等为论文的原则，最终设计一款符合市场的需求、有用性强的智能扫地机器人。智能扫地机器人要具有精确以及准时的回避障碍物的力量这一功能。在此，本文利用了 STM32F407VET6 单片机作为把握器，通过 C610 无刷电机调速器对无刷电机把握，自行设计硬件电路，在数字 PID 算法的和随机掩盖法的把握策略下完成路径清洁任务。本论文智能扫地机器人外配

2、4 个超声波传感器进行避障测距，从而完成避障系统的设计。该论文智能扫地机器人结构轻快，携带便利，能够自主的完成清洁任务，相较于市场上上千元的智能扫地机器人，本论文的智能扫地机器人显得性价比更高。在人们日常的居家生活中能够起着格外便捷的角色，给人们带来便利。关键词：路径规划；机器人；传感器；Intelligent home furnishing design intelligent sweeping robotAbstractIntelligent sweeping robot is a hot research project in the field of science and techn

3、ology. The research result is to replace the low efficiency and high time cost manual labor with high efficiency artificial intelligence sweeping. This paper takes the path planning algorithm at home and abroad as a reference, and takes convenient use, low cost,good reliability as the principle of t

4、he paper, and finally designs an intelligent sweeping robot which meets the market demand and has strong practicability.Intelligent sweeping robot should have the ability of avoiding obstacles accurately and timely. In this paper, stm32f407vet6single-chip microcomputer isused as the controller to co

5、ntrol the BLDCM through c610 BLDCM governor, and the hardware circuit is designed by ourselves. The path cleaning task is completed under the control strategy of digital PID algorithm and random covering method. In this paper, the intelligent sweeping robot is equipped with four ultrasonic sensors f

6、or obstacle avoidance ranging, so as to complete the design of obstacle avoidance system.Compared with thousands of intelligent sweeping robots in the market, the intelligent sweeping robot in this paper is more cost-effective. It can play a very convenient role in peoples daily home life and bring

7、convenience to people.Keywords:Path planning; robot; sensor;目录 HYPERLINK l _TOC_250018 前言1 HYPERLINK l _TOC_250017 智能扫地机器人在国内外的进展1智能扫地机器人进展前景与趋势2设计任务与要求3 HYPERLINK l _TOC_250016 系统总体方案3 HYPERLINK l _TOC_250015 系统避障原理3 HYPERLINK l _TOC_250014 系统方案设计4 HYPERLINK l _TOC_250013 CAN 通信5 HYPERLINK l _TOC_2

8、50012 CAN 通信的介绍5 HYPERLINK l _TOC_250011 CAN 通信的原理5 HYPERLINK l _TOC_250010 CAN 通信的特点6系统硬件设计6电源电路设计6 HYPERLINK l _TOC_250009 传感器的介绍7 HYPERLINK l _TOC_250008 超声波传感器的介绍7BIM088 的介绍8 HYPERLINK l _TOC_250007 软件设计8 HYPERLINK l _TOC_250006 软件开发环境的介绍8 HYPERLINK l _TOC_250005 PID 算法8 HYPERLINK l _TOC_250004

9、FreeRTOS 的介绍9 HYPERLINK l _TOC_250003 软件的程序实现9 HYPERLINK l _TOC_250002 5 结论12 HYPERLINK l _TOC_250001 参考文献13致谢14 HYPERLINK l _TOC_250000 附录15前言近年来，科学技术的高速进展，人们的目光渐渐的移到人工智能方面的争辩，也正由于这样，智能机器人的争辩也被人们越来越的受重视，其中包括智能扫地机器人也正在渐渐的渐渐步入人们的日常生活中来。在现在的 21 世纪初，国内的生活水平不断的提高以及生活节奏不断的加快，智能扫地机机器人原本只是在欧美市场上存在的，但是由于国内需

10、求不断的扩大，开头渐渐的涌入国内，也渐渐地被人们所接受；可以预见，智能扫地机器人将来会像电视机洗衣机等其他普一般通的家用电器一样成为每个家庭的必备家用电器，智能扫地机器人也会朝着更加智能化的进展，甚至会在不久的将来渐渐的代替人工清洁。在各种场所开头融入智能扫地机器人的现在，市场上对其提出了性价比，使用便利，安全牢靠等的新要求，因此本论文设计一款符合理论而已有能迎合市场需要的智能扫地机器人有着重大的意义。智能扫地机器人在国内外的进展瑞典伊莱克斯三叶虫专业家电制造商于 1997 年制造了第一代扫地机器人，他们将单滚刷及无边刷的设计安装在机器的后部，这台第一代扫地机器人具有当它没有电能时可以自动回充

11、的功能，而已还具有防止跌落的功能。不过它并不是十全十美，它当时的运算速度和反应速度以及机器前行速度都相当的慢，以至于清洁起来的效率什么低下。第一代的扫地机器人的厚度设计格外不合理，以至于它无法深化很多家具底部进行清洁。第一代的扫地机器人的价格是格外的昂扬，令很多的消费者望而却步，所以第一代扫地机器人的关注热度很低。2001 年诞生于英国黑科技公司 Dyson(戴森)的一款名为 DC06 的扫地机器人，由于其精致的造工，使用了众多价格昂贵的传感器和处理性能优越的计算机，即使这台扫地机器人具有强大的清洁功能和精准的探测功能，自始至终都不能够推向市场面对大众，上市的话或许估量要卖人民币 20000

12、元。虽然第一代的产品的有着很大的不足，但是第一代的产品它也算是一种跨时代的产品，时至今日，第一代的扫地机器热还影响着不少的现在的扫地机器人，而且第一代扫地机器人的部分技术至今而然沿用。从前只是一家设计天空探测、排爆、战地救援、安保等不同机器人的一家美国科技公司 iRobot，这家公司在 2002 做出了一个大胆的打算，他们打算从军用型机器人设计转型到家用型机器人设计。于此，它们在 2002 年设计生产出了第一台自己的扫地机器人 Roomba，让人万万没想到的是，当时只是生产了 15000 台的 iRobot 通过发布会的成功举办，一下子把10000 多台扫地机器人出售出去。发布会的成本也在示意

13、着他们市场的猛烈的需求，在这一年的圣诞节追加生产了50000 台扫地机器人，并且都全部卖了出去，这一系列的成功，开启了扫地机器人的进展爆发的阶段。2010 年诞生于美国加州硅谷的一家公司 Neato 的 Neato XV-11，是 Neato 公司制造的第一台扫地机器人，这台扫地机器人具有识别整个房子结构的功能，而且还能用最短的时间和最短的路劲完成清洁任务。Neato 公司推出的这台扫地机器人可谓具有划时代的代表意义，象征着扫地机器人智能时代真正的开启。2010 年至今，越来越多的公司进入智能扫地机器人市场，它们为智能扫地机器人市场投入了大量的资源，也因此增大了行业间的竞争，大力推动了智能扫地

14、机人的技术创新，到目前为止，国内除了台湾Proscenic 外，大陆苏州怡凯电器推出的智能扫地机器人也取得良好的成果。智能扫地机器人的进展前景与趋势我们可以想象在不久的将来，我们的每个人的家庭都会拥有像智能扫地机器人这样的一般的家电一样的家庭必要帮手。智能扫地机器人在将来也会朝着越来越智能化的发展的道路前进，信任在不久的将来可以渐渐的代替人类来完成清洁工作，解放人类的双手。智能扫地机器人渐渐进入人们的眼睑，在人群中受到广泛的欢迎，但是智能扫地机器人在人们的使用过程中仍旧暴露出了很多的问题和缺陷。例如，会重复清扫同一个地方，扫地机器人高度过高，无法进入桌底清扫，扫地机器人完成清洁任务需要消耗几个

15、小时甚至跟多的时间，扫地机器人消耗完电池都没能完成清洁任务等等问题。特殊是良好的路径规划方案缺少和价格普遍偏高这两个问题更是阻碍了其进一步 应用。在智能扫地机器人步入人们的生活场所的同时，设计一款性价比高、使用便携、牢靠性高的智能扫地机器人具有重大的意义。设计任务扫地机器人在工作的环境当中无时无刻要面对的是简单的环境，所以良好的路径规划算法是推断一个扫地机器人的标准。本论文的扫地机器人主要解决高重复率、时效性低低、掩盖率低的问题，所以在查找国内外文献的时候主要以如何解决重复率、时效性、掩盖率的问题。分析了现有的智能机器人的各种的路径规划算法，分析了它们的优点和缺点，想到当前市场上的智能扫地机器

16、人存在清扫随机盲目、并没有设计良好的路径规划的现象。所以打算从扫地机器人工作环境的实际状况为着落点，从扫地机器人的工作环境的总体和局部这两个方面动身，争辩出一种有效牢靠的路径规划算法，不单单能够对目前已有的路径规划算法取其精华去其糟粕，还能够依据实际的工作环境的信息进行更深层次的优化。争辩一种能够对障碍物准时精确牢靠的回避、推断前方物体是障碍物还是墙壁、适应各种障碍物的避障算法。争辩了国内外智能扫地机器人的机械结构构造，参考国内外智能扫地机器人的设计原理和学习相关的设计理论。本论文要求基于STM32 单片机设计智能扫地机器人，协作外接多种传感器接过数据处理和采集信息，设计硬件电路驱动电机以使机

17、器人完成清洁任务。系统总体方案系统避障原理在智能扫地机器人的实际工作环境中，智能扫地机器人在完成它的清洁任务的过程当中无疑要面对很多的障碍物，这个时候考验一个智能扫地机器人的力量的就看它能否顺当的躲避障碍物，这是衡量机器的一个标准。但是这里也有一个难题需要处理，怎样推断前方消灭的是墙壁还是障碍物呢？智能扫地机器人在清洁工作的过程中遇到的障碍物的外形并不是都是一样的， 所以我们把全部的障碍物统一认为是规章的法规来处理。这项处理的功能建立在扫地机器人外部安装的超声波传感器。扫地机器人身上共安装了三个超声波测距传感器，机身前部，即头部的超声波传感器用来确定停止的位置，左右两侧的超声波传感器用来确定绕

18、线位置。扫地机器人工作环境可以视为是一个二维的片面图形，它和障碍物之间的距离可以用 L 来表示。扫地机器人跟障碍物的转弯值为k。L2k 按当前速度走， kL2k 速度为 k/L，Lk 避障转弯。在机器人绕开物体避障过程中，依据弧长和角度的关系:（|h1-h2|= *|R-r| ）;(2-1)|R-r| 为机器人左右两轮之间的间隔， h1、h2 分别为两轮的编码器的值，则可求得， 扫地机器人则依据原来的程序连续执行，连续执行从前的清洁任务，也可以用此公式为回位数据。假设机器人单向行驶完一次需行走的长度为m，当超声波传感器感应到前方有物体时，机器人以及行驶的的距离为n1，超声波传感器传回的数据并且

19、处理得到的距离为 n2，假如 n1+n2 TIM9_CH1 PE6- TIM9_CH2*/void LED_Init(void)TIM_OC_InitTypeDef sConfigOC; TIM_HandleTypeDef htim9;htim9.Instance = TIM9; htim9.Init.Prescaler = 168-1;htim9.Init.CounterMode = TIM_COUNTERMODE_UP; htim9.Init.Period = 1000;/1000ms htim9.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; H

20、AL_TIM_PWM_Init(&htim9);sConfigOC.OCMode = TIM_OCMODE_PWM1; sConfigOC.Pulse = 0;sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW;HAL_TIM_PWM_ConfigChannel(&htim9, &sConfigOC, TIM_CHANNEL_1); HAL_TIM_PWM_ConfigChannel(&htim9, &sConfigOC, TIM_CHANNEL_2);HAL_TIM_PWM_Start(&htim9,TIM_CHANNEL_1);/ HAL_TIM_PWM_S

21、tart(&htim9,TIM_CHANNEL_2);/* TIM4 init function */void TIM4_Init(void)/TIM_OC_InitTypeDef sConfigOC; TIM_HandleTypeDef htim4;htim4.Instance = TIM4; htim4.Init.Prescaler = 84-1;htim4.Init.CounterMode = TIM_COUNTERMODE_UP; htim4.Init.Period = 20000;/20ms htim4.Init.ClockDivision = TIM_CLOCKDIVISION_D

22、IV1; HAL_TIM_PWM_Init(&htim4);/1900 2620 2250sConfigOC.OCMode = TIM_OCMODE_PWM1; sConfigOC.Pulse = 2250;sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_1);HAL_TIM_PWM_MspInit(&htim4); HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_1);void HAL_TIM_PWM_MspIn

23、it(TIM_HandleTypeDef *htim) HAL_RCC_TIM4_CLK_ENABLE(); HAL_RCC_TIM9_CLK_ENABLE();GPIO_InitTypeDef GPIO_InitStruct; if(htim-Instance=TIM4)/*TIM4 GPIO Configuration PD12 TIM4_CH1PD13- TIM4_CH2PD14- TIM4_CH3PD15- TIM4_CH4*/ HAL_RCC_GPIOD_CLK_ENABLE();GPIO_InitStruct.Pin = GPIO_PIN_12; GPIO_InitStruct.M

24、ode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GPIO_PULLUP; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; GPIO_InitStruct.Alternate = GPIO_AF2_TIM4; HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);else if(htim-Instance=TIM9)/*TIM9 GPIO ConfigurationPE5- TIM9_CH1 PE6- TIM9_CH2*/ HAL_RCC_GPIOE_CLK_ENABLE();GPIO_

25、InitStruct.Pin = GPIO_PIN_5|GPIO_PIN_6; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GPIO_PULLUP; GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;GPIO_InitStruct.Alternate = GPIO_AF3_TIM9; HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);/void GREEN_LED(uint16_t bright)TIM9-CCR1 = bright;/void RED_L

26、ED(uint16_t bright)TIM9-CCR2 = bright;/*状态灯*/*串口通信*/ #include usart1.h#include stm32f4xx_hal.h/,printf,use MicroLIB/#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f) #if 1#pragma import( use_no_semihosting)/ struct FILEint handle;FILE stdout;/_sys_exit() void _sys_exit(int x)x = x;/fputcint fputc

27、(int ch, FILE *f)while(USART1-SR&0X40)=0);/-, USART1-DR = (uint8_t) ch;return ch;#endifUART_HandleTypeDef huart1;DMA_HandleTypeDefUART2RxDMA_Handler; DMA_HandleTypeDefUART4RxDMA_Handler;/void USART1_Init(void)huart1.Instance = USART1; huart1.Init.BaudRate = 115200;huart1.Init.WordLength = UART_WORDL

28、ENGTH_8B; huart1.Init.StopBits = UART_STOPBITS_1; huart1.Init.Parity = UART_PARITY_NONE; huart1.Init.Mode = UART_MODE_TX_RX; huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;HAL_UART_Init(&huart1);/void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)GPIO_InitTypeDef GPIO_InitStruct; if(uartHandle-Instan

29、ce=USART1) HAL_RCC_GPIOA_CLK_ENABLE(); HAL_RCC_USART1_CLK_ENABLE();/* PA9 USART1_TXPA10 USART1_RX */GPIO_InitStruct.Pin = GPIO_PIN_9 | GPIO_PIN_10; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GPIO_PULLUP; GPIO_InitStruct.Speed = GPIO_SPEED_FAST; GPIO_InitStruct.Alternate = GPIO_AF

30、7_USART1; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);else if(uartHandle-Instance=USART2)/ HAL_RCC_USART2_CLK_ENABLE(); HAL_RCC_DMA1_CLK_ENABLE(); HAL_RCC_GPIOD_CLK_ENABLE();/* PD5 USART2_TXPD6 USART2_RX */GPIO_InitStruct.Pin = GPIO_PIN_6; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GP

31、IO_PULLUP; GPIO_InitStruct.Speed = GPIO_SPEED_FAST; GPIO_InitStruct.Alternate = GPIO_AF7_USART2; HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);/RX UART2RxDMA_Handler.Instance=DMA1_Stream5;UART2RxDMA_Handler.Init.Channel=DMA_CHANNEL_4;UART2RxDMA_Handler.Init.Direction = DMA_PERIPH_TO_MEMORY; UART2RxDMA_Hand

32、ler.Init.PeriphInc = DMA_PINC_DISABLE; UART2RxDMA_Handler.Init.MemInc = DMA_MINC_ENABLE; UART2RxDMA_Handler.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; UART2RxDMA_Handler.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; UART2RxDMA_Handler.Init.Mode = DMA_CIRCULAR; UART2RxDMA_Handler.Init.Priority =

33、DMA_PRIORITY_LOW; UART2RxDMA_Handler.Init.FIFOMode = DMA_FIFOMODE_DISABLE;HAL_DMA_Init(&UART2RxDMA_Handler); HAL_LINKDMA(uartHandle,hdmarx,UART2RxDMA_Handler);HAL_NVIC_SetPriority(USART2_IRQn, 5, 0); HAL_NVIC_EnableIRQ(USART2_IRQn);/USART2else if(uartHandle-Instance=UART4) HAL_RCC_UART4_CLK_ENABLE()

34、; HAL_RCC_DMA1_CLK_ENABLE(); HAL_RCC_GPIOA_CLK_ENABLE();/* PA0 UART4_TXPA1 UART4_RX */GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_1; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GPIO_PULLUP; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; GPIO_InitStruct.Alternate = GPIO_AF8_UART4; HAL

35、_GPIO_Init(GPIOA, &GPIO_InitStruct);/RX UART4RxDMA_Handler.Instance=DMA1_Stream2;UART4RxDMA_Handler.Init.Channel=DMA_CHANNEL_4;UART4RxDMA_Handler.Init.Direction = DMA_PERIPH_TO_MEMORY; UART4RxDMA_Handler.Init.PeriphInc = DMA_PINC_DISABLE; UART4RxDMA_Handler.Init.MemInc = DMA_MINC_ENABLE; UART4RxDMA_

36、Handler.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; UART4RxDMA_Handler.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; UART4RxDMA_Handler.Init.Mode = DMA_CIRCULAR; UART4RxDMA_Handler.Init.Priority = DMA_PRIORITY_LOW; UART4RxDMA_Handler.Init.FIFOMode = DMA_FIFOMODE_DISABLE;HAL_DMA_Init(&UART4RxDMA_H

37、andler); HAL_LINKDMA(uartHandle,hdmarx,UART4RxDMA_Handler);HAL_NVIC_SetPriority(UART4_IRQn, 5, 0); HAL_NVIC_EnableIRQ(UART4_IRQn);/-void Ni_Ming(uint8_t fun,float Pid_ref1,float Pid_ref2,float Pid_ref3,float Pid_ref4)uint8_t send_buf21; unsigned char *p1,*p2,*p3,*p4; p1=(unsigned char *)&Pid_ref1;p2

38、=(unsigned char *)&Pid_ref2; p3=(unsigned char *)&Pid_ref3; p4=(unsigned char *)&Pid_ref4;send_buf0=0XAA; / send_buf1=0XAA; / send_buf2=fun;/ send_buf3=16;/send_buf4=(unsigned char)(*(p1+3); send_buf5=(unsigned char)(*(p1+2); send_buf6=(unsigned char)(*(p1+1); send_buf7=(unsigned char)(*(p1+0);send_

39、buf8=(unsigned char)(*(p2+3); send_buf9=(unsigned char)(*(p2+2); send_buf10=(unsigned char)(*(p2+1); send_buf11=(unsigned char)(*(p2+0); send_buf12=(unsigned char)(*(p3+3); send_buf13=(unsigned char)(*(p3+2); send_buf14=(unsigned char)(*(p3+1);send_buf15=(unsigned char)(*(p3+0);send_buf16=(unsigned

40、char)(*(p4+3); send_buf17=(unsigned char)(*(p4+2); send_buf18=(unsigned char)(*(p4+1); send_buf19=(unsigned char)(*(p4+0);send_buf20=0;for(uint8_t i=0;i20;i+)send_buf20+=send_bufi;/ for(uint8_t i=0;iDR=send_bufi;/*串口通信*/*can 通信*/ #include can_receive.h#include stm32f4xx_hal.h/tof/static tof_can_data

41、_t tof_data;/static gyro_info_t gyro_info;/static motor_measure_t motor_chassis8;/CANvoid HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)if(hcan = (&hcan1)uint8_t Data8; CAN_RxHeaderTypeDef RxMeg; HAL_StatusTypeDefHAL_RetVal;HAL_RetVal = HAL_CAN_GetRxMessage(&hcan1, CAN_RX_FIFO0, &RxMeg,

42、Data); if(HAL_OK=HAL_RetVal)switch (RxMeg.StdId)case CAN_3508_M1_ID: case CAN_3508_M2_ID:case CAN_3508_M3_ID: case CAN_3508_M4_ID: case CAN_3508_M5_ID: case CAN_3508_M6_ID: case CAN_3508_M7_ID: case CAN_3508_M8_ID:static uint8_t i = 0;/IDi = RxMeg.StdId - 0 x201;/ get_motor_measuer(&motor_chassisi,

43、Data, RxMeg.StdId); break;default:break;if(hcan = (&hcan2)uint8_t Data8; CAN_RxHeaderTypeDef RxMeg; HAL_StatusTypeDefHAL_RetVal;HAL_RetVal = HAL_CAN_GetRxMessage(&hcan2, CAN_RX_FIFO0, &RxMeg, Data); if(HAL_OK=HAL_RetVal)switch (RxMeg.StdId)case 101:static int16_t pitch_connt = 0; static int16_t raw_

44、pit,raw_v_z = 0;static float pitch_angle, last_pitch_angle = 0; raw_v_z = Data28 | Data3;raw_pit = Data48 | Data5;/- gyro_info.v_z = (float)raw_v_z * 0.057295f;/-100pitch_angle = (float)raw_pit/100;/pit if(pitch_angle 330)pitch_connt-;else if(pitch_angle - last_pitch_angle) -330) pitch_connt+;gyro_i

45、nfo.pit = pitch_angle + pitch_connt * 360; last_pitch_angle = pitch_angle;break;case 0 x300:/if(Data08 | Data1) 1000)/tof_data.dis_r = Data08 | Data1;/if(Data28 | Data3) 1000)/tof_data.dis_l = Data28 | Data3; break;case 0 x401:gyro_info.yaw=0.01f*(int32_t)(Data024)|(int32_t)(Data116)| (int32_t)(Data

46、2last_ecd = ptr-ecd;ptr-ecd= Data0 speed_rpm = (uint16_t)(Data2 ecd - ptr-last_ecd 4096)ptr-round_cnt-;ptr-ecd_raw_rate = ptr-ecd - ptr-last_ecd - 8192;else if (ptr-ecd - ptr-last_ecd round_cnt+;ptr-ecd_raw_rate = ptr-ecd - ptr-last_ecd + 8192;elseptr-ecd_raw_rate = ptr-ecd - ptr-last_ecd;int32_t te

47、mp_sum = 0;ptr-rate_bufptr-buf_cut+ = ptr-ecd_raw_rate; if (ptr-buf_cut = 5)ptr-buf_cut = 0;for (uint8_t i = 0; i rate_bufi;ptr-last_filter_rate = ptr-filter_rate; ptr-filter_rate = (int32_t)(temp_sum/5);ptr-total_ecd = ptr-round_cnt * 8192 + ptr-ecd - ptr-offset_ecd;/* total angle, unit is degree *

48、/ptr-angle = ptr-total_ecd / (8192.0f/360.0f*19.0f);/void CAN_CMD_CHASSIS(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)uint8_t Data8; uint32_t pTxMailbox;CAN_TxHeaderTypeDef TxMeg;TxMeg.StdId = CAN_CHASSIS_ALL_ID; TxMeg.IDE = CAN_ID_STD;TxMeg.RTR = CAN_RTR_DATA; TxMeg.DLC = 0 x08;D

49、ata0 = motor1 8; Data1 = motor1; Data2 = motor2 8; Data3 = motor2; Data4 = motor3 8; Data5 = motor3; Data6 = motor4 8; Data7 = motor4;HAL_CAN_AddTxMessage(&hcan1, &TxMeg, Data, &pTxMailbox);/void CAN_CMD_LIFT(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)uint8_t Data8; uint32_t pTxM

50、ailbox;CAN_TxHeaderTypeDef TxMeg;TxMeg.StdId = 0 x1ff; TxMeg.IDE = CAN_ID_STD; TxMeg.RTR = CAN_RTR_DATA; TxMeg.DLC = 0 x08;Data0 = motor1 8; Data1 = motor1; Data2 = motor2 8; Data3 = motor2; Data4 = motor3 8; Data5 = motor3; Data6 = motor4 8; Data7 = motor4;HAL_CAN_AddTxMessage(&hcan1, &TxMeg, Data,

51、 &pTxMailbox);/ID100;mode:0 x30time1000ms void CAN_CMD_GYRO_CALI(uint8_t mode, uint16_t time)uint8_t Data8; uint32_t pTxMailbox;CAN_TxHeaderTypeDef TxMeg;TxMeg.StdId = 100; TxMeg.IDE = CAN_ID_STD; TxMeg.RTR = CAN_RTR_DATA; TxMeg.DLC = 0 x03;Data0 = mode;Data1 = time 8;Data2 = time ; Data3 = 0;Data4

52、= 0;Data5 = 0;Data6 = 0;Data7 = 0;/10ms uint32_t tickstart = HAL_GetTick();uint32_t wait = 10;if (wait HAL_MAX_DELAY)wait += (uint32_t)1;while(HAL_GetTick() - tickstart) v_z,0,0,0);/4ms vTaskDelay(2);chassis_high_water = uxTaskGetStackHighWaterMark(NULL);/void chassis_init(chassis_move_t *chassis_in

53、it)if (chassis_init = NULL)return;/PIDconst static float motor_speed_pid3 = 700, 0, 0;/PIDconst static float motor_pos_pid3 = 0, 0, 0;/ for (uint8_t i = 0; i motor_chassisi.chassis_motor_measure = get_Motor_Measure_Point(i);/chassis_init-chassis_RC = get_remote_control_point();/chassis_init-gyro_dat

54、a = get_GYRO_Measure_Point();/tofchassis_init-tof_measure = get_tof_Info_Measure_Point();/PID for (uint8_t i = 0; i motor_speed_pidi, PID_POSITION, motor_speed_pid, 10000, 0);/PID for (uint8_t i= 0; i motor_pos_pidi, PID_POSITION, motor_pos_pid, 0, 0);/ZPIDCHISSIS_PID_Init(&chassis_init-chassis_gryo

55、_pid, 2000, 0, 12, 0, 0);/kp_out ki_out kp ki kd 20 60CHISSIS_PID_Init(&chassis_init-chassis_acc_pid, 60, 0, 0.3, 0, 0);/chassis_init-gyro_angle_start = chassis_init-gyro_data-yaw;/ chassis_feedback_update(chassis_init);/void chassis_feedback_update(chassis_move_t *chassis_update)/PIDchassis_update-

56、motor_chassis0.speed=chassis_update-motor_chassis0.chassis_motor_measure-filter_rate / 19.0f;chassis_update-motor_chassis1.speed=chassis_update-motor_chassis1.chassis_motor_measure-filter_rate / 19.0f;chassis_update-motor_chassis2.speed=chassis_update-motor_chassis2.chassis_motor_measure-filter_rate

57、 / 19.0f;chassis_update-motor_chassis3.speed=chassis_update-motor_chassis3.chassis_motor_measure-filter_rate / 19.0f; chassis_update-vw_mouse = chassis_update-chassis_RC-mouse.x; chassis_update-tof_h = chassis_update-tof_measure-tof_h;/switch(chassis_update-chassis_RC-rc.s0)case 1:chassis_mode = RC_

58、MODE; break;case 3:chassis_mode = KEY_MODE; break;case 2:chassis_mode = STOP_MODE; break;default:break;/if(xTaskGetTickCount() - chassis_update-chassis_RC-time 88)chassis_mode = STOP_MODE;/PIDvoid chassis_control_loop(chassis_move_t *chassis_control)switch(chassis_mode)case RC_MODE:/chassis_control-

59、vx =chassis_control-chassis_RC-rc.ch0 * 60.0f/660.0f; chassis_control-vy =chassis_control-chassis_RC-rc.ch1 * 60.0f/660.0f; chassis_control-vw_offset += chassis_control-chassis_RC-rc.ch2 * 50/660 *0.02; chassis_control-vw_set = chassis_control-vw_offset + chassis_control-gyro_angle_start; break;case

60、 KEY_MODE:/chassis_control-key_time+;/4ms if(chassis_control-chassis_RC-rc.s1 = 1)chassis_control-vy_offset = 12;chassis_control-vx_offset = 12;else if(chassis_control-chassis_RC-key.v & SHIFT)/shitfelsechassis_control-vy_offset = 60;chassis_control-vx_offset = 50;chassis_control-vy_offset = 50;chas