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

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

经过测试发现霍尔编码器的效果不太行，因为霍尔编码器读到的角度是离散的，导致由角度算得的速度也是离散的，二使用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);
/******************************************************************************/
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

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;
}
1
2
3
4
5
6
7
8

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 byte
    dl=IIC_ReadByte(0);   //0-end trans
    IIC_Stop();

    return ((dh<<8)+dl);
}
/***************************************************************************/

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39

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 infinity
    d_angle = angle_data - _angle_data_prev;
    // if overflow happened track it as full rotation
    if(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;

}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

3-6速度解算

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

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

******************************************************************************/
// Shaft velocity calculation
float as5600_getVelocity(void)
{
    unsigned long now_us;
    float Ts, angle_c, vel;

    // calculate sample time
    now_us = systick_getval_us(STM0);//_micros();
    if(now_us<velocity_calc_timestamp)Ts = (float)(velocity_calc_timestamp - now_us)/9*1e-6;
    else
        Ts = (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 angle
    angle_c = as5600_getAngle();
    // velocity calculation
    vel = (angle_c - _angle_prev)/Ts;

    // save variables for future pass
    _angle_prev = angle_c;
    velocity_calc_timestamp = now_us;

    return vel;
}
/******************************************************************************/

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

3-7速度滤波解算

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

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

	y_vel_prev=y;

	return y;
}
1
2
3
4
5
6
7
8

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;
}
1
2
3
4
5
6
7
8
9
10
11

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);

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38

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 low
    iic_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 high
    iic_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; //时钟输出0
    return 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(); //发送ACK
    else
        IIC_NAck(); //发送nACK
    return 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的读数据
        else
            temp = IIC_ReadByte(0); //最后一个字节NACK

        data_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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375

四、总结

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