/*
 * LS_CAN_Motor_Frames.c
 *
 *  Created on: Mar 21, 2025
 *      Author: BingooRobotFJX
 */

#include <motors.h>
#include "fsm_state_control.h"
#include "BHBF_ROBOT.h"
#include "msp_TI5MOTOR.h"
FDCANHandler *TankWashing_Motor_Controller;

DispacherController *TankWashing_DispacherController;
void MotorCommandsLoop();
void TankWashing_MotorDecodeCAN(uint32_t canID, uint8_t *buffer,
		uint32_t length);
int32_t speed_E01m_per_min_to_hz(int32_t speed_E01m_per_min);
int32_t speed_degree_per_second_to_hz(int32_t degree_per_second);
char Ti5_Motor_Need_To_Activate = 0;
int  Activite_Delay=0;
/*
 *
 * */
void TankWashing_Motor_Controller_intialize(FDCANHandler *Handler)//CAN1的
{
	//初始化

	TankWashing_Motor_Controller = Handler;
	TankWashing_Motor_Controller->CAN_Decode =TankWashing_MotorDecodeCAN;

	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, "Ti5_1",
			0, ComError_Ti5_1);
	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, "Ti5_2",
			0, ComError_Ti5_2);
	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, "Ti5_3",
			0, ComError_Ti5_3);
	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, "Ti5_4",
			0, ComError_Ti5_4);
	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, "Ti5_5",
			0, ComError_Ti5_5);
	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, "Ti5_6",
			0, ComError_Ti5_6);

	TankWashing_DispacherController = Handler->dispacherController;
	TankWashing_DispacherController->DispacherCallTime = 100;
	TankWashing_DispacherController->Add_Dispatcher_List(TankWashing_DispacherController, MotorCommandsLoop);

	for (int i = 1; i < 7; i++)
	{

		GetCSPByCommand(i, TankWashing_Motor_Controller, 4); //get the data from the motors
		Motor_ClearFault(i, TankWashing_Motor_Controller, 4);
	}

	SetMinimumAllowableNegativeVelocity(5, -5000, TankWashing_Motor_Controller,
			4);
	SetMaximumAllowablePositiveVelocity(5, 5000, TankWashing_Motor_Controller,
			4);
	SetMinimumAllowableNegativeVelocity(6, -5000, TankWashing_Motor_Controller,
			4);
	SetMaximumAllowablePositiveVelocity(6, 5000, TankWashing_Motor_Controller,
			4);


}

/*
 *
 * */
void MotorCommandsLoop()
{
		if(Ti5_Motor_Need_To_Activate == 1)
		{
			if(++Activite_Delay<=80) return;

			for (int i = 1; i < 7; i++)
			{
				Motor_ClearFault(i, TankWashing_Motor_Controller, 4);
				Motor_GetFaultState(i, TankWashing_Motor_Controller, 4);
				GetCSPByCommand(i, TankWashing_Motor_Controller, 4);

			}
			for (int i = 1; i < 5; i++) /*four drive motors*/

			{
				Motor_SetVelocityModeAndTargetVelocity(i,speed_E01m_per_min_to_hz(Ti5_Motor[i]->Target_Velcity),
						TankWashing_Motor_Controller, 4);
			}

			Motor_SetVelocityModeAndTargetVelocity(5,speed_degree_per_second_to_hz(Ti5_Motor[5]->Target_Velcity),
									TankWashing_Motor_Controller, 4);
			if (Ti5_Motor[6]->Run_Mode == 1)	//电流模式
			{

				SetCurrentModeAndTargetCurent(6, Ti5_Motor[6]->Target_Current,
						TankWashing_Motor_Controller, 4);
			} else	// if(Motor[6]->Run_Mode==2)
			{
				Motor_SetVelocityModeAndTargetVelocity(6, speed_degree_per_second_to_hz(Ti5_Motor[6]->Target_Velcity),
						TankWashing_Motor_Controller, 4);
			}
		}
}



/*
 * 解析函数
 *
 * */
void TankWashing_MotorDecodeCAN(uint32_t canID, uint8_t *buffer,
		uint32_t length)
{

	switch (canID)
	{
		case 1:
		{
			HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
					"Ti5_1", 1);
		}
			break;
		case 2:
		{
			HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
					"Ti5_2", 1);
		}
			break;
		case 3:
		{
			HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
					"Ti5_3", 1);
		}
			break;
		case 4:
		{
			HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
					"Ti5_4", 1);
		}
			break;
		case 5:
		{
			HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
					"Ti5_5", 1);
		}
			break;
		case 6:
		{
			HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
					"Ti5_6", 1);
		}
			break;
	}

	DecodeTi5MotorCAN(canID, buffer, length);
}



/*
 *
 * 0.1m/min
 *
 * */
int32_t speed_E01m_per_min_to_hz(int32_t speed_E01m_per_min)
{
	return (int32_t) (speed_E01m_per_min*0.1 * 6 /360 *100 * CV.wheel_Reduction_Ratio
		        / (3.14 * CV.wheel_Diameter_m));
}
/*
 * °/s to hz  send to motors
 * */
int32_t speed_degree_per_second_to_hz(int32_t degree_per_second)
{
	return (int32_t) degree_per_second*60/360*6*100*101/360;
}