大疆3508、2006的输出轴的角度获取

因为大疆3508、2006电调的报文只有转子的角度，在使用过程中可能需要用的到输出轴的角度，比如用PID控制电机的输出轴角度。

把转子的角度累积到输出轴上
算法思路如下：
1、我们知道转子每周的角度随着方向不一样有两种情况，一种累加的到360度后从0开始，另外一种是类减的到0度后从360开始。我们可以利用两次接受的转子的角度差。假设正转累加，反转累减

2、转子的速度方向知道，假设正转为正，反转为负。
3、设定电机输出轴角度为out_angle，转子转的圈数为cyclinder_number，前后两秒的转子角度差为angle_error
4、
1）当转子速度为正时，angle_error出现负值，代表转子已经转了一圈，此时cyclinder_number++；
2）当转子速度为负时，angle_error出现正值，代表转子已经逆着转了一圈，此时cyclinder_number–，
5、输出轴角度为cyclinder_number*K+转子角度

代码如下

if(rx_message.StdId == MOTOR_ID_READ6)
		{
			MOTOR_FEEDBACK[motor_6].angle_value  = rx_message.Data[0] << 8 | rx_message.Data[1];
			MOTOR_FEEDBACK[motor_6].speed_rpm    = rx_message.Data[2] << 8 | rx_message.Data[3];
			MOTOR_FEEDBACK[motor_6].real_current = rx_message.Data[4] << 8 | rx_message.Data[5];
			MOTOR_FEEDBACK[motor_6].temperature = rx_message.Data[6];
			
			MOTOR_FEEDBACK[motor_6].real_angle = MOTOR_FEEDBACK[motor_6].angle_value/8192.0f*360.0f;
			
			////////////////////////////////////////////////////////////////////////////////////////////////////
      angle_error = MOTOR_FEEDBACK[motor_6].angle_value - angle_old;     //转子的角度差 = 现在转子角度 - 上一次的转子角度
			if(MOTOR_FEEDBACK[motor_6].speed_rpm > 0)                          //如果转子速度为正
			{
				if(-18<cyclinder_number&&cyclinder_number < 18)                   //判读输出轴是否转了一圈
				{
					if(angle_error < -1000)                                          //角度为负
					{ 
					  cyclinder_number++;                                            //转子已经转了一圈，cyclinder_number++
					} 
				}
				else
				{
					cyclinder_number = 0;
				}
			}
			else if(MOTOR_FEEDBACK[motor_6].speed_rpm < 0)
			{
				if(-18<cyclinder_number&&cyclinder_number < 18)
				{
					if(angle_error > 1000)
					{
					  cyclinder_number--;
					}
				}
				else
				{
					cyclinder_number = 0;
				}				
			}
		
			MOTOR_FEEDBACK[motor_6].out_angle = MOTOR_FEEDBACK[motor_6].angle_value + cyclinder_number*8191;     //输出轴的机械刻度
			if(MOTOR_FEEDBACK[motor_6].out_angle >=0)                                                            //如果这个条件使用，在elsed中处理一下控制角度的周期
			{
				MOTOR_FEEDBACK[motor_6].out_real_angle = MOTOR_FEEDBACK[motor_6].out_angle/(8191*18.00f)*360;       //输出轴的机械刻度换算成角度
			}
			else
			{
				MOTOR_FEEDBACK[motor_6].out_real_angle = MOTOR_FEEDBACK[motor_6].out_angle/(8191*18.00f)*360;        //输出轴的机械刻度换算成角度
			}
      angle_old = MOTOR_FEEDBACK[motor_6].angle_value;                                                       //记录此时的转子角度，下次接收时作“上一次角度”使用
			//////////////////////////////////////////////////////////////////////////////////////////////////////
		}
		

代码不复杂，也有点小问题，比如每次断电，角度会归零（有点值，主要是转子的角度）
总的来说就是把转子的角度累积到输出轴上

关注点赞博主并私信，送工程文件

仅为个人拙见，欢迎指正交流，转发注明出处哦，谢谢

标签：3508,angle,转子,大疆,角度,2006,MOTOR,cyclinder,FEEDBACK
来源： https://blog.csdn.net/ORANGEbb/article/details/118613321