/*
 * motors.c
 *
 *  Created on: 4.3, 2026
 *      Author: SHI
 *      MKS驱动器，42步进电机
 *      波特率 115200
 */
#include "motors.h"

struct UARTHandler *Motor_485_Controller;
DispacherController *Motor_485_DispacherController;

void MotorCommandsLoop();
void Motor_485_Decode(uint8_t *buffer, uint16_t length);
int32_t speed_mpmin_to_rpmin(double speed_m_per_min);

void Motor_Controller_intialize(struct UARTHandler *Handler)//485
{
	//初始化

	Motor_485_Controller = Handler;
//	Motor_485_Controller->UART_Decode = Motor_485_Decode;
	Motor_485_Controller->Wait_time = 6; //等待10ms 最低不要低于4；

	Motor_485_DispacherController = Handler->dispacherController;
	Motor_485_DispacherController->Dispacher_Enable = 1;//不周期性发送
	Motor_485_DispacherController->DispacherCallTime = 100;
//	Motor_485_DispacherController->DispacherCallTime = 0;
	Motor_485_DispacherController->Add_Dispatcher_List(Motor_485_DispacherController,
			MotorCommandsLoop);
	//将 MotorCommandsLoop 函数注册到分发控制器dispacherController中，使其按照设定的周期被自动调用

	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, "Motor_485_1",
			0, ComError_LS_LeftMotor);
	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, "Motor_485_2",
			0, ComError_LS_RightMotor);
	LOG("Motor_485 initialized success");
//	for (int i = 1; i < 3; i++)
//	{
//
//		GetCSPByCommand(i, Motor_485_Controller, 4); //get the data from the motors
//
//		Motor_ClearFault(i, Motor_485_Controller, 4);
//		//Motor_SetVelocityModeAndTargetVelocity(i,Motor[i]->Target_Velcity);
//	}

}
void MotorCommandsLoop()
{
	int32_t Target_Velcity_Left  = speed_mpmin_to_rpmin(Motor485[1]->Target_Velcity);
	int32_t Target_Velcity_Right = speed_mpmin_to_rpmin(Motor485[2]->Target_Velcity);

	speedModeRun_485(Motor485[1]->MotorID, Target_Velcity_Left,  Motor_485_Controller); //从机地址，速度，
	speedModeRun_485(Motor485[2]->MotorID, Target_Velcity_Right, Motor_485_Controller); //从机地址，速度，

}

void Motor_485_Decode(uint8_t *buffer, uint16_t length)
{
	uint8_t canID = buffer[1];
	switch (canID)
	{
		case 1:
		{
			HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
					"Motor_485_1", 1);
		}
			break;
		case 2:
		{
			HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
					"Motor_485_2", 1);
		}
			break;

	}

	Decode485Motor(buffer, length);
}

int32_t speed_mpmin_to_rpmin(double speed_m_per_min)
{
	return (int32_t) (speed_m_per_min * CV.wheel_Reduction_Ratio
	                    / (3.14159f * CV.wheel_Diameter_m));
	//speed_m_per_min=10 对应 796 RPM
}