Robomaster C610+M2007+stm32组成伺服系统

写这篇文章之前，首先要感谢前人的基础（https://blog.csdn.net/Introver_t/article/details/80103041）。没有了解过基础之前，建议初学者先了解大疆C610+M2007的相关软硬件资料，再看看伺服系统的基本三环结构，当然stm32是需要熟练掌握的。
参考上述文章，再结合大疆官网给出的demo改动，才得以调试出 C610+M2007组成伺服系统用以参加四川省机器人大赛，并获得19年度一等奖。
好了，进入正题。
首先是通信，c610使用can 总线通信，因此需要对STM32的CAN初始化：
Can2Init();
由于我的系统中有4套电机，所以我过滤的时候保留这些电机的地址：

Can_filter_Structure.CAN_FilterIdHigh	= ((uint16_t)CAN_2006Moto1_ID << 5);
Can_filter_Structure.CAN_FilterIdLow	= ((uint16_t)CAN_2006Moto2_ID << 5);
Can_filter_Structure.CAN_FilterMaskIdHigh	= ((uint16_t)CAN_2006Moto3_ID << 5); 
Can_filter_Structure.CAN_FilterMaskIdLow	= ((uint16_t)CAN_2006Moto4_ID << 5);

这是第一点要注意的，第二：初始化的顺序相当重要，STM32F407单片机CAN总线初始化需要注意一定的顺序，不然老是有can总线初始化失败或者串口工作不正常。如果使用STM32CUBE也许可以避免上述问题。我这里

初始化的顺序：
	uart6_init(115200);
	uart4_init(9600);
	uart3_init(9600);
	CAN1_Configuration();     //CAN13?¨º??¡¥
	Can2Init();   
	InitBar();   //舵机

先串口，再CAN1、CAN2，最后TIM

初始化过后，CAN2接收函数内部需要改动：
1、增加操作系统接口OSIntEnter()，OSIntExit();（无操作系统的无视此行）
2、针对地址做过滤，免得收到无效信息（CAN_2006Moto1_ID……）

moto_measure_t moto_chassis[4] = {0};//4 chassis moto
void CAN2_RX0_IRQHandler(void)
{
	CanRxMsg rx_message;
	u8 i;
	OSIntEnter(); 
	
//	extern moto_measure_t moto_chassis[4];

	if (CAN_GetITStatus(CAN2,CAN_IT_FMP0)!= RESET)
	{
		CAN_ClearITPendingBit(CAN2, CAN_IT_FMP0);
		CAN_Receive(CAN2, CAN_FIFO0, &rx_message);
		
		if((rx_message.IDE == CAN_Id_Standard)&&(rx_message.IDE == CAN_RTR_Data)&&(rx_message.DLC == 8)) //标准帧、数据帧、数据长度为8
		{
			if((rx_message.StdId == CAN_2006Moto1_ID)||(rx_message.StdId == CAN_2006Moto2_ID)\
			 ||(rx_message.StdId == CAN_2006Moto3_ID)||(rx_message.StdId == CAN_2006Moto4_ID))
			{
				i = rx_message.StdId - CAN_2006Moto1_ID;
		    get_moto_measure(&moto_chassis[i], &rx_message);
			}
		}
	}
	OSIntExit();
}

然后就是重点了：

void TurnMoterSpeedPosCircleTask(void)
{
	int i;
	OS_ERR err;
	extern int Display1;
//	Can2Init();
	for(i=0; i<TurnMotorNum; i++)
  {	
		/*pid, maxout, intergral_limit, deadband, kp, ki, kd*/
    pid_param_init(&SpeedTurnMotorPid[i], 10000, 0, 100, 2.5, 0, 2.0); 
	  pid_param_init(&LocationTurnMotorPid[i], 10000, 0, 20, 0.3, 0.01, 0.35);
  }

	while(1)	
	{
		for(i=0; i<TurnMotorNum; i++)
		{	
			pid_calculate(&LocationTurnMotorPid[i], moto_chassis[i].total_angle, TurnMotorPos[i]);
			pid_calculate(&SpeedTurnMotorPid[i], moto_chassis[i].speed_rpm, LocationTurnMotorPid[i].output);	
		
		}
#ifndef MotorDebug
		set_moto_current(SpeedTurnMotorPid[0].output, SpeedTurnMotorPid[1].output,\
                  	 SpeedTurnMotorPid[2].output, SpeedTurnMotorPid[3].output);
#endif
		OSTimeDlyHMSM(0,0,0,10,OS_OPT_TIME_HMSM_NON_STRICT,&err); 
	}
}

上述代码使用pid控制器构建了速度环和位置环。

pid_param_init(&SpeedTurnMotorPid[i], 10000, 0, 100, 2.5, 0, 2.0); 这是对速度环的初始化，pid_param_init(&LocationTurnMotorPid[i], 10000, 0, 20, 0.3, 0.01, 0.35);是对位置环路的初始化。

   void pid_param_init(
	PID_TypeDef * pid, 
	uint16_t maxout,
	uint16_t intergral_limit,
	float deadband,
	float 	kp, 
	float 	ki, 
	float 	kd)

设置的参数有：pid, maxout, intergral_limit, deadband, kp, ki, kd。pid指你需要设定的PID控制器，本质是一个结构体。maxout指最大输出量，也就是限制幅度。intergral_limit是抗饱和积分，该参数可以限制积分器的最大输出值，我没有用，直接设置0。deadband是死区，意思是误差绝对值如果小于该参数，PID输出0，防止系统产生抖动。然后就是kp, ki, kd三个参数。这是初始化。
接下来就是PID环路的运算，我有四个电机，每个电机都要分别运算速度、位置环路，因此有个4循环：

  for(i=0; i<TurnMotorNum; i++)
		{	
			pid_calculate(&LocationTurnMotorPid[i], moto_chassis[i].total_angle, TurnMotorPos[i]);
			pid_calculate(&SpeedTurnMotorPid[i], moto_chassis[i].speed_rpm, LocationTurnMotorPid[i].output);	
		}

循环内部的PID运算环路：
float pid_calculate(PID_TypeDef* pid, float measure, float set)
参数意义分别是：PID控制器，分别对应速度环、位置环。measure是测量值，也就是通过编码器获取得到的速度值或者位置值。set是设定值，也就是期望达到的值。该函数运算后，将控制量（输出量）存放在结构体PID_TypeDef* pid当中，以供后续调用使用。
需要指出的是：位置环的设定值是指定值（例如：需要转到xx度），而速度环的设定值是位置环的输出值：LocationTurnMotorPid[i].output，而速度环路的输出值，直接给电流环作为输入使用：

set_moto_current(SpeedTurnMotorPid[0].output, SpeedTurnMotorPid[1].output,\
                  	 SpeedTurnMotorPid[2].output, SpeedTurnMotorPid[3].output);

上述伺服系统作为机器人舵轮的转向系统使用，欢迎交流Q:348913335
代码分享：https://download.csdn.net/download/lianqingde/87817797
运行视频：https://www.bilibili.com/video/av76449463/