393 lines
14 KiB
C
393 lines
14 KiB
C
#include "tmc5160.h"
|
||
#include "app.h"
|
||
|
||
|
||
// CS2置于 “低电平” 使能SPI
|
||
// 写的时候高位地址要加上 0x80
|
||
// 手册第5章是寄存器表，5.3.1是斜坡相关寄存器；第11章有实际单位换算表；第20章 图20.3 有运动控制流程图
|
||
// 寄存器 "RAMPMODE"->"0x20" 斜坡运动模式： 0->位置模式；1->速度模式至正VMAMX；2->速度模式至负VMAX；3->速度保持不变，最多使用2位
|
||
// 寄存器 "VSTART"->"0x23" 电机起动速度，最多使用18位，VSTART<=VSTOP
|
||
// 寄存器 "A1"->"0x24" VSTART->V1的加速度，最多使用16位
|
||
// 寄存器 "V1"->"0x25" 第一加/减速阶段速度阈值，最多使用20位；取0时禁用A1和D1，仅用AMAX和DMAX
|
||
// 寄存器 "AMAX"->"0x26" V1->VMAX的加速度，最多使用16位
|
||
// 寄存器 "DMAX"->"0x28" VMAX->V1的减速度，最多使用16位
|
||
// 寄存器 "VMAX"->"0x27" 斜坡运动目标速度，最多使用23位
|
||
// 寄存器 "D1"->"0x2A" V1->VSTOP的减速度，最多使用16位，不要在位置模式下置0
|
||
// 寄存器 "VSTOP"->"0x2B" 电机停止速度，最多使用18位，VSTART<=VSTOP，位置模式下>=10
|
||
// 一圈 200*256 微步，即每步转动 9/1280 度， 1 微步/秒 = 1.953e-5 转/秒
|
||
|
||
#define TMC5160A_CS2_L HAL_GPIO_WritePin(GPIOC,CS2_Pin,GPIO_PIN_RESET) //CS2低电平
|
||
#define TMC5160A_CS2_H HAL_GPIO_WritePin(GPIOC,CS2_Pin,GPIO_PIN_SET) //CS2高电平
|
||
|
||
#define TMC5160A_MOSI_L HAL_GPIO_WritePin(GPIOB,GPIO_PIN_15,GPIO_PIN_RESET) //MOSI低电平
|
||
#define TMC5160A_MOSI_H HAL_GPIO_WritePin(GPIOB,GPIO_PIN_15,GPIO_PIN_SET) //MOSI高电平
|
||
|
||
#define TMC5160A_CLK_L HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,GPIO_PIN_RESET) //CLK低电平
|
||
#define TMC5160A_CLK_H HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,GPIO_PIN_SET) //CLK高电平
|
||
|
||
#define TMC5160A_ReadVal HAL_GPIO_ReadPin(GPIOB,GPIO_PIN_14) //MISO
|
||
|
||
void TMC5160A_Init_Gpio(void)
|
||
{
|
||
GPIO_InitTypeDef GPIO_InitStruct = {0};
|
||
__HAL_RCC_GPIOB_CLK_ENABLE();
|
||
/**SPI2 GPIO Configuration
|
||
PB13 ------> SPI2_SCK
|
||
PB14 ------> SPI2_MISO
|
||
PB15 ------> SPI2_MOSI
|
||
*/
|
||
GPIO_InitStruct.Pin = GPIO_PIN_13|GPIO_PIN_15;
|
||
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
|
||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
|
||
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
|
||
|
||
GPIO_InitStruct.Pin = GPIO_PIN_14;
|
||
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
|
||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
|
||
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
|
||
}
|
||
|
||
/*************常用参数配置***************/
|
||
uint32_t rampmode = 0x00000000; //斜坡运动模式 0-位置；1-速度
|
||
int xtarget = 0x00000000; //位置模式的目标位置，51200/圈
|
||
uint32_t speed_1 = 0x00004E20; //V1 过渡阶段的目标速度
|
||
uint32_t speed_max = 0x00009C40; //VMAX 目标速度
|
||
uint32_t acc_1 = 0x000005DC; //A1 过渡阶段的加速度
|
||
uint32_t acc_max = 0x000003E8; //AMAX 最大加速度
|
||
uint32_t dcr_1 = 0x00000BB8; //D1 过渡阶段的减速度
|
||
uint32_t dcr_max = 0x000007D0; //DMAX 最大减速度
|
||
/*************常用参数配置***************/
|
||
|
||
void tmc5160_init(void)
|
||
{
|
||
// 纯SPI模式
|
||
TMC5160_SPIWriteInt(0x00, 0x00000004,1); // writing value 0x0000000C = 12 = 0.0 to address 0 = 0x00(GCONF) 0x00000008 不能移动 会左右抖动
|
||
// TMC5160_SPIWriteInt(0x03, 0x00000000,1); // writing value 0x00000000 = 0 = 0.0 to address 1 = 0x03(SLAVECONF)
|
||
// TMC5160_SPIWriteInt(0x05, 0x00000000,1); // writing value 0x00000000 = 0 = 0.0 to address 2 = 0x05(X_COMPARE)
|
||
// TMC5160_SPIWriteInt(0x06, 0x00000000,1); // writing value 0x00000000 = 0 = 0.0 to address 3 = 0x06(OTP_PROG)
|
||
// TMC5160_SPIWriteInt(0x08, 0x0000000E,1); // writing value 0x00000011 = 17 = 0.0 to address 4 = 0x08(FACTORY_CONF)
|
||
// TMC5160_SPIWriteInt(0x09, 0x00010606,1); // writing value 0x00010606 = 67078 = 0.0 to address 5 = 0x09(SHORT_CONF)
|
||
// TMC5160_SPIWriteInt(0x0A, 0x00080400,1); // writing value 0x00080400 = 525312 = 0.0 to address 6 = 0x0A(DRV_CONF)
|
||
// TMC5160_SPIWriteInt(0x0B, 0x00000000,1); // writing value 0x00000000 = 0 = 0.0 to address 7 = 0x0B(GLOBAL_SCALER)
|
||
// 速度相关的驱动控制寄存器
|
||
TMC5160_SPIWriteInt(0x10, 0x00070E02,1); // IHOLD->0~7bit; IRUN->12~8bit
|
||
TMC5160_SPIWriteInt(0x11, 0x0000000A,1); // writing value 0x0000000A = 10 = 0.0 to address 9 = 0x11(TPOWERDOWN)
|
||
// TMC5160_SPIWriteInt(0x13, 0x000001F4,1); // writing value 0x00000041 = 65 = 0.0 to address 10 = 0x13(TPWMTHRS)
|
||
TMC5160_SPIWriteInt(0x14, 0x00000010,1); // writing value 0x00004189 = 16777 = 0.0 to address 11 = 0x14(TCOOLTHRS)
|
||
TMC5160_SPIWriteInt(0x15, 0x00000010,1); // writing value 0x00000000 = 0 = 0.0 to address 12 = 0x15(THIGH)
|
||
// 斜波发生器运动寄存器
|
||
TMC5160_SPIWriteInt(RAMPMODE_ADDR, rampmode,1); // writing value 0x00000000 = 0 = 0.0 to address 13 = 0x20(RAMPMODE)
|
||
TMC5160_SPIWriteInt(XACTUAL_ADDR, 0x00000000,1); // writing value 0xFFCC12F0 = 0 = 0.0 to address 14 = 0x21(XACTUAL)
|
||
TMC5160_SPIWriteInt(0x23, 0x00000000,1); // writing value 0x00000000 = 0 = 0.0 to address 15 = 0x23(VSTART)
|
||
TMC5160_SPIWriteInt(A1_ADDR, acc_1,1); // A1
|
||
TMC5160_SPIWriteInt(AMAX_ADDR, acc_max,1); // AMAX
|
||
TMC5160_SPIWriteInt(V1_ADDR, speed_1,1); // V1
|
||
TMC5160_SPIWriteInt(VMAX_ADDR, speed_max,1); // VMAX
|
||
TMC5160_SPIWriteInt(D1_ADDR, dcr_1,1); // D1
|
||
TMC5160_SPIWriteInt(DMAX_ADDR, dcr_max,1); // DMAX
|
||
TMC5160_SPIWriteInt(0x2B, 0x00000010,1); // VSTOP >= 0x0000000A
|
||
TMC5160_SPIWriteInt(0x2C, 0x00003FFF,1); // TZEROWAIT
|
||
TMC5160_SPIWriteInt(XTARGET_ADDR, xtarget,1); // writing value 0xFFCC12F0 = 0 = 0.0 to address 24 = 0x2D(XTARGET)
|
||
|
||
// TMC5160_SPIWriteInt(0x33, 0x00000000,1); // writing value 0x00000000 = 0 = 0.0 to address 25 = 0x33(VDCMIN)
|
||
// TMC5160_SPIWriteInt(0x34, 0x00000000,1); // writing value 0x00000400 = 1024 = 0.0 to address 26 = 0x34(SW_MODE)
|
||
// // 编码器寄存器
|
||
// TMC5160_SPIWriteInt(0x38, 0x00000000,1); // writing value 0x00000000 = 0 = 0.0 to address 27 = 0x38(ENCMODE)
|
||
// TMC5160_SPIWriteInt(0x39, 0x00000000,1); // writing value 0x00000000 = 0 = 0.0 to address 28 = 0x39(X_ENC)
|
||
// TMC5160_SPIWriteInt(0x3A, 0x00010000,1); // writing value 0x00010000 = 65536 = 0.0 to address 29 = 0x3A(ENC_CONST)
|
||
// TMC5160_SPIWriteInt(0x3D, 0x00000000,1); // writing value 0x00000000 = 0 = 0.0 to address 30 = 0x3D(ENC_DEVIATION)
|
||
// // 电机驱动寄存器-电机微步控制寄存器
|
||
// TMC5160_SPIWriteInt(0x60, 0xAAAAB554,1); // writing value 0xAAAAB554 = 0 = 0.0 to address 31 = 0x60(MSLUT[0])
|
||
// TMC5160_SPIWriteInt(0x61, 0x4A9554AA,1); // writing value 0x4A9554AA = 1251300522 = 0.0 to address 32 = 0x61(MSLUT[1])
|
||
// TMC5160_SPIWriteInt(0x62, 0x24492929,1); // writing value 0x24492929 = 608774441 = 0.0 to address 33 = 0x62(MSLUT[2])
|
||
// TMC5160_SPIWriteInt(0x63, 0x10104222,1); // writing value 0x10104222 = 269500962 = 0.0 to address 34 = 0x63(MSLUT[3])
|
||
// TMC5160_SPIWriteInt(0x64, 0xFBFFFFFF,1); // writing value 0xFBFFFFFF = 0 = 0.0 to address 35 = 0x64(MSLUT[4])
|
||
// TMC5160_SPIWriteInt(0x65, 0xB5BB777D,1); // writing value 0xB5BB777D = 0 = 0.0 to address 36 = 0x65(MSLUT[5])
|
||
// TMC5160_SPIWriteInt(0x66, 0x49295556,1); // writing value 0x49295556 = 1227445590 = 0.0 to address 37 = 0x66(MSLUT[6])
|
||
// TMC5160_SPIWriteInt(0x67, 0x00404222,1); // writing value 0x00404222 = 4211234 = 0.0 to address 38 = 0x67(MSLUT[7])
|
||
// TMC5160_SPIWriteInt(0x68, 0xFFFF8056,1); // writing value 0xFFFF8056 = 0 = 0.0 to address 39 = 0x68(MSLUTSEL)
|
||
// TMC5160_SPIWriteInt(0x69, 0x00F70000,1); // writing value 0x00F70000 = 16187392 = 0.0 to address 40 = 0x69(MSLUTSTART)
|
||
// // 电机驱动寄存器-驱动寄存器组
|
||
TMC5160_SPIWriteInt(0x6C, 0x000100C3,1); // writing value 0x00410153 = 4260179 = 0.0 to address 41 = 0x6C(CHOPCONF)
|
||
TMC5160_SPIWriteInt(0x6D, 0x00000000,1); // writing value 0x00030000 = 196608 = 0.0 to address 42 = 0x6D(COOLCONF)
|
||
// TMC5160_SPIWriteInt(0x6E, 0x00000000,1); // writing value 0x00000000 = 0 = 0.0 to address 43 = 0x6E(DCCTRL)
|
||
TMC5160_SPIWriteInt(0x70, 0xC40C001E,1); // writing value 0xC40C001E = 0 = 0.0 to address 44 = 0x70(PWMCONF)
|
||
}
|
||
|
||
uint8_t data_r[5] = {0};//存放接收到的数据
|
||
|
||
void TMC5160_SPIWriteInt(uint8_t addr, uint32_t data, uint8_t rw)//rw = 1为写，0为读
|
||
{
|
||
|
||
char i = 0;
|
||
unsigned long dat = 0;
|
||
for(i = 0; i < 5; i++) data_r[i] = 0;
|
||
|
||
TMC5160A_CS2_L;
|
||
delay(100);
|
||
if(rw)
|
||
{
|
||
addr |= 0x80;
|
||
}
|
||
else
|
||
{
|
||
addr &= 0x7F;
|
||
}
|
||
|
||
//地址
|
||
TMC5160A_CLK_H;
|
||
for(i = 0; i < 8; i++)
|
||
{
|
||
TMC5160A_CLK_L;
|
||
if(addr & 0x80)
|
||
{
|
||
TMC5160A_MOSI_H;
|
||
}
|
||
else
|
||
{
|
||
TMC5160A_MOSI_L;
|
||
}
|
||
|
||
addr <<= 1;
|
||
TMC5160A_CLK_H;
|
||
delay(20);
|
||
|
||
data_r[0] <<= 1;
|
||
if(TMC5160A_ReadVal)
|
||
{
|
||
data_r[0] |= 1;
|
||
}
|
||
|
||
|
||
|
||
}
|
||
//数据
|
||
for(i = 0; i < 32; i++)
|
||
{
|
||
TMC5160A_CLK_L;
|
||
if(data & 0x80000000)
|
||
{
|
||
TMC5160A_MOSI_H;
|
||
}
|
||
else
|
||
{
|
||
TMC5160A_MOSI_L;
|
||
}
|
||
data <<= 1;
|
||
TMC5160A_CLK_H;
|
||
delay(20);
|
||
|
||
dat <<= 1;
|
||
if(TMC5160A_ReadVal)
|
||
{
|
||
dat |= 1;
|
||
}
|
||
|
||
}
|
||
|
||
data_r[0] = addr;
|
||
data_r[1] = dat >> 24;
|
||
data_r[2] = dat >> 16;
|
||
data_r[3] = dat >> 8;
|
||
data_r[4] = dat >> 0;
|
||
|
||
|
||
TMC5160A_CS2_H; //SPI_CS片选拉1
|
||
|
||
}
|
||
|
||
void TMC5160_SPIReadInt(uint8_t addr, uint8_t record[5])//从addr寄存器读取数据，然后记录到record中
|
||
{
|
||
char i;
|
||
for(i = 0; i < 5; i++) data_r[i] = 0;
|
||
|
||
TMC5160_SPIWriteInt(addr,0,0);
|
||
TMC5160_SPIWriteInt(addr,0,0);
|
||
memcpy(record, data_r, 5);
|
||
}
|
||
|
||
|
||
uint8_t XA[5]={0},VA[5]={0},RAMP_STAT[5],DRV_STAT[5];
|
||
signed int XA_32 = 0,VA_32 = 0,RAMP_STAT_32 = 0,DRV_STAT_32 = 0; //有正负
|
||
uint16_t SG_RESULT_16 = 0;
|
||
char tmc5160_sw =0;
|
||
|
||
//void tmc5160_act()
|
||
//{
|
||
// TMC5160_SPIReadInt(XACTUAL_ADDR,XA);//读取实际位置XACTUAL
|
||
// XA_32 = Raw_32(XA);
|
||
// TMC5160_SPIReadInt(VACTUAL_ADDR,VA);//读取实际速度VACTUAL
|
||
// VA_32 = Raw_32(VA);
|
||
|
||
// if(tmc5160_sw == 1)
|
||
// {
|
||
// HAL_GPIO_WritePin(LED_ERR_GPIO_Port,LED_ERR_Pin,GPIO_PIN_RESET); //蓝灯常亮
|
||
//
|
||
// rampmode = 0x00000001;
|
||
// TMC5160_SPIWriteInt(RAMPMODE_ADDR,rampmode,1); //开启速度模式
|
||
//
|
||
//// rampmode = 0x00000000;
|
||
//// TMC5160_SPIWriteInt(RAMPMODE_ADDR,rampmode,1); //开启位置模式
|
||
//// if( XA_32 == 0x00000000 )
|
||
//// {
|
||
//// xtarget = 0x00025800;
|
||
//// TMC5160_SPIWriteInt(XTARGET_ADDR, xtarget,1); //正向转动3圈
|
||
//// }
|
||
////
|
||
//// if( XA_32== 0x00025800 )
|
||
//// {
|
||
//// xtarget = 0x00000000;
|
||
//// TMC5160_SPIWriteInt(XTARGET_ADDR, xtarget,1); //接着反向转3圈（返回起点）
|
||
//// }
|
||
// }else
|
||
// {
|
||
// TMC5160_SPIWriteInt(XTARGET_ADDR,XA_32,1); //把当前位置设定为目标位置，进入减速阶段
|
||
//
|
||
// if( abs(VA_32) <= 0x0000000A ) // 视作已经停止
|
||
// {
|
||
// HAL_GPIO_TogglePin(LED_ERR_GPIO_Port,LED_ERR_Pin); //蓝灯闪烁
|
||
//
|
||
// }
|
||
|
||
// }
|
||
|
||
//
|
||
//}
|
||
|
||
signed int Raw_32(uint8_t raw[5]) //把5*8bit数据中的0~31位拼接成1*32bit的数值并返回
|
||
{
|
||
uint32_t result = 0;
|
||
|
||
result |= raw[1];
|
||
result <<= 8;
|
||
result |= raw[2];
|
||
result <<= 8;
|
||
result|= raw[3];
|
||
result <<= 8;
|
||
result|= raw[4];
|
||
|
||
return result;
|
||
}
|
||
|
||
uint32_t X_temp = 0;
|
||
char busy_flag = 0;
|
||
void tmc5160_operate(char operate_mode, uint32_t steps)
|
||
{
|
||
// TMC5160_SPIReadInt(XACTUAL_ADDR,XA); //读取实际位置XACTUAL
|
||
// XA_32 = Raw_32(XA);
|
||
// TMC5160_SPIReadInt(VACTUAL_ADDR,VA); //读取实际速度VACTUAL
|
||
// VA_32 = Raw_32(VA);
|
||
// TMC5160_SPIReadInt(RAMP_STAT_ADDR,RAMP_STAT); //读取斜坡状态
|
||
// RAMP_STAT_32 = Raw_32(RAMP_STAT);
|
||
// TMC5160_SPIReadInt(DRV_STAT_ADDR,DRV_STAT); //读取驱动器状态
|
||
// DRV_STAT_32 = Raw_32(DRV_STAT);
|
||
// SG_RESULT_16 = ((uint16_t)DRV_STAT_32) & 0x03FF;//驱动器状态共32bit，其中0-9bit为负载值，读到的负载值越低，负载越高，0代表最高负载
|
||
|
||
|
||
switch(operate_mode)
|
||
{
|
||
case 0 : //电机停止
|
||
{
|
||
rampmode = 0x00000000;
|
||
TMC5160_SPIWriteInt(RAMPMODE_ADDR,rampmode,1); //开启位置模式
|
||
|
||
TMC5160_SPIWriteInt(XTARGET_ADDR, XA_32, 1); //把当前位置设为目标位置，开启减速停止
|
||
|
||
busy_flag = 0;
|
||
}
|
||
break;
|
||
case 1 : //电机开始正向运动
|
||
{
|
||
if( busy_flag == 0)
|
||
{
|
||
busy_flag = 1;
|
||
X_temp = XA_32; //记录此时的实际位置
|
||
|
||
rampmode = 0x00000000;
|
||
TMC5160_SPIWriteInt(RAMPMODE_ADDR,rampmode,1); //开启位置模式
|
||
|
||
TMC5160_SPIWriteInt(XTARGET_ADDR, X_temp + steps, 1); //此时的实际位置+步长作为目标位置
|
||
}
|
||
if( RAMP_STAT_32 & 0x00000200 ) //到达目标位置，XACTUAL = XTARGET时，第9位会被置 1
|
||
{
|
||
busy_flag = 0;
|
||
X_temp = 0;
|
||
|
||
}
|
||
}
|
||
break;
|
||
case 2 : //电机开始反向运动
|
||
{
|
||
if( busy_flag == 0)
|
||
{
|
||
busy_flag = 1;
|
||
X_temp = XA_32;
|
||
|
||
rampmode = 0x00000000;
|
||
TMC5160_SPIWriteInt(RAMPMODE_ADDR,rampmode,1); //开启位置模式
|
||
|
||
TMC5160_SPIWriteInt(XTARGET_ADDR, X_temp - steps, 1);
|
||
}
|
||
if( RAMP_STAT_32 & 0x00000200 )
|
||
{
|
||
busy_flag = 0;
|
||
X_temp = 0;
|
||
|
||
}
|
||
}
|
||
break;
|
||
|
||
case 3 : //电机回到起点
|
||
{
|
||
if( busy_flag == 0)
|
||
{
|
||
busy_flag = 1;
|
||
rampmode = 0x00000000;
|
||
TMC5160_SPIWriteInt(RAMPMODE_ADDR,rampmode,1); //开启位置模式
|
||
|
||
speed_max <<=1;
|
||
TMC5160_SPIWriteInt(VMAX_ADDR, speed_max, 1); //以二倍速返回零点
|
||
TMC5160_SPIWriteInt(XTARGET_ADDR, 0x00000000, 1);
|
||
}
|
||
if( RAMP_STAT_32 & 0x00000200 )
|
||
{
|
||
busy_flag = 0;
|
||
speed_max >>=1;
|
||
TMC5160_SPIWriteInt(VMAX_ADDR, speed_max, 1); //恢复原速
|
||
}
|
||
}
|
||
break;
|
||
}
|
||
|
||
}
|
||
|
||
|
||
void motor_protect_ads(float threshold_neg, float threshold_pos)
|
||
{
|
||
if( (X_ads1220_prc <= threshold_neg) && ( (tmc5160_sw == 2) || (tmc5160_sw == 3) ) )
|
||
{
|
||
tmc5160_sw = 0;
|
||
}
|
||
if( (X_ads1220_prc >= threshold_pos) && ( (tmc5160_sw == 1) || (tmc5160_sw == 3) ) )
|
||
{
|
||
tmc5160_sw = 0;
|
||
}
|
||
|
||
}
|
||
|
||
void motor_protect_ocin(void)
|
||
{
|
||
if( (ocin1 == 0x00) && ( (tmc5160_sw == 2) || (tmc5160_sw == 3) ) )
|
||
{
|
||
tmc5160_sw = 0;
|
||
}
|
||
if( (ocin2 == 0x00) && ( (tmc5160_sw == 1) || (tmc5160_sw == 3) ) )
|
||
{
|
||
tmc5160_sw = 0;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|