#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, 	0x00070A02,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)
{
		
	switch(operate_mode)
	{
		case 0 :	//电机停止
		{
			VA_32 = Raw_32(VA);
			
			if( VA_32 != 0)
			{
				busy_flag = 1;
				
				rampmode = 0x00000000;
				TMC5160_SPIWriteInt(RAMPMODE_ADDR,rampmode,1);		//开启位置模式

				TMC5160_SPIWriteInt(XTARGET_ADDR, XA_32, 1);		//把当前位置设为目标位置，开启减速停止
			}else
			{
				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(busy_flag == 1)		
			{
				delay(100);												//静止状态下会被 误判 成已经到达目标位置，因此进行延时等待寄存器发生变化。
				TMC5160_SPIReadInt(RAMP_STAT_ADDR,RAMP_STAT);			//运动过程中读取斜坡状态
				RAMP_STAT_32 = Raw_32(RAMP_STAT);
				
				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(busy_flag == 1)		
			{
				delay(100);											//静止状态下会被 误判 成已经到达目标位置，因此进行延时等待寄存器发生变化。
				TMC5160_SPIReadInt(RAMP_STAT_ADDR,RAMP_STAT);		//读取斜坡状态
				RAMP_STAT_32 = Raw_32(RAMP_STAT);
				
				if(RAMP_STAT_32 & 0x00000200)						//到达目标位置，XACTUAL = XTARGET时，第9位会被置 1
				{
					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;

		default :
		{
		}
		break;
	}
		
}


void motor_protect_ads(float threshold_neg, float threshold_pos)
{
	if( (X_ads1220_prc <= threshold_neg) && (motor_direc != 1)  )
	{
		tmc5160_operate(0, 0);
		if(busy_flag == 0) motor_direc = 1;
	}
	if( (X_ads1220_prc >= threshold_pos) && ( motor_direc != 2 )   )
	{
		tmc5160_operate(0, 0);
		if(busy_flag == 0) motor_direc = 2;
	}

}

void motor_protect_ocin(void)
{
	if( (ocin1 == 0) && (motor_direc != 1)  )
	{
		tmc5160_operate(0, 0);
		if(busy_flag == 0) motor_direc = 1;
	}
	if( (ocin2 == 0) && ( motor_direc != 2 )   )
	{
		tmc5160_operate(0, 0);
		if(busy_flag == 0) motor_direc = 2;
	}
}