/*
 * motors.c
 *
 *  Created on: Nov 11, 2024
 *      Author: akeguo
 */
#include "motors.h"
#include "msp_TI5MOTOR.h"

FDCANHandler *FiveWheel_Motor_Controller;

DispacherController *FiveWheel_DispacherController;
void MotorCommandsLoop();
void FiveWheel_MotorDecodeCAN(uint32_t canID, uint8_t *buffer,
		uint32_t length);

void TankWashing_Motor_Controller_intialize(FDCANHandler *Handler)//CAN1的
{
	//初始化

	FiveWheel_Motor_Controller = Handler;
	FiveWheel_Motor_Controller->CAN_Decode =FiveWheel_MotorDecodeCAN;

	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, "Ti5_1",
			0, ComError_Ti5_LeftMotor);
	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, "Ti5_2",
			0, ComError_Ti5_RightMotor);
	FiveWheel_DispacherController = Handler->dispacherController;
	FiveWheel_DispacherController->DispacherCallTime = 100;
	FiveWheel_DispacherController->Add_Dispatcher_List(FiveWheel_DispacherController, MotorCommandsLoop);

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

		GetCSPByCommand(i, FiveWheel_Motor_Controller, 4); //get the data from the motors

		Motor_ClearFault(i, FiveWheel_Motor_Controller, 4);
		//Motor_SetVelocityModeAndTargetVelocity(i,Motor[i]->Target_Velcity);
	}
}
void MotorCommandsLoop()
{
	for (int i = 1; i < 3; i++)
	{
		Motor_ClearFault(i, FiveWheel_Motor_Controller, 4);
		Motor_GetFaultState(i, FiveWheel_Motor_Controller, 4);
		GetCSPByCommand(i, FiveWheel_Motor_Controller, 4);

	}
	for (int i = 1; i < 3; i++)

	{
		Motor_SetVelocityModeAndTargetVelocity(i, Motor[i]->Target_Velcity,
				FiveWheel_Motor_Controller, 4);
		ReadPosition(i, FiveWheel_Motor_Controller, 4);
		Motor_GetFaultState(i, FiveWheel_Motor_Controller, 4);
	}

}

void FiveWheel_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);
}