代码收藏家 CAN总线控制伺服电机详解
硬件
用的是C12B驱动器，支持485/CAN，PWM，232通信。
可以CAN通信的伺服电机
CAN发送格式，
上面是在说明书上截取的部分图片。
用的STM32F103系列芯片，参考杨桃电子的CAN通信驱动，
双模式
` //位置模式
buff[0]=0x00;
buff[1]=0x1A;//写数据不保存
buff[2]=0x50;//位置模式选择/0x50
buff[3]=0x00;
buff[4]=0x00;//0xd0PC数字输入/0xc0外部脉冲
buff[5]=0x05;//位置调试模式低16位数据输入
buff[6]=0x27;
buff[7]=0x10;
CAN_Send_Msg(buff,8);
//力矩模式
buff[0]=0x00;
buff[1]=0x1A;//写数据不保存
buff[2]=0x06;
buff[3]=speed>>8;
buff[4]=speed;
buff[5]=0x00;
buff[6]=0x00;
buff[7]=0x01;
`
期间我还为CAN通信加了一个PID，但是伺服电机可以不加软件PID，伺服电机与驱动器会形成闭环，我们只需要接收驱动器发回来的数据就可以了。
PID
``
/
`****************************(C) COPYRIGHT 2019 DJI****************************`
* `@file pid.c/h`
* `@brief pid实现函数，包括初始化，PID计算函数，`
`*/`
`#include "pid.h"`
`#include "main.h"`
`/**`
* `@brief pid struct data init`
* `@param[out] pid: PID struct data point`
* `@param[in] mode: PID_POSITION: normal pid`
* `PID_DELTA: delta pid`
* `@param[in] PID: 0: kp, 1: ki, 2:kd`
* `@param[in] max_out: pid max out`
* `@param[in] max_iout: pid max iout`
* `@retval none`
`*/`
`/**`
* `@brief pid struct data init`
* `@param[out] pid: PID结构数据指针`
* `@param[in] mode: PID_POSITION:普通PID`
* `PID_DELTA: 差分PID`
* `@param[in] PID: 0: kp, 1: ki, 2:kd`
* `@param[in] max_out: pid最大输出`
* `@param[in] max_iout: pid最大积分输出`
* `@retval none`
`*/`
`void PID_init(pid_type_def *pid, uint8_t mode, const fp32 PID[3], fp32 max_out, fp32 max_iout)`
`{`
`if (pid == NULL || PID == NULL)`
`{`
`return;`
`}`
`pid->mode = mode;`
`pid->Kp = PID[0];`
`pid->Ki = PID[1];`
`pid->Kd = PID[2];`
`pid->max_out = max_out;`
`pid->max_iout = max_iout;`
`pid->Dbuf[0] = pid->Dbuf[1] = pid->Dbuf[2] = 0.0f;`
`pid->error[0] = pid->error[1] = pid->error[2] = pid->Pout = pid->Iout = pid->Dout = pid->out = 0.0f;`
`}`
`/**`
* `@brief pid calculate`
* `@param[out] pid: PID struct data point`
* `@param[in] ref: feedback data`
* `@param[in] set: set point`
* `@retval pid out`
`*/`
`/**`
* `@brief pid计算`
* `@param[out] pid: PID结构数据指针`
* `@param[in] ref: 反馈数据`
* `@param[in] set: 设定值`
* `@retval pid输出`
`*/`
`fp32 PID_calc(pid_type_def *pid, fp32 ref, fp32 set)`
`{`
`if (pid == NULL)`
`{`
`return 0.0f;`
`}`
`pid->error[2] = pid->error[1];`
`pid->error[1] = pid->error[0];`
`pid->set = set;`
`pid->fdb = ref;`
`pid->error[0] = set - ref;`
`if (pid->mode == PID_POSITION)`
`{`
`pid->Pout = pid->Kp * pid->error[0];`
`pid->Iout += pid->Ki * pid->error[0];`
`pid->Dbuf[2] = pid->Dbuf[1];`
`pid->Dbuf[1] = pid->Dbuf[0];`
`pid->Dbuf[0] = (pid->error[0] - pid->error[1]);`
`pid->Dout = pid->Kd * pid->Dbuf[0];`
`LimitMax(pid->Iout, pid->max_iout);`
`pid->out = pid->Pout + pid->Iout + pid->Dout;`
`LimitMax(pid->out, pid->max_out);`
`}`
`else if (pid->mode == PID_DELTA)`
`{`
`pid->Pout = pid->Kp * (pid->error[0] - pid->error[1]);`
`pid->Iout = pid->Ki * pid->error[0];`
`pid->Dbuf[2] = pid->Dbuf[1];`
`pid->Dbuf[1] = pid->Dbuf[0];`
`pid->Dbuf[0] = (pid->error[0] - 2.0f * pid->error[1] + pid->error[2]);`
`pid->Dout = pid->Kd * pid->Dbuf[0];`
`pid->out += pid->Pout + pid->Iout + pid->Dout;`
`LimitMax(pid->out, pid->max_out);`
`}`
`return pid->out;`
````
`}`
`/**`
* `@brief pid out clear`
* `@param[out] pid: PID struct data point`
* `@retval none`
`*/`
`/**`
* `@brief pid 输出清除`
* `@param[out] pid: PID结构数据指针`
* `@retval none`
`*/`
`void PID_clear(pid_type_def *pid)`
`{`
`if (pid == NULL)`
`{`
`return;`
`}`
`pid->error[0] = pid->error[1] = pid->error[2] = 0.0f;`
`pid->Dbuf[0] = pid->Dbuf[1] = pid->Dbuf[2] = 0.0f;`
`pid->out = pid->Pout = pid->Iout = pid->Dout = 0.0f;`
`pid->fdb = pid->set = 0.0f;`
``}``
CAN的发送和接收
`#include "can.h"
#include "delay.h"
#include "oled0561.h"
#include "stm32f10x_can.h"
#include "pid.h"
//#include "main.h"
u8 x;
motor_measure_t motor_chassis[7];
extern pid_type_def chassis_pid[4];
extern int s;
extern int16_t speedset;
// u8 CAN_ReceiveBuff[8];//有八个数据
u8 CAN1_Configuration(void){ //CAN初始化（返回0表示设置成功，返回其他表示失败）
GPIO_InitTypeDef GPIO_InitStructure;
CAN_InitTypeDef CAN_InitStructure;
CAN_FilterInitTypeDef CAN_FilterInitStructure;
#if CAN_INT_ENABLE
NVIC_InitTypeDef NVIC_InitStructure;
#endif
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); //使能PORTA时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_CAN1, ENABLE); //使能CAN1时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽
GPIO_Init(GPIOA, &GPIO_InitStructure); //初始化IO
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; //上拉输入
GPIO_Init(GPIOA, &GPIO_InitStructure); //初始化IO
//CAN单元设置
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_Mode_Normal 普通模式，CAN_Mode_LoopBack 回环模式;
//设置波特率
CAN_InitStructure.CAN_SJW=tsjw; //重新同步跳跃宽度(Tsjw)为tsjw+1个时间单位 CAN_SJW_1tq CAN_SJW_2tq CAN_SJW_3tq CAN_SJW_4tq
CAN_InitStructure.CAN_BS1=tbs1; //Tbs1=tbs1+1个时间单位CAN_BS1_1tq ~ CAN_BS1_16tq
CAN_InitStructure.CAN_BS2=tbs2; //Tbs2=tbs2+1个时间单位CAN_BS2_1tq ~ CAN_BS2_8tq
CAN_InitStructure.CAN_Prescaler=brp; //分频系数(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); //滤波器初始化
#if CAN_INT_ENABLE //以下是用于CAN中断方式接收的设置
CAN_ITConfig(CAN1,CAN_IT_FMP0,ENABLE); //FIFO0消息挂号中断允许.
NVIC_InitStructure.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; // 主优先级为1
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; // 次优先级为0
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
#endif
return 0;
}
//电机运行程序
u8 buff[8];//有八个数据
void motor_can(u16 speed){
buff[0]=0x00;
buff[1]=0x1A;
buff[2]=0x06;
buff[3]=speed>>8;
buff[4]=speed;
buff[5]=0x00;
buff[6]=0x00;
buff[7]=0x00;
CAN_Send_Msg(buff,8);
delay_s(2);
buff[0]=0x00;
buff[1]=0x1A;
buff[2]=0x06;
buff[3]=speed>>8;
buff[4]=speed;
buff[5]=0x00;
buff[6]=0x00;
buff[7]=0x01;
CAN_Send_Msg(buff,8);
// //位置模式
// buff[0]=0x00;
// buff[1]=0x1A;//写数据不保存
// buff[2]=0x50;//位置模式选择/0x50
// buff[3]=0x00;
// buff[4]=0x00;//0xd0PC数字输入/0xc0外部脉冲
// buff[5]=0x05;//位置调试模式低16位数据输入
// buff[6]=0x27;
// buff[7]=0x10;
// CAN_Send_Msg(buff,8);
// //力矩模式
// buff[0]=0x00;
// buff[1]=0x1A;//写数据不保存
// buff[2]=0x06;
// buff[3]=speed>>8;
// buff[4]=speed;
// buff[5]=0x00;
// buff[6]=0x00;
// buff[7]=0x01;
//
delay_s(5);
}
//CAN发送一组数据(固定格式:ID为0X12,标准帧,数据帧)
//msg:数据指针,最大为8个字节，len:数据长度(最大为8)
//返回值:0,成功; 其他,失败;
u8 CAN_Send_Msg(u8* msg,u8 len){ //u8* msg 指向要发送的数据部分
u8 mbox;
u16 i=0;
CanTxMsg TxMessage;
TxMessage.StdId=5; // 标准标识符
TxMessage.ExtId=0x000; // 设置扩展标识符
TxMessage.IDE=CAN_Id_Standard; // 标准帧
TxMessage.RTR=CAN_RTR_Data; // 数据帧
TxMessage.DLC=len; // 要发送的数据长度
for(i=0;i<len;i++)
TxMessage.Data[i]=msg[i]; //写入数据
mbox= CAN_Transmit(CAN1,&TxMessage);
i=0;
while((CAN_TransmitStatus(CAN1,mbox)==CAN_TxStatus_Failed)&&(i<0XFFF))i++; //等待发送结束
if(i>=0XFFF)return 1;
return 0;
}
//can口接收数据查询
//buf:数据缓存区;
//返回值:0,无数据被收到，其他,接收的数据长度;
u8 CAN_Receive_Msg(u8 *buf){
u32 i;
CanRxMsg RxMessage;
if(CAN_MessagePending(CAN1,CAN_FIFO0)==0)return 0;//没有接收到数据,直接退出
CAN_Receive(CAN1,CAN_FIFO0,&RxMessage);//读取数据
for(i=0;i<8;i++) //把8个数据放入参数数组
buf[i]=RxMessage.Data[i];
return RxMessage.DLC; //返回数据数量
}
#define get_motor_measure(ptr, data) \
{ \
(ptr)->last_ecd = (ptr)->ecd; \
(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]); \
(ptr)->speed_rpm = (uint16_t)((data)[6] <<8 | (data)[7]); \
(ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]); \
(ptr)->temperate = (data)[5]; \
}//
//CAN的中断接收程序（中断处理程序）
//必须在can.h文件里CAN_INT_ENABLE为1才能使用中断
//数据处理尽量在中断函数内完成，外部处理要在处理前关CAN中断，防止数据覆盖
void USB_LP_CAN1_RX0_IRQHandler(void){
CanRxMsg RxMessage;
vu8 CAN_ReceiveBuff[8]; //CAN总线中断接受的数据寄存器
vu8 i = 0;
vu8 u8_RxLen = 0;
CAN_ReceiveBuff[0] = 0; //清空寄存器
RxMessage.StdId = 5;
RxMessage.ExtId = 0x00;
RxMessage.IDE = 0;
RxMessage.RTR = 0;
RxMessage.DLC = 0;
RxMessage.FMI = 0;
for(i=0;i<8;i++){
RxMessage.Data[i]=0x00;
}
CAN_Receive(CAN1,CAN_FIFO0,&RxMessage); //读出FIFO0数据
u8_RxLen = RxMessage.DLC; //读出数据数量
if(RxMessage.StdId==5){//判断ID是否一致
CAN_ReceiveBuff[i] = RxMessage.DLC; //将收到数据数量放到数组0的位置
for( i=0;i<u8_RxLen; i++){ //将收到的数据存入CAN寄存器
CAN_ReceiveBuff[i] = RxMessage.Data[i]; //将8位数据存入CAN接收寄存器
// //CAN查寻方式的接收处理
get_motor_measure(&motor_chassis[i], CAN_ReceiveBuff);
//if(motor_chassis[0].temperate==0xe4){
// motor_chassis[0].speed_rpm=200;
OLED_DISPLAY_8x16(6,8*8,motor_chassis[0].speed_rpm/100+0x30);//
OLED_DISPLAY_8x16(6,9*8,(motor_chassis[0].speed_rpm%100)/10+0x30);//
OLED_DISPLAY_8x16(6,10*8,motor_chassis[0].speed_rpm%10+0x30);//
// OLED_DISPLAY_8x16(6,11*8,CAN_ReceiveBuff[7]%10+0x30);//
OLED_DISPLAY_8x16(6,12*8,'n');//
//}
// s=115;
OLED_DISPLAY_8x16(6,13*8,s/100+0x30);//
OLED_DISPLAY_8x16(6,14*8,(s%100)/10+0x30);//
OLED_DISPLAY_8x16(6,15*8,s%10+0x30);//
PID_calc(&chassis_pid[0],motor_chassis[0].speed_rpm, speedset);//----pid计算
motor_can(chassis_pid[0].out);
delay_ms(1000);
s=266;
delay_ms(100);
NVIC_DisableIRQ(USB_LP_CAN1_RX0_IRQn); //失能CAN1消息接收中断 USB_LP_CAN1_RX0_IRQn
CAN_ClearITPendingBit(CAN1,CAN_IT_FMP0);//清除中断标志位
CAN_FIFORelease(CAN1,CAN_FIFO0);//释放FIFO0
CAN_ITConfig(CAN1,CAN_IT_FMP0, DISABLE);
// NVIC_InitTypeDef.NVIC_IRQChannelCmd = ENABLE;
}
}
}
`
主函数
`#include "stm32f10x.h" //STM32头文件
#include "sys.h"
#include "delay.h"
#include "touch_key.h"
#include "relay.h"
#include "oled0561.h"
#include "main.h"
#include "can.h"
#include "pid.h"
#include "struct_typedef.h"
vu8 CAN_ReceiveBuff[8];
pid_type_def chassis_pid[4];
uint8_t i=0;
int s;
extern motor_measure_t motor_chassis[7];
int16_t speed_rpm;
int16_t speedset=200; //
const static fp32 PID[3] = {5.0f, 0.0f, 0.0f};
int main (void){//主程序
extern u8 buff[8];//有八个数据
// u8 x;
delay_ms(100); //上电时等待其他器件就绪
RCC_Configuration(); //系统时钟初始化
// TOUCH_KEY_Init();//触摸按键初始化
RELAY_Init();//继电器初始化
CAN1_Configuration(); //CAN总线初始化 返回0表示成功
I2C_Configuration();//I2C初始化
OLED0561_Init(); //OLED初始化
OLED_DISPLAY_8x16_BUFFER(0," dgut-lyl "); //显示字符串
OLED_DISPLAY_8x16_BUFFER(2," CAN TEST "); //显示字符串
OLED_DISPLAY_8x16_BUFFER(6," speed:"); //显示字符串
PID_init(&chassis_pid[i], PID_DELTA, PID, 16000.0f, 2000.0f);
while(1){
//
PID_calc(&chassis_pid[0],motor_chassis[0].speed_rpm,speedset);//----pid计算
motor_can(chassis_pid[0].out);
delay_ms(1000);
s=266;
}`
最终效果
nit(); //OLED初始化
OLED_DISPLAY_8x16_BUFFER(0," dgut-lyl “); //显示字符串
OLED_DISPLAY_8x16_BUFFER(2,” CAN TEST “); //显示字符串
OLED_DISPLAY_8x16_BUFFER(6,” speed:"); //显示字符串
PID_init(&chassis_pid[i], PID_DELTA, PID, 16000.0f, 2000.0f);
while(1){
//
PID_calc(&chassis_pid[0],motor_chassis[0].speed_rpm,speedset);//—-pid计算
motor_can(chassis_pid[0].out);
delay_ms(1000);
s=266;
}`
最终效果
[外链图片转存中…(img-SQsUhaAr-1680877483076)]
