/*
 * msp_TI5MOTOR.c
 *
 *  Created on: Oct 26, 2023
 *      Author: shiya
 */
#include "MSP/msp_Elevated_Vehicle.h"

void GF_MSP_Elevated_CANRx_Callback_CANOPEN();

typedef enum {
	CLEAR_ALARM = 0x0001,
    CLEAR_DATA = 0x0000,
    SERVO_POWER_ON = 0x0002,
    PROGRAM_START = 0x0004,
    RETURN_TO_START = 0x0008,
    EMERGENCY_STOP = 0x0010,
    PAUSE_PROGRAM = 0x0020,
    SERVO_POWER_OFF = 0x0040,
    CLEAR_COLLISION_ALARM = 0x0080,
    PROGRAM_END_FEEDBACK = 0x0100,
    RESERVED_PROGRAM_1 = 0x0200,
    RESERVED_PROGRAM_2 = 0x0400,
    CLEAR_RESERVATION = 0x0800
} Robot_Action;

Elevated_Vhicle_Prarameters*  Elevated_CANOPEN;
uint8_t Elevated_FDCAN_CH;


uint8_t GF_MSP_Elevated_Vehicle_Init(uint8_t FDCAN_CH, Elevated_Vhicle_Prarameters* _Vhicle)
{
	Elevated_FDCAN_CH=FDCAN_CH;
	Elevated_CANOPEN=_Vhicle;
	GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_FDCAN1_RxFifo0Callback, GF_MSP_Elevated_CANRx_Callback_CANOPEN);

	return 1;
}

typedef struct {
    uint8_t data[8];
} CAN_Message;

uint16_t combined_bytes = 0;
void GF_MSP_Elevated_CANRx_Callback_CANOPEN()
{

	if(CAN_ID==0x588)
	{
		combined_bytes = ((uint16_t)CAN_Buf[4] << 8) | CAN_Buf[5];
		switch ((Robot_Action)combined_bytes) {

				case CLEAR_ALARM:
					// 执行清除数据动作
					// 这里可以添加具体的清除数据代码
					Elevated_CANOPEN->Robot_Mode=1;
					break;

		        case CLEAR_DATA:
		            // 执行清除数据动作
		            // 这里可以添加具体的清除数据代码
		        	Elevated_CANOPEN->Robot_Mode=2;
		            break;
		        case SERVO_POWER_ON:
		            // 执行伺服上电动作
		            // 这里可以添加具体的伺服上电代码
		        	Elevated_CANOPEN->Robot_Mode=3;
		            break;
		        case PROGRAM_START:
		            // 执行程序启动动作
		            // 这里可以添加具体的程序启动代码
		        	Elevated_CANOPEN->Robot_Mode=4;
		            break;
		        case RETURN_TO_START:
		            // 执行回程序首行动作
		            // 这里可以添加具体的回程序首行代码
		        	Elevated_CANOPEN->Robot_Mode=5;
		            break;
		        case EMERGENCY_STOP:
		            // 执行急停动作
		            // 这里可以添加具体的急停代码
		        	Elevated_CANOPEN->Robot_Mode=6;
		            break;
		        case PAUSE_PROGRAM:
		            // 执行暂停动作
		            // 这里可以添加具体的暂停代码
		        	Elevated_CANOPEN->Robot_Mode=7;
		            break;
		        case SERVO_POWER_OFF:
		            // 执行关闭伺服动作
		            // 这里可以添加具体的关闭伺服代码
		        	Elevated_CANOPEN->Robot_Mode=8;
		            break;
		        case CLEAR_COLLISION_ALARM:
		            // 执行清除碰撞报警动作
		            // 这里可以添加具体的清除碰撞报警代码
		        	Elevated_CANOPEN->Robot_Mode=9;
		            break;
		        case PROGRAM_END_FEEDBACK:
		            // 执行程序结束反馈动作
		            // 这里可以添加具体的程序结束反馈代码
		        	Elevated_CANOPEN->Robot_Mode=10;
		            break;
		        case RESERVED_PROGRAM_1:
		            // 执行预约程序1动作
		            // 这里可以添加具体的预约程序1代码
		        	Elevated_CANOPEN->Robot_Mode=11;
		            break;
		        case RESERVED_PROGRAM_2:
		            // 执行预约程序2动作
		            // 这里可以添加具体的预约程序2代码
		        	Elevated_CANOPEN->Robot_Mode=12;
		            break;
		        case CLEAR_RESERVATION:
		            // 执行清空预约动作
		            // 这里可以添加具体的清空预约代码
		        	Elevated_CANOPEN->Robot_Mode=13;
		            break;
		        default:
		            // 处理未知报文
		            break;
		    }
	}

}

int Control_State_Elevated=0;
void GF_MSP_Elevated_Timer()
{

	switch(Control_State_Elevated)
	{
	case 0:
		Elevated_Stutas_Control();
		Control_State_Elevated++;
		break;
	case 5:
		Control_State_Elevated++;
		break;
	case 10:
		Elevated_Distance_Stutas();
		Control_State_Elevated=0;
		break;

	default:
		Control_State_Elevated++;
		break;
	}
}


void FDCANfunction_Elevated(uint32_t ID,uint8_t Length,uint8_t *Data)
{
	GF_BSP_FDCAN_Senddata(Elevated_FDCAN_CH,ID,Length,Data);
}

void Elevated_Stutas_Control()
{
	uint8_t Pos_can_buff_Motor_To[8];
	uint32_t Motor_ID_1=0X610;
	int Length_date_1=2;//
	Pos_can_buff_Motor_To[0]=Elevated_CANOPEN->Device_Status;
	Pos_can_buff_Motor_To[1]=Elevated_CANOPEN->Paint_Stutas;

	FDCANfunction_Elevated(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}



void Elevated_Distance_Stutas()
{
	uint8_t Pos_can_buff_Motor_To[8];
	uint32_t Motor_ID_1=0X611;
	int Length_date_1=8;//
	Pos_can_buff_Motor_To[0]=(uint8_t)(Elevated_CANOPEN->Ranging_Sensor_Data[0]>>8);
	Pos_can_buff_Motor_To[1]=(uint8_t)(Elevated_CANOPEN->Ranging_Sensor_Data[0] & 0xFF);

	Pos_can_buff_Motor_To[2]=(uint8_t)(Elevated_CANOPEN->Ranging_Sensor_Data[1]>>8);
	Pos_can_buff_Motor_To[3]=(uint8_t)(Elevated_CANOPEN->Ranging_Sensor_Data[1] & 0xFF);

	Pos_can_buff_Motor_To[4]=(uint8_t)(Elevated_CANOPEN->Ranging_Sensor_Data[2]>>8);
	Pos_can_buff_Motor_To[5]=(uint8_t)(Elevated_CANOPEN->Ranging_Sensor_Data[2] & 0xFF);

	Pos_can_buff_Motor_To[6]=(uint8_t)(Elevated_CANOPEN->Ranging_Sensor_Data[3]>>8);
	Pos_can_buff_Motor_To[7]=(uint8_t)(Elevated_CANOPEN->Ranging_Sensor_Data[3] & 0xFF);

	FDCANfunction_Elevated(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}




