/*
 * msp_TTMotor_ZQ.c
 *
 *  Created on: Oct 10, 2024
 *      Author: akeguo
 */
#include "msp_TTMotor_ZQ.h"
#include "bsp_FDCAN.h"

FDCANHandler *ZQ_Motor_Controller;
HardWareController *ZQ_HardController;
DispacherController *ZQ_DispacherController;
void Send1CANTest();
void ZQ_Motor_Controller_intialize(FDCANHandler *Handler)
{

	ZQ_Motor_Controller = Handler;
	ZQ_Motor_Controller->CAN_Decode = DecodeMotorCAN;

	ZQ_HardController = Handler->canHardWareController;
	//there are three motors, thus add three hardwares
	ZQ_HardController->Add_PCOMHardWare(ZQ_HardController,
			"ZQ_CAN_ID0_LeftMotor", 0);
	ZQ_HardController->Add_PCOMHardWare(ZQ_HardController,
			"ZQ_CAN_ID1_RightMotor", 0);
	ZQ_HardController->Add_PCOMHardWare(ZQ_HardController,
			"ZQ_CAN_ID2_SwingMotor", 0);

	ZQ_DispacherController = Handler->dispacherController;
//	ZQ_DispacherController->Add_Dispatcher_List(ZQ_DispacherController,
//			Send1CANTest);
}

void Send1CANTest()
{
	uint8_t data[8] =
	{ 1, 2, 3, 4, 5, 6, 7, 8 };
	ZQ_Motor_Controller->CAN_Send(ZQ_Motor_Controller, 20, 8, data);
}

void DecodeMotorCAN(uint32_t canID, uint8_t *buffer, uint32_t length)
{




	//buffer[0] is CAN ID;
	if (1)
	{
		ZQ_HardController->Set_PCOMHardWare(ZQ_HardController,
				"ZQ_CAN_ID0_LeftMotor", 1);
	}
	if (1)
	{
		ZQ_HardController->Set_PCOMHardWare(ZQ_HardController,
				"ZQ_CAN_ID1_RightMotor", 1);
	}
	if (1)
	{
		ZQ_HardController->Set_PCOMHardWare(ZQ_HardController,
				"ZQ_CAN_ID2_SwingMotor", 1);
	}
}

//sub function region
//activate the motor
void ActivateMotor(int CANID)
{

	memcpy(&ZQ_Motor_Controller->Tx_Buf, 0x10, 0x10);
	ZQ_Motor_Controller->CAN_Send(ZQ_Motor_Controller, CANID, 2,
			ZQ_Motor_Controller->Tx_Buf);
}

void CANSendMessageSDO(int CAN_ID, uint8_t Function, uint16_t ControlWord,
		uint8_t subWord, int32_t ControlWordValue)
{
	//copy the corresponsiding data to the send Array
	ZQ_Motor_Controller->Tx_Buf[0] = Function;
	memcpy(&ZQ_Motor_Controller->Tx_Buf[1], &ControlWord, 2);
	ZQ_Motor_Controller->Tx_Buf[3] = subWord;
	memcpy(&ZQ_Motor_Controller->Tx_Buf[4], &ControlWordValue, 4);
	//send 8 bytes data
	ZQ_Motor_Controller->CAN_Send(ZQ_Motor_Controller, 600 + CAN_ID, 8,
			ZQ_Motor_Controller->Tx_Buf);

}

void CANSendMessageSDO_ADD_To_SendList(int CAN_ID, uint8_t Function,
		uint16_t ControlWord, uint8_t subWord, int32_t ControlWordValue)
{
	//copy the corresponsiding data to the send Array
	ZQ_Motor_Controller->Tx_Buf[0] = Function;
	memcpy(&ZQ_Motor_Controller->Tx_Buf[1], &ControlWord, 2);
	ZQ_Motor_Controller->Tx_Buf[3] = subWord;
	memcpy(&ZQ_Motor_Controller->Tx_Buf[4], &ControlWordValue, 4);
	//send 8 bytes data

	ZQ_Motor_Controller->AddCANSendList(ZQ_Motor_Controller, 600 + CAN_ID, 8,
			ZQ_Motor_Controller->Tx_Buf, NULL);
}

void Postion_Velcocity_Run_SetMode(int CAN_ID)
{
	CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2f, 0x6060, 0x00, 1); //设置操作模式
}

void Postion_Velcocity_Run_SetParameter(int CAN_ID, int TargetPosition,
		int TargetSpeed, int AccTime, int DecTime)
{
	if (TargetSpeed >= 2500)
	{
		TargetSpeed = 2500;
	}
	if (TargetSpeed < 0)
	{
		TargetSpeed = 0;
	}
	CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x23, 0x6083, 0x00, AccTime); //设置加速时间
	// Thread.Sleep(2);
	CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x23, 0x6084, 0x00, DecTime); //设置减速时间
	//Thread.Sleep(2);
	CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x23, 0x607a, 0x00,
			TargetPosition);            //设置目标位置
	// Thread.Sleep(2);
	CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x23, 0x6081, 0x00,
			TargetSpeed * 10);           //设置运行速度  0.1rpm
	// Thread.Sleep(2);

}

void Postion_Velcocity_Run_Start(int CAN_ID)
{
	//Thread.Sleep(1);
	CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x6040, 0x00, 0x80); ///6:清除异常，向控制字
	//Thread.Sleep(1);
	CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x6040, 0x00, 0x06); ///伺服准备
	//Thread.Sleep(1);
	CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x6040, 0x00, 0x07); ///：伺服等待使能

	//Thread.Sleep(1);
	CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x6040, 0x00, 0x2f);    ///：
	//Thread.Sleep(1);
	CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x6040, 0x00, 0x3f);    ///：
	// Thread.Sleep(1);
	//Thread.Sleep(1); CANSendMessageSDO(CAN_ID, 0x2b, 0x6040, 0x00, 0x1f);///：
}
Postion_Velcocity_Run_Set_Position(int CAN_ID, int TargetPosition)
{
	CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x23, 0x607a, 0x00,
			TargetPosition);    //设置目标位置
	//Thread.Sleep(2);
	CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x6040, 0x00, 0x0f);    ///：

	CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x6040, 0x00, 0x3f);    ///：
}
void Driver_ReadError(int CAN_ID)
{
	//CANSendMessageSDO_ADD_To_SendList(CAN_ID, new byte[8] { 0x40, 0x0A, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00 });//40 0A 30 00 00 00 00 00

	CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x40, 0x300a, 0x00, 0); //40 0A 30 00 00 00 00 00
}
void SetMotorTargetPosition(int MotorID, int32_t TargetPosition)
{

	Postion_Velcocity_Run_Set_Position(MotorID, TargetPosition);

}
/// <summary>
/// 读取是否到最终达位置
/// </summary>
/// <param name="CAN_ID"></param>
void Driver_Read_Read_ReachedEnd(int CAN_ID)
{
	// CANSendMessageSDO(CAN_ID, new byte[8] { 0x40, 0x0A, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00 });//40 0A 30 00 00 00 00 00
	//Thread.Sleep(1);
	CANSendMessageSDO(CAN_ID, 0x40, 0x6041, 0x00, 0x00);     ///：

}
void Postion_Velcocity_Stop(int CANID)
{

	CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x6085, 0x00, 200); // 6085 的减速时间100ms
	CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x605a, 0x00, 6); // 0x06：按索引 6085 的减速时间进行减速，停下时电机 轴处于锁轴状态。
	CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x6040, 0x00, 0x02); //快速停机
}

void SetCurrentPositionZero(int CANID)
{
	CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2f, 0x6098, 0x00, 0x23); //1：设置定位的方式,向索	引0x6098:00写入0X23
	CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2f, 0x6060, 0x00, 6); //2：设置操作模式，向索引0x6060:00写入0x06
	CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x6040, 0x00, 0x80); //3:清除异常，向控制字	0x6040:00写入0x80
	CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x6040, 0x00, 0x06); //4：伺服准备，向控制字	0x6040:00写入0x06
	CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x6040, 0x00, 0x07); //5：伺服等待使能，向控制字0x6040:00写入0x07
	CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x6040, 0x00, 0x0F); //6：伺服使能，向控制字0x6040:00写入0x0F
	CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2f, 0x6040, 0x00, 0x1F); //7：启动原点定位,向控制字0x6040:00写入0x1F
}
void Intialize(int CANID, int intializePosition, int Speed, int AccTime,
		int DecTime, int waitTime)
{
	// CANSendMessage(CAN_ID, 0, new byte[] { 0x10, 0x10 }, 2);//Wait 2ms  Bootloader

	//copy the corresponsiding data to the send Array
	ZQ_Motor_Controller->Tx_Buf[0] = 0x10;
	ZQ_Motor_Controller->Tx_Buf[1] = 0x10;

	ZQ_Motor_Controller->AddCANSendList(ZQ_Motor_Controller, CAN_ID, 2,
			ZQ_Motor_Controller->Tx_Buf, NULL);


	Postion_Velcocity_Stop(CANID);
}

void BootLoader(int CAN_ID)
{
	ZQ_Motor_Controller->Tx_Buf[0] = 0x10;
	ZQ_Motor_Controller->Tx_Buf[1] = 0x10;

	ZQ_Motor_Controller->AddCANSendList(ZQ_Motor_Controller, CAN_ID, 2,
			ZQ_Motor_Controller->Tx_Buf, NULL);
}

  MotorParameters motorParameters[4];
 /// <summary>
 /// 电机驱动器状态刷新
 /// </summary>
 /// <param name="Data">数据</param>
 /// <param name="len">数据长度</param>
 void MotorStatusRefresh(char* Data, uint8_t len)
 {



 }