/*
 * 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, Ti5_1);
	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, "Ti5_2",
			0, Ti5_2);
//	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, "Ti5_3",
//			0, Ti5_3);
//	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, "Ti5_4",
//			0, Ti5_4);
//	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, "Ti5_5",
//			0, Ti5_5);
//	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, "Ti5_6",
//			0, Ti5_6);

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

//	SetMinimumAllowableNegativeVelocity(5, -3000, TankWashing_Motor_Controller,
//			4);
//	SetMaximumAllowablePositiveVelocity(5, 3000, TankWashing_Motor_Controller,
//			4);
//	SetMinimumAllowableNegativeVelocity(6, -3000, TankWashing_Motor_Controller,
//			4);
//	SetMaximumAllowablePositiveVelocity(6, 3000, TankWashing_Motor_Controller,
//			4);


}
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);
	}
//	Motor_SetVelocityModeAndTargetVelocity(2, Motor[2]->Target_Velcity,
//					TankWashing_Motor_Controller, 4);
//	Motor_SetVelocityModeAndTargetVelocity(4, Motor[4]->Target_Velcity,
//					TankWashing_Motor_Controller, 4);
	//SetMaximumPositivePosition(5, Motor[5]->EncoderOffset+900);
	//SetMinimumNegativePosition(5,Motor[5]->EncoderOffset-900);

//	if (Motor[6]->Run_Mode == 1)	//电流模式
//	{
//
//		SetCurrentModeAndTargetCurent(6, Motor[6]->Target_Current,
//				TankWashing_Motor_Controller, 4);
//	} else	// if(Motor[6]->Run_Mode==2)
//	{
//		Motor_SetVelocityModeAndTargetVelocity(6, Motor[6]->Target_Velcity,
//				TankWashing_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);
}