基于STM32F104的大疆3508、2006电机电调驱动程序(CAN通信、同时驱动多个电机)开源

      3508、2006电机在各种竞赛的使用很普及,其电调驱动方式有PWM和CAN两种,我用的是CAN,在此分享一下个人的学习心得;

官方说明
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

程序部分主要分三部分:

STM32F104的CAN通信配置

void CAN1_Configuration(void)
{
	GPIO_InitTypeDef GPIO_InitStructure; 
	CAN_InitTypeDef CAN_InitStructure;
	CAN_FilterInitTypeDef CAN_FilterInitStructure;
	NVIC_InitTypeDef NVIC_InitStructure;
	
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);	//使能PORTA时钟	                   											 
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_CAN1, ENABLE);	//使能CAN1时钟
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11| GPIO_Pin_12;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;			//复用功能
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;			//推挽输出
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;		//100MHz
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;			//上拉
	GPIO_Init(GPIOA, &GPIO_InitStructure);					//初始化PA11,PA12
	
	GPIO_PinAFConfig(GPIOA,GPIO_PinSource11,GPIO_AF_CAN1);	//GPIOA11复用为CAN1
	GPIO_PinAFConfig(GPIOA,GPIO_PinSource12,GPIO_AF_CAN1);	//GPIOA12复用为CAN1
	
	CAN_DeInit(CAN1);
	CAN_StructInit(&CAN_InitStructure);
	
	CAN_InitStructure.CAN_TTCM= DISABLE;		//非时间触发通信模式   
	CAN_InitStructure.CAN_ABOM= DISABLE;		//软件自动离线管理	  
	CAN_InitStructure.CAN_AWUM= DISABLE;		//睡眠模式通过软件唤醒(清除CAN->MCR的SLEEP位)
	CAN_InitStructure.CAN_NART= ENABLE;			//禁止报文自动传送 
	CAN_InitStructure.CAN_RFLM= DISABLE;		//报文不锁定,新的覆盖旧的  
	CAN_InitStructure.CAN_TXFP= DISABLE;		//优先级由报文标识符决定 
	CAN_InitStructure.CAN_Mode= CAN_Mode_Normal;	//模式设置 
	CAN_InitStructure.CAN_SJW= CAN_SJW_1tq;			//重新同步跳跃宽度(Tsjw)为tsjw+1个时间单位 CAN_SJW_1tq~CAN_SJW_4tq
	CAN_InitStructure.CAN_BS1= CAN_BS1_9tq;			//Tbs1范围CAN_BS1_1tq ~CAN_BS1_16tq
	CAN_InitStructure.CAN_BS2= CAN_BS2_4tq;			//Tbs2范围CAN_BS2_1tq ~	CAN_BS2_8tq
	CAN_InitStructure.CAN_Prescaler= 3;				//分频系数(Fdiv)为brp+1	
	CAN_Init(CAN1, &CAN_InitStructure);		// 初始化CAN1
	
	CAN_FilterInitStructure.CAN_FilterNumber= 0;							//过滤器0
	CAN_FilterInitStructure.CAN_FilterMode= CAN_FilterMode_IdMask; 
	CAN_FilterInitStructure.CAN_FilterScale= CAN_FilterScale_32bit;		//32位 
	CAN_FilterInitStructure.CAN_FilterIdHigh= 0x0000;					//32位ID
	CAN_FilterInitStructure.CAN_FilterIdLow= 0x0000;
	CAN_FilterInitStructure.CAN_FilterMaskIdHigh= 0x0000;				//32位MASK
	CAN_FilterInitStructure.CAN_FilterMaskIdLow= 0x0000;
	CAN_FilterInitStructure.CAN_FilterFIFOAssignment= CAN_Filter_FIFO0;	//过滤器0关联到FIFO0
	CAN_FilterInitStructure.CAN_FilterActivation= ENABLE;				//激活过滤器0
	CAN_FilterInit(&CAN_FilterInitStructure);	//滤波器初始化

	CAN_ITConfig(CAN1,CAN_IT_FMP0,ENABLE);		//FIFO0消息挂号中断允许
	CAN_ITConfig(CAN1,CAN_IT_TME,ENABLE);

	NVIC_InitStructure.NVIC_IRQChannel = CAN1_RX0_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;		// 主优先级为1
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;				// 次优先级为0
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	NVIC_Init(&NVIC_InitStructure);
	
	NVIC_InitStructure.NVIC_IRQChannel = CAN1_TX_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;		// 主优先级为1
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;				// 次优先级为0
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	NVIC_Init(&NVIC_InitStructure);
}




void CAN2_Configuration(void)
{
	GPIO_InitTypeDef GPIO_InitStructure; 
	CAN_InitTypeDef CAN_InitStructure;
	CAN_FilterInitTypeDef CAN_FilterInitStructure;
	NVIC_InitTypeDef NVIC_InitStructure;
	
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);	//使能PORTA时钟	                   											 
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_CAN2, ENABLE);	//使能CAN1时钟
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12| GPIO_Pin_13;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;			//复用功能
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;			//推挽输出
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;		//100MHz
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;			//上拉
	GPIO_Init(GPIOB, &GPIO_InitStructure);					//初始化PB12,PB13
	
	GPIO_PinAFConfig(GPIOB,GPIO_PinSource12,GPIO_AF_CAN2);	//GPIOB12复用为CAN2
	GPIO_PinAFConfig(GPIOB,GPIO_PinSource13,GPIO_AF_CAN2);	//GPIOB13复用为CAN2
	
	CAN_DeInit(CAN2);
	CAN_StructInit(&CAN_InitStructure);
	
	CAN_InitStructure.CAN_TTCM= DISABLE;		//非时间触发通信模式   
	CAN_InitStructure.CAN_ABOM= DISABLE;		//软件自动离线管理	  
	CAN_InitStructure.CAN_AWUM= DISABLE;		//睡眠模式通过软件唤醒(清除CAN->MCR的SLEEP位)
	CAN_InitStructure.CAN_NART= ENABLE;			//禁止报文自动传送 
	CAN_InitStructure.CAN_RFLM= DISABLE;		//报文不锁定,新的覆盖旧的  
	CAN_InitStructure.CAN_TXFP= DISABLE;		//优先级由报文标识符决定 
	CAN_InitStructure.CAN_Mode= CAN_Mode_Normal;	//模式设置 
	CAN_InitStructure.CAN_SJW= CAN_SJW_1tq;			//重新同步跳跃宽度(Tsjw)为tsjw+1个时间单位 CAN_SJW_1tq~CAN_SJW_4tq
	CAN_InitStructure.CAN_BS1= CAN_BS1_9tq;			//Tbs1范围CAN_BS1_1tq ~CAN_BS1_16tq
	CAN_InitStructure.CAN_BS2= CAN_BS2_4tq;			//Tbs2范围CAN_BS2_1tq ~	CAN_BS2_8tq
	CAN_InitStructure.CAN_Prescaler= 3;				//分频系数(Fdiv)为brp+1	
	CAN_Init(CAN2, &CAN_InitStructure);		// 初始化CAN1
	
	CAN_FilterInitStructure.CAN_FilterNumber= 14;							//过滤器0
	CAN_FilterInitStructure.CAN_FilterMode= CAN_FilterMode_IdMask; 
	CAN_FilterInitStructure.CAN_FilterScale= CAN_FilterScale_32bit;		//32位 
	CAN_FilterInitStructure.CAN_FilterIdHigh= 0x0000;					//32位ID
	CAN_FilterInitStructure.CAN_FilterIdLow= 0x0000;
	CAN_FilterInitStructure.CAN_FilterMaskIdHigh= 0x0000;				//32位MASK
	CAN_FilterInitStructure.CAN_FilterMaskIdLow= 0x0000;
	CAN_FilterInitStructure.CAN_FilterFIFOAssignment= CAN_Filter_FIFO0;	//过滤器0关联到FIFO0
	CAN_FilterInitStructure.CAN_FilterActivation= ENABLE;				//激活过滤器0
	CAN_FilterInit(&CAN_FilterInitStructure);	//滤波器初始化

	CAN_ITConfig(CAN2,CAN_IT_FMP0,ENABLE);		//FIFO0消息挂号中断允许
	CAN_ITConfig(CAN2,CAN_IT_TME,ENABLE);

	NVIC_InitStructure.NVIC_IRQChannel = CAN2_RX0_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;		// 主优先级为1
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;				// 次优先级为0
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	NVIC_Init(&NVIC_InitStructure);
	
	NVIC_InitStructure.NVIC_IRQChannel = CAN2_TX_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;		// 主优先级为1
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;				// 次优先级为0
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	NVIC_Init(&NVIC_InitStructure);
}



/* CAN1 TX IRQ */
void CAN1_TX_IRQHandler(void)
{
	if(CAN_GetITStatus(CAN1,CAN_IT_TME)!= RESET)
	{
		CAN_ClearITPendingBit(CAN1,CAN_IT_TME);
	}
}


/* CAN1 RX IRQ */
void CAN1_RX0_IRQHandler(void)
{

}



/* CAN2 TX IRQ */
void CAN2_TX_IRQHandler(void)
{
	if(CAN_GetITStatus(CAN2,CAN_IT_TME)!= RESET)
	{
		CAN_ClearITPendingBit(CAN2,CAN_IT_TME);
	}
}


/* CAN2 RX IRQ */
void CAN2_RX0_IRQHandler(void)
{
	
}

接收数据的程序

void Motor_ReadData(void) //反馈
{
	CanRxMsg rx_message;
	
	if (CAN_GetITStatus(CAN1,CAN_IT_FMP0)!= RESET)
	{
		CAN_ClearITPendingBit(CAN1, CAN_IT_FMP0);
		
		CAN_Receive(CAN1, CAN_FIFO0, &rx_message);
		
		if(rx_message.StdId == MOTOR_ID_READ1)
		{
//			memcpy(&MOTOR_FEEDBACK.angle_value 	, &rx_message.Data[0], 2);
//			memcpy(&MOTOR_FEEDBACK.speed_rpm 	, &rx_message.Data[2], 2);
//			memcpy(&MOTOR_FEEDBACK.real_current , &rx_message.Data[4], 2);
//			memcpy(&MOTOR_FEEDBACK.temperature	, &rx_message.Data[6], 1);
			
			MOTOR_FEEDBACK[motor_1].angle_value  = rx_message.Data[0] << 8 | rx_message.Data[1];
			MOTOR_FEEDBACK[motor_1].speed_rpm    = rx_message.Data[2] << 8 | rx_message.Data[3];
			MOTOR_FEEDBACK[motor_1].real_current = rx_message.Data[4] << 8 | rx_message.Data[5];
			MOTOR_FEEDBACK[motor_1].temperature = rx_message.Data[6];
			
			MOTOR_FEEDBACK[motor_1].real_angle = MOTOR_FEEDBACK[motor_1].angle_value/8192.0f*360.0f;
		}
		
		if(rx_message.StdId == MOTOR_ID_READ2)
		{
//			memcpy(&MOTOR_FEEDBACK.angle_value 	, &rx_message.Data[0], 2);
//			memcpy(&MOTOR_FEEDBACK.speed_rpm 	, &rx_message.Data[2], 2);
//			memcpy(&MOTOR_FEEDBACK.real_current , &rx_message.Data[4], 2);
//			memcpy(&MOTOR_FEEDBACK.temperature	, &rx_message.Data[6], 1);
			
			MOTOR_FEEDBACK[motor_2].angle_value  = rx_message.Data[0] << 8 | rx_message.Data[1];
			MOTOR_FEEDBACK[motor_2].speed_rpm    = rx_message.Data[2] << 8 | rx_message.Data[3];
			MOTOR_FEEDBACK[motor_2].real_current = rx_message.Data[4] << 8 | rx_message.Data[5];
			MOTOR_FEEDBACK[motor_2].temperature = rx_message.Data[6];
			
			MOTOR_FEEDBACK[motor_2].real_angle = MOTOR_FEEDBACK[motor_2].angle_value/8192.0f*360.0f;
		}
		
		if(rx_message.StdId == MOTOR_ID_READ3)
		{
//			memcpy(&MOTOR_FEEDBACK.angle_value 	, &rx_message.Data[0], 2);
//			memcpy(&MOTOR_FEEDBACK.speed_rpm 	, &rx_message.Data[2], 2);
//			memcpy(&MOTOR_FEEDBACK.real_current , &rx_message.Data[4], 2);
//			memcpy(&MOTOR_FEEDBACK.temperature	, &rx_message.Data[6], 1);
			
			MOTOR_FEEDBACK[motor_3].angle_value  = rx_message.Data[0] << 8 | rx_message.Data[1];
			MOTOR_FEEDBACK[motor_3].speed_rpm    = rx_message.Data[2] << 8 | rx_message.Data[3];
			MOTOR_FEEDBACK[motor_3].real_current = rx_message.Data[4] << 8 | rx_message.Data[5];
			MOTOR_FEEDBACK[motor_3].temperature = rx_message.Data[6];
			
			MOTOR_FEEDBACK[motor_3].real_angle = MOTOR_FEEDBACK[motor_3].angle_value/8192.0f*360.0f;
		}
		
		if(rx_message.StdId == MOTOR_ID_READ4)
		{
//			memcpy(&MOTOR_FEEDBACK.angle_value 	, &rx_message.Data[0], 2);
//			memcpy(&MOTOR_FEEDBACK.speed_rpm 	, &rx_message.Data[2], 2);
//			memcpy(&MOTOR_FEEDBACK.real_current , &rx_message.Data[4], 2);
//			memcpy(&MOTOR_FEEDBACK.temperature	, &rx_message.Data[6], 1);
			
			MOTOR_FEEDBACK[motor_4].angle_value  = rx_message.Data[0] << 8 | rx_message.Data[1];
			MOTOR_FEEDBACK[motor_4].speed_rpm    = rx_message.Data[2] << 8 | rx_message.Data[3];
			MOTOR_FEEDBACK[motor_4].real_current = rx_message.Data[4] << 8 | rx_message.Data[5];
			MOTOR_FEEDBACK[motor_4].temperature = rx_message.Data[6];
			
			MOTOR_FEEDBACK[motor_4].real_angle = MOTOR_FEEDBACK[motor_4].angle_value/8192.0f*360.0f;
		}
		
	}
}

发送数据的函数

void Motor_Set_Current(signed short int i1, signed short int i2, signed short int i3, signed short int i4) //发送
{
	CanTxMsg tx_message;
	
	tx_message.IDE = CAN_Id_Standard;
	tx_message.RTR = CAN_RTR_DATA;
	tx_message.DLC = 0x08; // 0x02对应一个电机  0x08 对应4个电机
	tx_message.StdId = 0x200;
	
	tx_message.Data[0] = i1 >> 8;
	tx_message.Data[1] = i1;
	tx_message.Data[2] = i2 >> 8;
	tx_message.Data[3] = i2;
	tx_message.Data[4] = i3 >> 8;
	tx_message.Data[5] = i3;
	tx_message.Data[6] = i4 >> 8;
	tx_message.Data[7] = i4;
	
	CAN_Transmit(CAN1,&tx_message);
}

四个电机控制

/* data define --------------------------------------------------------------------------------------*/
float expect_speed_1,expect_speed_2,expect_speed_3,expect_speed_4;
float a = 0.9;        //  a = wide/2
float b = 1.0;        //  b = length/2 
/***
  * @name underpan_control(int Vx, int Vy, int W)
  * @brief  Vn --> The velocity on the n-axis(As is to the right);    W --> The angular velocity of rotation
  * @param NONE
  * @retval NONE
  */
void underoan_control(float Vx, float Vy, float W)
{
	  expect_speed_1 = -(Vy - Vx + W*(a + b));
    expect_speed_2 = (Vy + Vx - W*(a + b));
    expect_speed_3 = (Vy - Vx - W*(a + b));
    expect_speed_4 = -(Vy + Vx + W*(a + b));
	
	  int m_1, m_2, m_3, m_4;
	m_1 = PID_Cal( &PID_SPEED[PID_1], MOTOR_FEEDBACK[motor_1].speed_rpm, expect_speed_1); //PID算法 计算实际值与目标值之间的差距,从而使实际值趋向目标值
	m_2 = PID_Cal( &PID_SPEED[PID_2], MOTOR_FEEDBACK[motor_2].speed_rpm, expect_speed_2); //PID算法 计算实际值与目标值之间的差距,从而使实际值趋向目标值
	m_3 = PID_Cal( &PID_SPEED[PID_3], MOTOR_FEEDBACK[motor_3].speed_rpm, expect_speed_3); //PID算法 计算实际值与目标值之间的差距,从而使实际值趋向目标值
	m_4 = PID_Cal( &PID_SPEED[PID_4], MOTOR_FEEDBACK[motor_4].speed_rpm, expect_speed_4); //PID算法 计算实际值与目标值之间的差距,从而使实际值趋向目标值
		Motor_Set_Current(m_1,m_2,m_3,m_4); 
	
}

我用的是四个麦克纳轮(O型摆放),void underoan_control(float Vx, float Vy, float W)可以直接控制车子的X,Y,Z轴的速度,使用方便,详解请关注我的博客,谢谢!!!!

点赞,关注,转发后,私信问我送工程文件
也可以在我上传的资源中直接用积分下载

侵删,谢谢

  • 130
    点赞
  • 427
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 16
    评论
评论 16
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

Orange--Lin

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值