本周对手头的一款大疆M3508直流无刷电机调试的相关内容进行整理及个人的代码进行分享。
一、M3508直流无刷电机
直流无刷电机的工作原理此处不做阐述,相关资料也易查询。
1.1电机结构与连接样式图
1.2电机参数
具体不多加阐述,感兴趣的可到官网下载相关资料和软件驱动,连接如下:
1.3分电板
电源采用12V,通过一块分电板对单片机主板、电机和电源模块进行连接,这个分电板通过PCB简单设计,实现多少电压输入,多少电压输出。分电板的PCB图如下所示:
2.STM32程序:
控制一个电机,需要1个PWM,2个高低电位
2.1 控制原理图
反馈值(测量值)- 设定值(目标值)= 控制偏差;
控制偏差给到控制器;
从而将偏差给到电机,在经过编码器获得反馈;
实现闭环
相关pid.c代码如下:
#include"pid.h"
#define speed_max 482
struct _pid
{
float ExpectedValue;//定义设定值
float ActualValue;//定义实际值
float err;//定义偏差值
float err_prev;//定义前一个的偏差值
float integral;//定义积分项
float Kp, Ki, Kd;//定义比例、积分、微分系数
float curmax, curmin;//定义电流最值
}pid;
void PID_Init(){//初始化pid各值
pid.ExpectedValue=0.0;
pid.ActualValue = 0.0;
pid.err = 0.0;
pid.err_prev = 0.0;
pid.integral = 0.0;
pid.Kp = 1.0;
pid.Ki = 0.1;
pid.Kd = 0.2;
pid.curmax = 3000;
pid.curmin = -3000;
}
//限幅
void abs_limit(float *object, float abs_max)
{
if(*object > abs_max) *object = abs_max;
if(*object < -abs_max) *object = -abs_max;
}
/*
输入:期望转速
输出:位置型单环PID结果
*/
float PID_Realize(float set_speed,float get_speed){//ste_speed期望转速 £»get_speed实际转速
pid.ExpectedValue = set_speed;
pid.ActualValue = get_speed;
pid.err = pid.ExpectedValue-pid.ActualValue;//计算误差err
//计算积分项
pid.integral += pid.err;
//位置式PID公式
float Value = pid.Kp*pid.err + pid.Ki*pid.integral+pid.Kp*(pid.err-pid.err_prev);
//误差换代
pid.err_prev=pid.err;
if(Value<pid.curmin)
return pid.curmin;
else if(Value>pid.curmax)
return pid.curmax;
else
return Value;
}
/********************************************************************************************************/
void pid_struct_init(pid_t* pid, float kp, float ki, float kd)
{
pid->err[NOW] = 0;
pid->err[LAST] = 0;
pid->err[LLAST] = 0;
pid->pos_out = 0;
pid->pout = 0;
pid->iout = 0;
pid->dout = 0;
pid->p = kp;
pid->i = ki;
pid->d = kd;
pid->pos_abs = 0;
pid->pos_goal = 0;
}
//µ¥»·pid
float single_pid_calculate(pid_t * pid_s)
{
pid_s->err[NOW] = pid_s->target - pid_s->measure;//当前误差
//p项 比例项
pid_s->pout = pid_s->p * pid_s->err[NOW];
//i项 积分项
pid_s->iout += pid_s->i * pid_s->err[NOW];
//d项 微分项
pid_s->dout = pid_s->d * (pid_s->err[NOW] - pid_s->err[LAST]);
pid_s->pos_out = pid_s->pout + pid_s->iout + pid_s->dout;
//误差更新
pid_s->err[LAST] = pid_s->err[NOW];
abs_limit(&(pid_s->pos_out),3000);
return pid_s->pos_out;
}
//双环pid
float double_pid_calculate(pid_t * pid_I , pid_t* pid_O)//I是速度,o是角度
{
/****************************角度环**************************************/
pid_O->err[NOW] = pid_O->target - pid_O->measure;//当前误差
/*
//p项 比例项
pid_O->pout = pid_O->p * (pid_O->err[NOW] - pid_O->err[LAST]);
//i项 积分项
pid_O->iout += pid_O->i * pid_O->err[NOW];
//d项 微分项
pid_O->dout = pid_O->d * (pid_O->err[NOW] - 2*pid_O->err[LAST] + pid_O->err[LLAST]);
*/
if((pid_O->err[NOW]>-50) || (pid_O->err[NOW]<50))
{
pid_O->iout = 0; //设置死区
}
pid_O->pout = pid_O->p * pid_O->err[NOW];
pid_O->iout += pid_O->i * pid_O->err[NOW];
pid_O->dout = pid_O->d * (pid_O->err[NOW] - pid_O->err[LAST]);
pid_O->pos_out = pid_O->pout + pid_O->iout + pid_O->dout;//算出速度应设置的值
abs_limit(&(pid_O->pos_out),3000);
//误差更新
pid_O->err[LAST] = pid_O->err[NOW];
pid_O->err[LLAST] = pid_O->err[LAST];
/*******************************速度环************************************/
pid_I->err[NOW] = pid_O->pos_out - pid_I->measure;//µ±Ç°Îó²î
//p项 比例项
pid_I->pout = pid_I->p * pid_I->err[NOW];
//i项 积分项
pid_I->iout += pid_I->i * pid_I->err[NOW];
//d项 微分项
pid_I->dout = pid_I->d * (pid_I->err[NOW] - pid_I->err[LAST]);
pid_I->result =pid_I->pout + pid_I->iout + pid_I->dout;//应设置的电流值
//误差更新
pid_I->err[LAST] = pid_I->err[NOW];
abs_limit(&(pid_I->result),8000);
return pid_I->result;
}
pid.h
enum
{
LLAST = 0,
LAST = 1,
NOW = 2,
};
void PID_Init(void);
float PID_Realize(float current,float ActualValue);
typedef struct __pid_t
{
float p;
float i;
float d;
float target; //目标值
float measure; //测量值
float err[3]; //误差
float result;
float pout;
float iout;
float dout;
float pos_goal; //目标角度
float pos_abs; //绝对角度
float pos_out; //本次位置式输出
}pid_t;
typedef struct
{
float pos_goal;//目标位置
float pos_abs;//绝对位置
float pos_offset;//位置补偿
float eer;//误差
float eer_eer;//上次误差
}ANGLE_TypeDef;
void pid_struct_init(pid_t* pid, float kp, float ki, float kd);
float single_pid_calculate(pid_t * pid_s);
float double_pid_calculate(pid_t * pid_I , pid_t* pid_O);
主函数
#include "main.h"
#include "key.h"
#include "led.h"
#include "can.h"
#include "tim.h"
#include "pid.h"
int i;
unsigned char Temp_Value_3508 = { 0 };//CAN接受电机温度
short Torque_Value_3508 = { 0 }; //CAN实际扭矩电流
short Velocity_Value_3508 = { 0 }; //CAN接受实际转速
short Position_Value_3508 = { 0 }; //CAN接受机械角度
short Velocity_3508_ID1=0; //CAN实际转速
short Position_3508_ID1=0; //CAN接受机械角度
short Torque_3508_ID1=0; //CAN实际扭转电流
unsigned char Temp_Value_3508_ID1=0; //CAN接受电机温度
pid_t PID,PID_I,PID_O; //o为角度,i为速度
short set_current1; //PID计算后当前需要传输电流值
short Velocity_3508_ID1_Set_Speed=0; //设定转速
ANGLE_TypeDef motor_angle;
void CAN1_TX_IRQHandler( void ) //CAN发送中断 清楚发送中断标志
{
if ( CAN_GetITStatus( CAN1, CAN_IT_TME ) != RESET )
{
CAN_ClearITPendingBit( CAN1, CAN_IT_TME );
}
}
void CAN1_RX0_IRQHandler( void ) //CA接受中断 更新当前实际转速 且清楚接受中断标志位
{
CanRxMsg rx_message;
if ( CAN_GetITStatus( CAN1, CAN_IT_FMP0 ) != RESET )//判断是否是fifo0消息挂起中断(即收到消息)
{
CAN_ClearITPendingBit( CAN1, CAN_IT_FMP0 );
CAN_Receive( CAN1, CAN_FIFO0, &rx_message);
if ( (rx_message.IDE == CAN_Id_Standard) && (rx_message.RTR == CAN_RTR_Data) && (rx_message.DLC == 8) ) /* ±ê×¼Ö¡¡¢Êý¾ÝÖ¡¡¢Êý¾Ý³¤¶ÈΪ8 */
{
Temp_Value_3508 = rx_message.Data[6];
Torque_Value_3508 = (rx_message.Data[4] << 8) | (rx_message.Data[5]);
Velocity_Value_3508 = (rx_message.Data[2] << 8) | (rx_message.Data[3]);
Position_Value_3508 = (rx_message.Data[0] << 8) | (rx_message.Data[1]);
Velocity_3508_ID1=Velocity_Value_3508;
Position_3508_ID1=Position_Value_3508;
Torque_3508_ID1=Torque_Value_3508;
Temp_Value_3508_ID1=Temp_Value_3508;
PID.measure = Velocity_3508_ID1;//²âÁ¿Öµ
PID_I.measure = Velocity_Value_3508;
PID_O.measure = Position_Value_3508;
}
}
}
int main(void)
{
delay_init( 180 );
delay_ms( 100 );
KEY_Init();
Led_Init();
CAN1_Mode_Init(CAN_SJW_1tq,CAN_BS2_7tq,CAN_BS1_7tq,3,CAN_Mode_Normal); £¬Ò»°ã²»Óö¯£¬¶¼ÊÇ500kbps
delay_ms(100);
PID_Init();
pid_struct_init(&PID,1,0.1,0.2);//³õʼ»¯pidµÄÈý¸ö²ÎÊýÖµp£¬i£¬d
pid_struct_init(&PID_I,5,0.1,3);
pid_struct_init(&PID_O,10,0.1,5);
Tick_TIM7_Init(1000);
while (1){
if(key==0)
{
motor_angle.pos_goal = 0;
LED3=1;
LED4=0;
}
else if(key==1)
{
motor_angle.pos_goal = -2160;
LED3=0;
LED4=1;
}
delay_ms(10);
}
}
void Motor_Angle_Cal(float T)
{
float res1, res2;
static float pos,pos_old;
pos =(PID_O.measure/8192.0f*360.0f);
motor_angle.eer=pos - pos_old;//½Ç¶ÈÎó²îµÄ¼ÆËã
if(motor_angle.eer>0)
{
res1=motor_angle.eer-T;
res2=motor_angle.eer;
}
else
{
res1=motor_angle.eer+T;
res2=motor_angle.eer;
}
if(fabs(res1)<fabs(res2))
{
motor_angle.eer_eer = res1;
}
else
{
motor_angle.eer_eer = res2;
}
motor_angle.pos_abs += motor_angle.eer_eer;
motor_angle.pos_offset=motor_angle.pos_goal-motor_angle.pos_abs;
pos_old = pos;
}
void TIM7_IRQHandler( void )
{
if ( TIM_GetITStatus( TIM7, TIM_IT_Update ) == SET )
{
/******************µ¥»·******************/
// set_current1 = single_pid_calculate(&PID);
/******************Ë«»·******************/
Motor_Angle_Cal(360);
PID_O.target = motor_angle.pos_offset;
PID_O.measure = 0;
set_current1 = 0;
// set_current1=PID_Realize(Velocity_3508_ID1_Set_Speed,Velocity_3508_ID1);
CAN1_Send_Msg(set_current1,set_current1,set_current1,set_current1);//ÉèÖõçÁ÷Öµ
}
TIM_ClearITPendingBit( TIM7, TIM_IT_Update );
}