/*
 * msp_JTBATTERY.c
 *
 *  Created on: Oct 26, 2023
 *      Author: shiya
 */
#include "MSP/msp_Elite_Robot.h"
#include "MSP/msp_Modbus_crc16.h"

//清除报警
//清除数据
//伺服上电
//程序启动
//回程序首行
//急停
//暂停
//关闭伺服
//清除碰撞报警
//程序结束反馈信号
//预约程序1
//预约程序2
//清空预约


//
UART_HandleTypeDef* Huart_Elite;
Robot_Joint_Angle*  Robot_Angele_1;
uint8_t Robot_stas[16];
uint8_t Robot_stas_All[19];
uint8_t Robot_stas_CRC[2];


uint8_t Elite_crc_H;
uint8_t Elite_crc_L;


uint8_t GF_MSP_Elite_Init(UART_HandleTypeDef* huart, Robot_Joint_Angle* _Robot_Angle )
{

	Huart_Elite=huart;
	Robot_Angele_1=_Robot_Angle;
	GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_RS485_1_RxCpltCallback, GF_Elite_Callback);//这句话的理解有问题
	HAL_UART_Receive_IT(Huart_Elite,Robot_stas,1);

	return 1;
}

void GF_MSP_Elite_Inquiry(int16_t Quit_Falg) {

	unsigned short CRC_Value;
	uint8_t Elite_crc_H;
	uint8_t Elite_crc_L;

    uint8_t Order[8];
    Order[0] = 0X03;
    Order[1] = 0X06;
    Order[2] = 0x00;
    Order[3] = 0x42;
    switch (Quit_Falg) {
          case CLEAR_ALARM_1:
               Order[4] = 0X00;
               Order[5] = 0x01;
              break;
          case CLEAR_DATA_2:
              Order[4] = 0X00;
              Order[5] = 0x00;
              break;
          case SERVO_POWER_ON_3:
              Order[4] = 0X00;
              Order[5] = 0x02;
              break;
          case PROGRAM_START_4:
              // 这里可以根据实际需求修改寄存器地址和数量
              Order[4] = 0X00;
              Order[5] = 0x04;
              break;
          case RETURN_TO_START_5:
              // 这里可以根据实际需求修改寄存器地址和数量
              Order[4] = 0X00;
              Order[5] = 0x08;
              break;
          case EMERGENCY_STOP_6:
              // 这里可以根据实际需求修改寄存器地址和数量
              Order[4] = 0X00;
              Order[5] = 0x10;
              break;
          case PAUSE_PROGRAM_7:
              // 这里可以根据实际需求修改寄存器地址和数量
              Order[4] = 0X00;
              Order[5] = 0x20;
              break;
          case SERVO_POWER_OFF_8:
              // 这里可以根据实际需求修改寄存器地址和数量
              Order[4] = 0X00;
              Order[5] = 0x40;
              break;
          case CLEAR_COLLISION_ALARM_9:
              // 这里可以根据实际需求修改寄存器地址和数量
              Order[4] = 0X00;
              Order[5] = 0x80;
              break;
          case PROGRAM_END_FEEDBACK_10:
              // 这里可以根据实际需求修改寄存器地址和数量
              Order[4] = 0X01;
              Order[5] = 0x00;
              break;
          case RESERVED_PROGRAM_1_11:
              // 这里可以根据实际需求修改寄存器地址和数量
              Order[4] = 0X02;
              Order[5] = 0x00;
              break;
          case RESERVED_PROGRAM_2_12:
              // 这里可以根据实际需求修改寄存器地址和数量
              Order[4] = 0X04;
              Order[5] = 0x00;
              break;
          case CLEAR_RESERVATION_13:
              // 这里可以根据实际需求修改寄存器地址和数量
              Order[4] = 0X08;
              Order[5] = 0x00;
              break;
          default:
              // 可以根据实际情况处理默认情况
              return;
      }

    // 计算 CRC 校验值
	CRC_Value=ModBusCRC16(Order,6); // CRC校验
	Elite_crc_H = (uint8_t)CRC_Value;
	Elite_crc_L = (uint8_t)( CRC_Value>>8);

    Order[6] = Elite_crc_H;
    Order[7] = Elite_crc_L;

    // 调用串口发送函数
    GF_BSP_UART_Transmit(1, Order, 8);
}


void GF_MSP_Elite_Joint_Position_Inquiry()
{

	unsigned short CRC_Value;
	uint8_t Elite_crc_H;
	uint8_t Elite_crc_L;

    uint8_t Order[8];
    Order[0] = 0X03;
    Order[1] = 0X03;
    Order[2] = 0x00;
    Order[3] = 0xC8;

    Order[4] = 0X00;
    Order[5] = 0x08;
    // 计算 CRC 校验值
	CRC_Value=ModBusCRC16(Order,6); // CRC校验
	Elite_crc_H = (uint8_t)CRC_Value;
	Elite_crc_L = (uint8_t)( CRC_Value>>8);

    Order[6] = Elite_crc_H;
    Order[7] = Elite_crc_L;

    // 调用串口发送函数
    GF_BSP_UART_Transmit(1, Order, 8);
}

int Robot_Flag=1;


//03 03 10 F5 45 D7 2F 2D 3E E5 3D 21 85 0C D1 00 00 00 00 77 EC;
void  GF_Elite_Callback()
{
	unsigned short CRC_Value;
	switch(Robot_Flag)
	{
	case 1:
		if(Robot_stas[0]==0X03)
		{
			HAL_UART_Receive_IT(Huart_Elite,Robot_stas,2);
			Robot_Flag=2;
		}
		else
		{
			HAL_UART_Receive_IT(Huart_Elite,Robot_stas,1);
			Robot_Flag=1;
		}
		break;
	case 2:
		if((Robot_stas[0]==0X03)&&(Robot_stas[1]==0X10))
		{
			HAL_UART_Receive_IT(Huart_Elite,Robot_stas,16);
			Robot_Flag=3;
			Robot_stas_All[0]=0x03;
			Robot_stas_All[1]=0x03;
			Robot_stas_All[2]=0x10;
		}
		else
		{
			HAL_UART_Receive_IT(Huart_Elite,Robot_stas,1);
			Robot_Flag=1;
		}
		break;
	case 3:
		for(int i=0;i<16;i++)
		{
			Robot_stas_All[i+3] = Robot_stas[i];
		}
		HAL_UART_Receive_IT(Huart_Elite,Robot_stas_CRC,2);
		CRC_Value=ModBusCRC16(Robot_stas_All,19); // CRC校验
		Elite_crc_H = (uint8_t)CRC_Value;
		Elite_crc_L = (uint8_t)( CRC_Value>>8);
		Robot_Flag=4;
		break;
	case 4:
		if(Elite_crc_H==Robot_stas_CRC[0] && Elite_crc_L==Robot_stas_CRC[1])
			{
			Robot_Command_Comp(Robot_stas);
			Robot_Flag=1;
			}
		else
			{
			Robot_Flag=1;

			}

		HAL_UART_Receive_IT(Huart_Elite,Robot_stas,1);
		break;

		default:
			break;
	}

}

void  Robot_Command_Comp(uint8_t Angle_Byte[])
{
	Robot_Angele_1->Joint_Angle[0]=((double)((int16_t)((int16_t)Angle_Byte[0]<<8|Angle_Byte[1])))/5000;
	Robot_Angele_1->Joint_Angle[1]=((double)((int16_t)((int16_t)Angle_Byte[2]<<8|Angle_Byte[3])))/5000;;
	Robot_Angele_1->Joint_Angle[2]=((double)((int16_t)((int16_t)Angle_Byte[4]<<8|Angle_Byte[5])))/5000;;
	Robot_Angele_1->Joint_Angle[3]=((double)((int16_t)((int16_t)Angle_Byte[6]<<8|Angle_Byte[7])))/5000;;
	Robot_Angele_1->Joint_Angle[4]=((double)((int16_t)((int16_t)Angle_Byte[8]<<8|Angle_Byte[9])))/5000;;
	Robot_Angele_1->Joint_Angle[5]=((double)((int16_t)((int16_t)Angle_Byte[10]<<8|Angle_Byte[11])))/5000;;

}









