#include "sys.h"
#include "delay.h"
#include "usart.h"
#include "led.h"
#include "beep.h"
#include "key.h"
#include "motor_driver.h"

//ALIENTEK 探索者STM32F407开发板 实验4
//串口通信实验 -库函数版本
//技术支持：www.openedv.com
//淘宝店铺：http://eboard.taobao.com
//广州市星翼电子科技有限公司  
//作者：正点原子 @ALIENTEK

void MX_TIM2_Init(void);
//void PWM_SetPulseWidth(uint16_t pulseWidth);
//void PWM_SetSpeed(int16_t speed);
void Error_Handler(void);

typedef uint32_t  u32;
typedef uint16_t u16;
typedef uint8_t  u8;
// 定义TMC2240寄存器地址
#define REG_MICROSTEPS 0x03
#define REG_CURRENT       0x05
#define EG_STATUS           0x0F


extern  volatile u16 rx_len;        // 接收计数
extern volatile u8 rx_flag;        // 接收完成标志
#define GCONF          0x00
#define IHOLD_IRUN     0x10
#define TPOWERDOWN     0x11
#define TPWMTHRS       0x13
#define TCOOLTHRS      0x14
#define CHOPCONF       0x6C
#define COOLCONF       0x6D
#define DRV_CONF       0x0A
#define DRV_STATUS     0x6F
#define PWMCONF 			 0x70

#define SOFT_SPI_DR_PORT     GPIOD
#define SOFT_SPI_DR_PIN      GPIO_Pin_3
//forward and reverse
#define D3_DR_forward()     GPIO_SetBits(SOFT_SPI_DR_PORT, SOFT_SPI_DR_PIN)
#define D3_DR_reverse()     GPIO_ResetBits(SOFT_SPI_DR_PORT, SOFT_SPI_DR_PIN)

void TMC2240_Init_rotating() {
    // 启用电压补偿和PWM模式
    TMC2240_WriteReg(GCONF, 0x000000C4);

    // 电流设置：IHOLD=0.5A, IRUN=1.2A
    TMC2240_WriteReg(IHOLD_IRUN, 0x00180C10);

    // 16微步（MRES=4），TOFF=3
    TMC2240_WriteReg(CHOPCONF, 0x030000C3);
		// 设置速度阈值500Hz（低于此值切StealthChop2）
    TMC2240_WriteReg(TPWMTHRS, 200);
    // 驱动配置（根据需求调整）
    TMC2240_WriteReg(DRV_CONF, 0x00000001);

    // 停机电流延时
    TMC2240_WriteReg(TPOWERDOWN, 10);
}

void TMC2240_Init() {
    // 启用StealthChop2自动调谐 + SpreadCycle动态切换
    TMC2240_WriteReg(GCONF, 0x000000C4 | (1 << 7)); // bit7=1
    
    // 电流设置：IRUN=1.2A, IHOLD=0.8A（公式计算值）
    //TMC2240_WriteReg(IHOLD_IRUN, 0x00190C10);
	   TMC2240_WriteReg(IHOLD_IRUN, 0x04090101); //可能运行电流太大，发热，这里调小试试
   // 降低微步分辨率（例如从256微步改为16微步）
		TMC2240_WriteReg(CHOPCONF, 0x070080C3 ); // MRES=4（16微步）
    
    // 设置速度阈值500Hz（低于此值切StealthChop2）
    TMC2240_WriteReg(TPWMTHRS, 50);
    
    // 其他配置保持不变
    TMC2240_WriteReg(DRV_CONF, 0x00000001);
		
    TMC2240_WriteReg(TPOWERDOWN, 10);
}
// 检查驱动器状态
void CheckDriverStatus() {
    uint32_t status = TMC2240_ReadReg(DRV_STATUS);//
    if (status & (1 << 26)) { // 检查短路到地
        printf("Short to ground detected\n");
    }
    if (status & (1 << 25)) { // 检查过温
        printf("Overtemperature detected\n");
    }
    if (status & (1 << 24)) { // 检查失速
        printf("Motor stall detected\n");
    }
}

void motor_steps(uint8_t num, uint8_t dir, uint16_t steps)
{
           GPIO_SetBits(GPIOD, GPIO_Pin_4);    //TMS2240 - 2使能信号
           delay_us(steps);		
           GPIO_ResetBits(GPIOD, GPIO_Pin_4);     //TMS2240 - 1使能信号      
}

void motor_stop(uint8_t num)
{
}


uint8_t crc8_calculate(const uint8_t *data, uint32_t length, uint8_t poly, uint8_t init_val, uint8_t ref_out)
{
    // 1. 初始化 CRC 寄存器
    uint8_t crc = init_val;
    
    // 2. 遍历每个字节
    for (uint32_t i = 0; i < length; i++)
    {
        // 3. 将当前字节与 CRC 寄存器异或（核心步骤）
        crc ^= data[i];
        
        // 4. 遍历当前字节的每一位（8 位）
        for (uint8_t j = 0; j < 8; j++)
        {
            // 5. 判断最高位是否为 1，决定是否异或多项式
            if (crc & 0x80)
            {
                // 左移 1 位，再异或多项式（等价于多项式除法）
                crc = (crc << 1) ^ poly;
            }
            else
            {
                // 最高位为 0，仅左移 1 位
                crc <<= 1;
            }
        }
    }
    
    // 6. 输出反转（若需要）
    if (ref_out)
    {
        // 按位反转 CRC 结果（如 0x12 → 0x48）
        uint8_t reversed = 0;
        for (uint8_t i = 0; i < 8; i++)
        {
            reversed |= ((crc >> i) & 0x01) << (7 - i);
        }
        crc = reversed;
    }
    
    return crc;
}

uint8_t crc8_standard(const uint8_t *data, uint32_t length)
{
    // 适配 CRC-8/MAXIM/ITU 标准：poly=0x07, init=0x00, ref_out=0
    return crc8_calculate(data, length, 0x07, 0x00, 0);
}

uint32_t status ;
u8 flg=0;

extern int flag_microswitch_left;   // 光电开关left 限位
extern int flag_microswitch_right;  // 光电开关right 限位


extern  uint8_t g_usart1_rx_buf[USART_REC_LEN];
extern  uint16_t g_usart1_rx_sta;
extern uint16_t g_usart1_rx_state;

extern  uint8_t g_usart2_rx_buf[USART_REC_LEN];
extern  uint16_t g_usart2_rx_sta;
extern uint16_t g_usart2_rx_state;


extern  uint8_t g_usart3_rx_buf[USART_REC_LEN];
extern  uint16_t g_usart3_rx_sta;
extern uint16_t g_usart3_rx_state;


extern uint16_t g_usart6_rx_state;

extern uint8_t g_rx1_buffer[RXBUFFERSIZE];                  /* HAL库使用的串口接收缓冲 */
/* 接收缓冲, 最大USART_REC_LEN个字节. */
extern uint8_t g_usart2_rx_buf[USART_REC_LEN];
extern uint16_t g_usart2_rx_sta;
extern uint8_t g_rx2_buffer[RXBUFFERSIZE];                  /* HAL库使用的串口接收缓冲 */
/* 接收缓冲, 最大USART_REC_LEN个字节. */
extern uint8_t g_usart3_rx_buf[USART_REC_LEN];
extern uint16_t g_usart3_rx_sta;
extern uint8_t g_rx3_buffer[RXBUFFERSIZE];                  /* HAL库使用的串口接收缓冲 */
/* 接收缓冲, 最大USART_REC_LEN个字节. */
extern uint8_t g_usart4_rx_buf[USART_REC_LEN];
extern uint16_t g_usart4_rx_sta;
extern uint8_t g_rx4_buffer[RXBUFFERSIZE];                  /* HAL库使用的串口接收缓冲 */
/* 接收缓冲, 最大USART_REC_LEN个字节. */
extern uint8_t g_usart5_rx_buf[USART_REC_LEN];
extern uint16_t g_usart5_rx_sta;
extern uint8_t g_rx5_buffer[RXBUFFERSIZE];                  /* HAL库使用的串口接收缓冲 */
/* 接收缓冲, 最大USART_REC_LEN个字节. */
extern uint8_t g_usart6_rx_buf[USART_REC_LEN];
extern uint16_t g_usart6_rx_sta;
extern uint8_t g_rx6_buffer[RXBUFFERSIZE];                  /* HAL库使用的串口接收缓冲 */

#define D_value 			 100   //脉络差值
int main(void)
{ 
 
	u8 t;
	u8 len;	
	u16 times=0;
	u8 start_vein_flg =0;  //寻脉启动标志
	int16_t adc1_zoo =0;
	int16_t adc2_zoo =0;
	int16_t adc3_zoo =0;   //三路脉的零点
	
	int16_t adc1_current =0;
	int16_t adc2_current =0;
	int16_t adc3_current =0;   //三路脉的当前值
	
	u8 ch =0;    //诊脉动作通道选择
	
  uint8_t frame_updata[6] = {0x30,0x30,0x30,0x30,0x30,0x30};   
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置系统中断优先级分组2
	delay_init(168);		//延时初始化 
	uart1_init(115200);	//串口1初始化波特率为115200 手指左
	uart2_init(115200);	//串口1初始化波特率为115200 手指中
	uart3_init(115200);	//串口1初始化波特率为115200 手指右
	uart4_init(115200);	//串口1初始化波特率为115200 用于信息打印
	uart5_init(115200);
	uart6_init(115200);	//串口6初始化波特率为115200 上传到slide
	gpio_Init();		  		//初始化gpio  
	//TIM1_PWM_Init(16, 32);
	//TIM2_PWM_Init(16, 32);
  //TIM4_PWM_Init(16, 32);
  //TIM_Cmd(TIM1, DISABLE);
	//TIM_Cmd(TIM2, DISABLE);
	//TIM_Cmd(TIM4, DISABLE);
	//---------------------------------------------
	// 初始化DRV8832电机驱动
    DRV8832_Init();
    
    // 初始化PWM定时器（如果需要调速）
   // PWM_Timer_Init();

  motor_spi1_init();
	motor_spi2_init();

	
    // 使能全局中断
    __enable_irq();
	//=======================================================================
	L_Motor_Control(MOTOR_REVERSE);
	R_Motor_Control(MOTOR_REVERSE);  
	delay_ms(500);
	M_Motor_Control(MOTOR_REVERSE);
	delay_ms(500);
	L_Motor_Control(MOTOR_STOP);
	M_Motor_Control(MOTOR_STOP);
	R_Motor_Control(MOTOR_STOP); //上电初始化后，先将电机提升到上位，不一定是到顶，这个可以后期待定
	//=======================================================================
	     //测试电机3路
     while(0) {
//        // 示例1：基本控制（全速正反转）
//        Motor_Control(MOTOR_FORWARD);  // 正转
//       // delay_ms(5000);
//        Motor_SafeDirectionChange(MOTOR_REVERSE);  // 安全切换到反转
//      //  delay_ms(5000);
//        
//        Motor_Control(MOTOR_BRAKE);    // 刹车
//        delay_ms(500);
//        
//        Motor_Control(MOTOR_STOP);     // 停止（滑行）
//        delay_ms(1000);
        
        // 示例2：PWM调速演示
			 
//			      L_Motor_SetSpeed(50, MOTOR_FORWARD);
//				    M_Motor_SetSpeed(50, MOTOR_FORWARD);
//				    R_Motor_SetSpeed(50, MOTOR_FORWARD);
//            delay_ms(5000);
//			      L_Motor_Control(MOTOR_STOP);
//			      M_Motor_Control(MOTOR_STOP);
//            R_Motor_Control(MOTOR_STOP);
//						
//			      L_Motor_SetSpeed(50, MOTOR_REVERSE);
//				    M_Motor_SetSpeed(50, MOTOR_REVERSE);
//				    R_Motor_SetSpeed(50, MOTOR_REVERSE);
//            delay_ms(5000);
//			      L_Motor_Control(MOTOR_STOP);
//			      M_Motor_Control(MOTOR_STOP);
//            R_Motor_Control(MOTOR_STOP);
//        // 正转加速
        for(uint8_t speed = 0; speed <= 100; speed += 10) 
			 {
            L_Motor_SetSpeed(speed, MOTOR_FORWARD);
				    M_Motor_SetSpeed(speed, MOTOR_FORWARD);
				    R_Motor_SetSpeed(speed, MOTOR_FORWARD);
            delay_ms(200);
        }
        delay_ms(2000);
        
//        // 正转减速
//        for(uint8_t speed = 100; speed > 0; speed -= 10) 
//				{
//            L_Motor_SetSpeed(speed, MOTOR_FORWARD);
//					  M_Motor_SetSpeed(speed, MOTOR_FORWARD);
//					  R_Motor_SetSpeed(speed, MOTOR_FORWARD);
//            delay_ms(200);
//        }
				
				
        L_Motor_SetSpeed(0, MOTOR_STOP);
				M_Motor_SetSpeed(0, MOTOR_STOP);
				R_Motor_SetSpeed(0, MOTOR_STOP);
				// 反转加速
				  for(uint8_t speed = 0; speed <= 100; speed += 10) 
			 {
            L_Motor_SetSpeed(speed, MOTOR_REVERSE);
				    M_Motor_SetSpeed(speed, MOTOR_REVERSE);
				    R_Motor_SetSpeed(speed, MOTOR_REVERSE);
            delay_ms(200);
        }
        delay_ms(2000);
        
//        // 反转减速
//        for(uint8_t speed = 100; speed > 0; speed -= 10) 
//				{
//            L_Motor_SetSpeed(speed, MOTOR_REVERSE);
//					  M_Motor_SetSpeed(speed, MOTOR_REVERSE); 
//					  R_Motor_SetSpeed(speed, MOTOR_REVERSE);
//            delay_ms(200);
//        }
			
				
        delay_ms(1000);
        
        // 检查故障状态
        if(L_Motor_GetFaultStatus()) {
            // 发生故障，停止电机
            L_Motor_Control(MOTOR_STOP);
            
            // 可以添加故障处理代码，如闪烁LED或串口输出
            // ...
            
            // 等待故障清除
            while(L_Motor_GetFaultStatus()) {
                delay_ms(100);
            }
        }
    }	
	
	
	//=======================================================================
	while(1)
	{
//    printf("I love you lao zhang\n");

//	  delay_ms(300);

//		 printf("I love you zhengshuo\n");

//     delay_ms(300);   

	//	 Uart5_Send_data(frame_updata,6);		
    //根据指令控制电机的动作
     	if(g_usart6_rx_state==1)  //串口6的接收数据
      {
				
				g_usart6_rx_state = 0;
				if(g_usart6_rx_buf[1]==0x01)  //1通道 BC 01 00 55 XX 66   启动
          {
						if(g_usart6_rx_buf[3]==0x55)
             {
							   L_Motor_Control(MOTOR_FORWARD);    //启动电机正传
						 } 
						if(g_usart6_rx_buf[3]==0xaa)   //BC 01 00 aa XX 66   反转
             {
							 
							  L_Motor_Control(MOTOR_REVERSE);
						 } 
						if(g_usart6_rx_buf[3]==0xff)   //BC 01 00 ff XX 66   关闭
             {
							 
							  L_Motor_Control(MOTOR_STOP);
						 }  
						
					}
       if(g_usart6_rx_buf[1]==0x02)  //2通道
          {
						if(g_usart6_rx_buf[3]==0x55)
             {
							  M_Motor_Control(MOTOR_FORWARD);    //启动电机正传
						 } 
						if(g_usart6_rx_buf[3]==0xaa)   //BC 02 00 aa XX 66   反转
             {
							 
							  M_Motor_Control(MOTOR_REVERSE);
						 } 
						if(g_usart6_rx_buf[3]==0xff)   //BC 02 00 ff XX 66   关闭
             {
							 
							  M_Motor_Control(MOTOR_STOP);
						 }  
						
					} 					
			 if(g_usart6_rx_buf[1]==0x03)  //3通道
          {
						if(g_usart6_rx_buf[3]==0x55)
             {
							   R_Motor_Control(MOTOR_FORWARD);    //启动电机正传
						 } 
						if(g_usart6_rx_buf[3]==0xaa)   //BC 03 00 aa XX 66   反转
             {
							 
							  R_Motor_Control(MOTOR_REVERSE);
						 } 
						if(g_usart6_rx_buf[3]==0xff)   //BC 03 00 ff XX 66   关闭
             {
							 
							  R_Motor_Control(MOTOR_STOP);
						 }  
					}
       //--------------------------------------------------------------- 					
       if(g_usart6_rx_buf[1]==0x04)  //如果地址是04 则是启动诊脉三个电机运行，先运行中间电机，其他两个电机延时后启动，中间电机寻到脉后，停止，
				                             //其他两个电机也是自动寻到脉后停止
          {
						if(g_usart6_rx_buf[3]==0xaa)
             {
							  start_vein_flg =1;    //寻脉启动标志
							  ch = 0;               //从中间开始寻
						 } 
						if(g_usart6_rx_buf[3]==0x55)
             {
							  start_vein_flg =2;    //停止寻脉，开始诊脉
						 }
            if(g_usart6_rx_buf[3]==0xff)
             {
							  start_vein_flg =3;    //停止诊脉，电机提升后停止
						 }
						                         //可以增加指令，指导电机在诊脉过程中进行浮中沉脉的动作
					}   					
			}	
		//=============================================================================================
    //以下是自动控制部分，是否要转移到定制中断中处理？
      if(start_vein_flg == 1)
      {
				switch(ch)
			 {
					case 0:
						
					if((adc2_current-adc2_zoo)<= D_value)
				 {
					M_Motor_Control(MOTOR_FORWARD);    //电机2正传
				 }
				 else
				 {
					M_Motor_Control(MOTOR_STOP);      //电机2停止
					 ch = 1;
				 }
			   break;
					
				 //-------------------------------------------------------
				 
					case 1:
						if((adc1_current-adc1_zoo)<= D_value)
				 {
					L_Motor_Control(MOTOR_FORWARD);    //电机1正传
				 }
				 else
				 {
					 L_Motor_Control(MOTOR_STOP);      //电机1停止
					 ch = 2;
				 }
			   break;
				 //--------------------------------------------------------
				 
					case 2:
						
					if((adc3_current-adc3_zoo)<= D_value)
				 {
					R_Motor_Control(MOTOR_FORWARD);    //电机3正传
				 }
				 else
				 {
					 R_Motor_Control(MOTOR_STOP);      //电机3停止
					 ch = 3;
					 
				 }
				 break; 
			 }
			}
     if(start_vein_flg == 3)	//停止	 停诊时，应左右两个电机回的高，中间的回的少，使中间突出一些	
		 {
			 L_Motor_Control(MOTOR_REVERSE);
	     R_Motor_Control(MOTOR_REVERSE);
			 delay_ms(500);
       M_Motor_Control(MOTOR_REVERSE); 			 
	     delay_ms(1000); 
	     L_Motor_Control(MOTOR_STOP);
	     M_Motor_Control(MOTOR_STOP);
	     R_Motor_Control(MOTOR_STOP); //停止诊脉后电机提升后，slide 也做提升动作  
			 start_vein_flg =0; 
		 }
			//-------------------------------------------------------------------------------
		  //工作流程 
		  //1、启动诊脉过程，左右，前后移动 通过slide板 控制X轴和Y轴的动作，（通过摄像头找位置)
		  //2、找到大概位置后，slide板控制Z轴向下，找到中间位置的脉后，Z轴停止
		  //3、开始左找脉，sensor板控制电机向下，slide 控制姿态电机，找到正确脉
		  //4、左找到脉后，右脉就已经定位了
		  //5、开始诊脉
		 
		//==============================================================
		//以下是ADC采集	
		//接收到的ADC数据 首位BB 结尾是66 中间的数据是 01 ADCL ADCH CRC8 
			                             //BB 01 A2 08 73 66 
		                               //BB -首位
                                   //01 -
                                   //A2 -ADCL
                                   //08 -ADCH
                                   //73 -CRC8
                                   //66 -结尾	   //
		                               //以上数据，如果传给上位机，无法分清楚是哪个通道的数据,因此将第二个数据设置为通道
		
		 if(g_usart1_rx_state == 1)    	 
        {                           
                        //  g_usart1_rx_buf[1] = 0x01;
                        //  g_usart1_rx_buf[3] = 0x34;
                        //  g_usart1_rx_buf[4] = crc8_standard(g_usart1_rx_buf,4);
     
					if(start_vein_flg==0) //未启动前，采集到的数据为当前通道的零点
          {
						adc1_zoo =((uint16_t)g_usart1_rx_buf[3]<<8)|g_usart1_rx_buf[2];
						
					}
					else //当前采集到的值
					{
						adc1_current = ((uint16_t)g_usart1_rx_buf[3]<<8)|g_usart1_rx_buf[2];
					}	
			    g_usart1_rx_state = 0;
					//--------------------------------------------------------------                             
					g_usart1_rx_buf[1]=0x01; //实际这个是原始数据没有变化
					g_usart1_rx_buf[4] = crc8_standard(g_usart1_rx_buf,4); //重新校验
					//--------------------------------------------------------------
          memcpy(frame_updata,g_usart1_rx_buf,6 );                
			    memset(g_usart1_rx_buf, 0x0, 6);	
			    g_rx1_buffer[0] = 0x0;	 
          Uart6_Send_data(frame_updata,6);		
        }
				if(g_usart2_rx_state == 1)
        {
                        
          if(start_vein_flg==0) //未启动前，采集到的数据为当前通道的零点
          {
						adc2_zoo=((uint16_t)g_usart2_rx_buf[3]<<8)|g_usart2_rx_buf[2];
						
					}
					else //当前采集到的值
					{
						adc2_current = ((uint16_t)g_usart2_rx_buf[3]<<8)|g_usart2_rx_buf[2];
					}	
          g_usart2_rx_state = 0;
					//-------------------------------------------------------------------
					g_usart2_rx_buf[1]=0x02; //通道设置为02
					g_usart2_rx_buf[4] = crc8_standard(g_usart2_rx_buf,4); //重新校验
					//-------------------------------------------------------------------
          memcpy(frame_updata,g_usart2_rx_buf,6 );                
			    memset(g_usart2_rx_buf, 0x0, 6);	
			    g_rx2_buffer[0] = 0x0;	 
          Uart6_Send_data(frame_updata,6);		
        }
				if(g_usart3_rx_state == 1)
        {
					
					if(start_vein_flg==0) //未启动前，采集到的数据为当前通道的零点
          {
						adc3_zoo=((uint16_t)g_usart3_rx_buf[3]<<8)|g_usart3_rx_buf[2];
						
					}
					else  //当前采集到的值
					{
						adc3_current = ((uint16_t)g_usart3_rx_buf[3]<<8)|g_usart3_rx_buf[2];
					}	
					
          g_usart3_rx_state = 0;
					//-------------------------------------------------------------------
					g_usart3_rx_buf[1]=0x03; //通道设置为03
					g_usart3_rx_buf[4] = crc8_standard(g_usart3_rx_buf,4); //重新校验
					//-------------------------------------------------------------------
          memcpy(frame_updata,g_usart3_rx_buf,6 );                
			    memset(g_usart3_rx_buf, 0x0, 6);	
			    g_rx3_buffer[0] = 0x0;	 
          Uart6_Send_data(frame_updata,6);		
        }
				
		//-------------------------------------------------
	}
}

