【FOC控制】英飞凌TC264无刷驱动方案simplefoc移植（5）-磁编码器移植

【FOC控制】英飞凌TC264无刷驱动方案simplefoc移植（5）-磁编码器移植
- 一、编码器选择
- 二、通讯方式 I²C接口
- 三、TC264测试全部程序
- - 3-1初始化定义
  - 3-2初始化函数
  - 3-3 as5600原始数据读取
  - 3-5角度解算
  - 3-6速度解算
  - 3-7速度滤波解算
  - 3-8软件IIC代码
- 四、总结

经过测试发现霍尔编码器的效果不太行，因为霍尔编码器读到的角度是离散的，导致由角度算得的速度也是离散的，二使用simplefoc最关键的数据就是角度和速度，所以我决定使用磁编码器完成下面的操作。

关于霍尔编码器的移植可以参考我之前的文章

一、【FOC控制】英飞凌TC264无刷驱动方案simplefoc移植（1）-霍尔编码器移植
二、逐飞科技TC264开源库

一、编码器选择

这里选用的一款常见的磁编码器，这款编码器在simplefoc的库中，arduino中比较常见。

引脚图
引脚含义

二、通讯方式 I²C接口

AS5600支持双线快速模式加I²C-slave

AS5600始终作为I²C总线上的从设备运行。通过开路漏极I/O连接到总线SDA和输入SC

主机MCU（主）启动数据传输，AS5600的地址是0x36（二进制的，0110110）。

三、TC264测试全部程序

3-1初始化定义

宏定义数据：
包括设备的地址，磁编码器的线数，角度寄存器的高位，角度寄存器的低位。
全局变量：
调用所需的函数：

/******************************************************************************/
#define Slave_Addr       0x36
#define  AS5600_CPR      4096
#define  RAW_Angle_Hi    0x0C
#define  RAW_Angle_Lo    0x0D/******************************************************************************/
extern long  cpr; //电机极对数
extern float full_rotation_offset;//周期角度 圈数
extern long  _angle_data_prev;// 更新角度时的前一次角度数据
extern unsigned long velocity_calc_timestamp;//前一次计算的速度的时刻
extern float _angle_prev;//更新速度时的前一次角度
/******************************************************************************/
void AS5600_Init ( void );
float as5600_getAngle(void);
float as5600_getVelocity(void);
/******************************************************************************/

3-2初始化函数

iic总线初始化，这里使用的软件iic初始化，软件iic的程序在展示。
清除圈数和角度数据

void AS5600_Init ( void )
{IIC_Init();_angle_data_prev = 0;full_rotation_offset = 0;velocity_calc_timestamp=0;cpr=AS5600_CPR;
}

3-3 as5600原始数据读取

读取原始数据的时候，直接读取从设备中寄存器中的数据，寄存器的地址 RAW_Angle_Hi 0x0C RAW_Angle_Low 0x0D

因为iic总线一次读的数据是八位的，而实际的角度数据是一个十六位的数据，所以我们只需要读取两次iic总线的数据，然后通过移位加或的操作(dh<<8)+dl就可以得到原始数据了。

/***************************************************************************/
uint8 AS5600_ReadOneByte(uint8 addr)
{uint8 temp;IIC_Start();IIC_SendByte(Slave_Addr<<1);IIC_WaitAck();IIC_SendByte(addr);IIC_WaitAck();IIC_Start();IIC_SendByte((Slave_Addr<<1)+1);IIC_WaitAck();temp=IIC_ReadByte(0);IIC_Stop();return temp;
}
/***************************************************************************/
uint16 AS5600_ReadRawAngleTwo(void)
{uint8 dh,dl;IIC_Start();IIC_SendByte(Slave_Addr<<1);IIC_WaitAck();IIC_SendByte(RAW_Angle_Hi);IIC_WaitAck();IIC_Start();IIC_SendByte((Slave_Addr<<1)+1);IIC_WaitAck();dh=IIC_ReadByte(1);   //1-ack for next bytedl=IIC_ReadByte(0);   //0-end transIIC_Stop();return ((dh<<8)+dl);
}
/***************************************************************************/

3-5角度解算

角度解算的逻辑是，先读取编码器原始数据，然后计算角度的变化量，如果角度的变化量fabs(d_angle) > (0.8*cpr),那么这个时候就认为磁编码器又转过了一圈。

磁编码器一般为增量式编码器，即从0->4096->0，会有一个4096的跳变，二计算角度需要将这个变化检测出来。

最后就是通过计算得出角度，注意此时的单位为弧度！！！！

/******************************************************************************/
float as5600_getAngle(void)
{float angle_data,d_angle;float return_speed;angle_data = AS5600_ReadRawAngleTwo();// tracking the number of rotations// in order to expand angle range form [0,2PI] to basically infinityd_angle = angle_data - _angle_data_prev;// if overflow happened track it as full rotationif(fabs(d_angle) > (0.8*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI;// save the current angle value for the next steps// in order to know if overflow happened_angle_data_prev = angle_data;// return the full angle// (number of full rotations)*2PI + current sensor angle
//    printf ( "angle:%0.2f,%0.2f, ", angle_data,d_angle);return_speed = (full_rotation_offset + (( angle_data / (float)cpr) * _2PI)) ;return  return_speed;}

3-6速度解算

速度的计算核心就是，得到两次的角度数据，然后除以时间。

时间的获取源自逐飞的#include <zf_stm_systick.h>文件中，直接调用即可

******************************************************************************/
// Shaft velocity calculation
float as5600_getVelocity(void)
{unsigned long now_us;float Ts, angle_c, vel;// calculate sample timenow_us = systick_getval_us(STM0);//_micros();if(now_us<velocity_calc_timestamp)Ts = (float)(velocity_calc_timestamp - now_us)/9*1e-6;elseTs = (float)(0xFFFFFF - now_us + velocity_calc_timestamp)/9*1e-6;// quick fix for strange cases (micros overflow)if(Ts == 0 || Ts > 0.5) Ts = 1e-3;// current angleangle_c = as5600_getAngle();// velocity calculationvel = (angle_c - _angle_prev)/Ts;// save variables for future pass_angle_prev = angle_c;velocity_calc_timestamp = now_us;return vel;
}
/******************************************************************************/

3-7速度滤波解算

速度滤波主要采用低通滤波器实现

float LPF_velocity(float x)
{y = 0.7*y_vel_prev + 0.3*x;y_vel_prev=y;return y;
}

sensor_direction是电机转向的方向，该函数返回滤波之后的速度

// shaft velocity calculation
float shaftVelocity(void)
{float as_speed,lpf_speed;as_speed = as5600_getVelocity();// if no sensor linked return previous value ( for open loop )//if(!sensor) return shaft_velocity;lpf_speed = sensor_direction*LPF_velocity(as_speed);
//    printf ( "angle:%0.2f,%0.2f\n", as_speed,lpf_speed);return lpf_speed;
}

3-8软件IIC代码

myiic.h

#include "zf_gpio.h"/* 注意 IIC总线规定，IIC空闲时 SCL和SDA都为高电平 最好外部上拉（一定不能下拉） */
/* 模拟 IIC需要注意，IIC地址左移一位 例如MPU6050 模拟就是地址 0xD0 */
/* 想换用别的IO 直接修改宏定义 SOFT_IIC_SCL_PIN 、 SOFT_IIC_SDA_PIN 即可 */
#define IIC_SCL_PIN   P33_11   /*!< P13_1  作为 SCL */
#define IIC_SDA_PIN   P33_5   /*!< P13_2  作为 SDA */#define SDA_OUT        gpio_dir (IIC_SDA_PIN, GPO, PUSHPULL)   //输出方向
#define SDA_IN         gpio_dir (IIC_SDA_PIN, GPI, NO_PULL)    //输入方向#define IIC_SCL_INIT   gpio_init (IIC_SCL_PIN, GPO, 1, PUSHPULL);
#define IIC_SDA_INIT   gpio_init (IIC_SDA_PIN, GPO, 1, PUSHPULL);#define IIC_SCL_H      gpio_set (IIC_SCL_PIN, 1)        //IO口输出高电平
#define IIC_SCL_L      gpio_set (IIC_SCL_PIN, 0)     //IO口输出低电平#define IIC_SDA_H       gpio_set (IIC_SDA_PIN, 1)       //IO口输出高电平
#define IIC_SDA_L       gpio_set (IIC_SDA_PIN, 0)       //IO口输出低电平#define IIC_SDA_READ   gpio_get(IIC_SDA_PIN)void          IIC_Init(void);
void          IIC_Start(void);               //发送IIC开始信号
void          IIC_Stop(void);                //发送IIC停止信号
void          IIC_Ack(void);                 //IIC发送ACK信号
void          IIC_NAck(void);                //IIC不发送ACK信号
unsigned char IIC_WaitAck(void);            //IIC等待ACK信号
void          IIC_SendByte(unsigned char data_t);    //IIC发送一个字节
unsigned char IIC_ReadByte(unsigned char ack);      //IIC读取一个字节unsigned char IIC_ReadByteFromSlave(unsigned char I2C_Addr,unsigned char reg,unsigned char *buf);
unsigned char IIC_ReadMultByteFromSlave(unsigned char dev, unsigned char reg, unsigned char length, unsigned char *data_t);
unsigned char IIC_WriteByteToSlave(unsigned char I2C_Addr,unsigned char reg,unsigned char buf);
unsigned char IIC_WriteMultByteToSlave(unsigned char dev, unsigned char reg, unsigned char length, unsigned char* data_t);

myiic.c

/*************************************************************************
*  函数名称：IIC延时
*  功能说明：ADC初始化函数
*  参数说明：us      ：  延时时间
*  函数返回：无
*  修改时间：2020年3月10日
*  应用举例：iic_delay(1);  //简单的延时
*  内部调用 修改这里可以调整IIC速率
*************************************************************************/
void iic_delay()
{/* 200MHz 系统时钟下 模拟IIC速度为 400Khz */unsigned char  i = 0;for(i = 0; i < 30; i++) //修改这里可以调整IIC速率{__asm("NOP"); /* delay */}
}/*************************************************************************
*  函数名称：void IIC_Init(void)
*  功能说明：模拟IIC初始化
*  参数说明：无
*  函数返回：无
*  修改时间：2020年3月10日
*  应用举例：IIC_Init();  //模拟IIC初始化 IIC管脚在LQ_SOFTI2C.h中定义
*************************************************************************/
void IIC_Init(void)
{IIC_SCL_INIT;IIC_SDA_INIT;IIC_SCL_H;IIC_SDA_H;
}/*************************************************************************
*  函数名称：void IIC_Start(void)
*  功能说明：模拟IIC起始信号
*  参数说明：无
*  函数返回：无
*  修改时间：2020年3月10日
*  应用举例：IIC_Start();
*************************************************************************/
void IIC_Start(void)
{SDA_OUT;   //sda线输出IIC_SDA_H;IIC_SCL_H;iic_delay();iic_delay();iic_delay();iic_delay();iic_delay();IIC_SDA_L; //START:when CLK is high,DATA change form high to lowiic_delay();iic_delay();iic_delay();IIC_SCL_L; //钳住I2C总线，准备发送或接收数据
}/*************************************************************************
*  函数名称：void IIC_Stop(void)
*  功能说明：模拟IIC停止信号
*  参数说明：无
*  函数返回：无
*  修改时间：2020年3月10日
*  应用举例：IIC_Stop();
*************************************************************************/
void IIC_Stop(void)
{SDA_OUT; //sda线输出IIC_SCL_L;IIC_SDA_L; //STOP:when CLK is high DATA change form low to highiic_delay();iic_delay();iic_delay();IIC_SCL_H;iic_delay();iic_delay();iic_delay();IIC_SDA_H; //发送I2C总线结束信号iic_delay();
}/*************************************************************************
*  函数名称：unsigned char IIC_WaitAck(void)
*  功能说明：模拟IIC等待应答信号
*  参数说明：无
*  函数返回：1，接收应答失败    0，接收应答成功
*  修改时间：2020年3月10日
*  应用举例：内部调用 有效应答：从机第9个 SCL=0 时 SDA 被从机拉低,并且 SCL = 1时 SDA依然为低
*************************************************************************/
unsigned char IIC_WaitAck(void)
{unsigned char  ucErrTime=0;SDA_IN; //SDA设置为输入  （从机给一个低电平做为应答）IIC_SDA_H;iic_delay();IIC_SCL_H;iic_delay();while(IIC_SDA_READ){ucErrTime++;if(ucErrTime>100){IIC_Stop();return 1;}}IIC_SCL_L; //时钟输出0return 0;
} /*************************************************************************
*  函数名称：void IIC_Ack(void)
*  功能说明：模拟IIC产生ACK应答
*  参数说明：无
*  函数返回：无
*  修改时间：2020年3月10日
*  应用举例：内部调用 主机接收完一个字节数据后，主机产生的ACK通知从机一个字节数据已正确接收
*************************************************************************/
void IIC_Ack(void)
{IIC_SCL_L;SDA_OUT;IIC_SDA_L;iic_delay();iic_delay();iic_delay();IIC_SCL_H;iic_delay();iic_delay();iic_delay();IIC_SCL_L;
}/*************************************************************************
*  函数名称：void IIC_NAck(void)
*  功能说明：模拟IIC不产生ACK应答
*  参数说明：无
*  函数返回：无
*  修改时间：2020年3月10日
*  应用举例：内部调用 主机接收完最后一个字节数据后，主机产生的NACK通知从机发送结束，释放SDA,以便主机产生停止信号
*************************************************************************/
void IIC_NAck(void)
{IIC_SCL_L;SDA_OUT;IIC_SDA_H;iic_delay();iic_delay();iic_delay();IIC_SCL_H;iic_delay();iic_delay();iic_delay();IIC_SCL_L;
}/*************************************************************************
*  函数名称：void IIC_SendByte(unsigned char data_t)
*  功能说明：模拟IIC发送一个字节
*  参数说明：data   :  发送的字节
*  函数返回：无
*  修改时间：2020年3月10日
*  应用举例：IIC_SendByte(0x12);
*************************************************************************/
void IIC_SendByte(unsigned char data_t)
{                        unsigned char  t;SDA_OUT;IIC_SCL_L; //拉低时钟开始数据传输for(t=0;t<8;t++){//        IIC_SDA_READ = data_t&0x80;if(data_t&0x80){IIC_SDA_H;}else{IIC_SDA_L;}IIC_SCL_H;;iic_delay();data_t<<=1;iic_delay();iic_delay();IIC_SCL_L;iic_delay();}iic_delay();
}/*************************************************************************
*  函数名称：unsigned char IIC_ReadByte(unsigned char ack)
*  功能说明：模拟IIC读取一个字节
*  参数说明：ack=1 时，主机数据还没接收完 ack=0 时主机数据已全部接收完成
*  函数返回：接收到的字节
*  修改时间：2020年3月10日
*  应用举例：IC_ReadByte(0x12);
*************************************************************************/
unsigned char IIC_ReadByte(unsigned char ack)
{unsigned char  i,receive=0;SDA_IN; //SDA设置为输入模式 等待接收从机返回数据for(i=0;i<8;i++ ){IIC_SCL_L;iic_delay();iic_delay();iic_delay();IIC_SCL_H;receive<<=1;if(IIC_SDA_READ)receive++; //从机发送的电平iic_delay();}if(ack)IIC_Ack(); //发送ACKelseIIC_NAck(); //发送nACKreturn receive;
}/*************************************************************************
*  函数名称：void ADC_init(void)
*  功能说明：模拟IIC读取指定设备 指定寄存器的一个值
*  参数说明：* @param    I2C_Addr  目标设备地址* @param    reg       目标寄存器* @param    buf       存放读出字节
*  函数返回：1失败 0成功
*  修改时间：2020年3月10日
*  应用举例：IIC_ReadByteFromSlave(0xD0, 0x75, &data);   //读 IIC地址为 0xD0器件（MPU6050）寄存器0x75
*************************************************************************/
unsigned char IIC_ReadByteFromSlave(unsigned char I2C_Addr,unsigned char reg,unsigned char *buf)
{IIC_Start();IIC_SendByte(I2C_Addr);  //发送从机地址if(IIC_WaitAck()) //如果从机未应答则数据发送失败{IIC_Stop();return 1;}IIC_SendByte(reg); //发送寄存器地址IIC_WaitAck();IIC_Start();IIC_SendByte(I2C_Addr+1); //进入接收模式IIC_WaitAck();*buf=IIC_ReadByte(0);IIC_Stop(); //产生一个停止条件return 0;
}/*************************************************************************
*  函数名称：void ADC_init(void)
*  功能说明：模拟IIC写指定设备 指定寄存器的一个值
*  参数说明：* @param    I2C_Addr  目标设备地址* @param    reg       目标寄存器* @param    data      写入的数据
*  函数返回：1失败 0成功
*  修改时间：2020年3月10日
*  应用举例：IIC_ReadByteFromSlave(0xD0, 0X6B, 0X80);   //IIC地址为 0xD0器件（MPU6050）寄存器0x6B 写入0x80
*************************************************************************/
unsigned char IIC_WriteByteToSlave(unsigned char I2C_Addr,unsigned char reg,unsigned char data_t)
{IIC_Start();IIC_SendByte(I2C_Addr); //发送从机地址if(IIC_WaitAck()){IIC_Stop();return 1; //从机地址写入失败}IIC_SendByte(reg); //发送寄存器地址IIC_WaitAck();IIC_SendByte(data_t);if(IIC_WaitAck()){IIC_Stop();return 1; //数据写入失败}IIC_Stop(); //产生一个停止条件//return 1; //status == 0;return 0;
}/*************************************************************************
*  函数名称：unsigned char IIC_ReadMultByteFromSlave(unsigned char dev, unsigned char reg, unsigned char length, unsigned char *data_t)
*  功能说明：模拟IIC读取指定设备 指定寄存器的n个值
*  参数说明：* @param    dev       目标设备地址* @param    reg       目标寄存器* @param    length    读取长度* @param    data      存放读出数据
*  函数返回：1失败 0成功
*  修改时间：2020年3月10日
*  应用举例：IIC_ReadByteFromSlave(0xD0, 0X3B, 14, &data);   //读 14个字节
*************************************************************************/
unsigned char IIC_ReadMultByteFromSlave(unsigned char dev, unsigned char reg, unsigned char length, unsigned char *data_t)
{unsigned char  count = 0;unsigned char  temp;IIC_Start();IIC_SendByte(dev); //发送从机地址if(IIC_WaitAck()){IIC_Stop();return 1; //从机地址写入失败}IIC_SendByte(reg); //发送寄存器地址IIC_WaitAck();IIC_Start();IIC_SendByte(dev+1); //进入接收模式IIC_WaitAck();for(count=0;count<length;count++){if(count!=(length-1))temp = IIC_ReadByte(1); //带ACK的读数据elsetemp = IIC_ReadByte(0); //最后一个字节NACKdata_t[count] = temp;}IIC_Stop(); //产生一个停止条件//return count;return 0;
}/*************************************************************************
*  函数名称：unsigned char IIC_WriteMultByteToSlave(unsigned char dev, unsigned char reg, unsigned char length, unsigned char* data_t)
*  功能说明：模拟IIC写指定设备 指定寄存器的n个值
*  参数说明：* @param    dev       目标设备地址* @param    reg       目标寄存器* @param    length    写入长度* @param    data      存放写入数据
*  函数返回： 1失败 0成功
*  修改时间：2020年3月10日
*  应用举例：IIC_WriteMultByteToSlave(0xD0, 0X6B, 1, 0X80);   //向寄存器0x6B写入0x80
*************************************************************************/
unsigned char IIC_WriteMultByteToSlave(unsigned char dev, unsigned char reg, unsigned char length, unsigned char* data_t)
{unsigned char  count = 0;IIC_Start();IIC_SendByte(dev); //发送从机地址if(IIC_WaitAck()){IIC_Stop();return 1; //从机地址写入失败}IIC_SendByte(reg); //发送寄存器地址IIC_WaitAck();for(count=0;count<length;count++){IIC_SendByte(data_t[count]);if(IIC_WaitAck()) //每一个字节都要等从机应答{IIC_Stop();return 1; //数据写入失败}}IIC_Stop(); //产生一个停止条件return 0;
}

四、总结

最后本次关于tc264系列移植磁编码器的源代码已经完全开放，但是需要注意的是，磁铁的选择与安装，尽量选择 **圆形强力磁铁高强度薄钕铁贴片 **
因为磁编码器的关键还是去检测磁场的变化，所以当有代码无法显示的时候，先可以检测硬件设备。