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

char TT_Motor_Need_To_Activate = 0;

FDCANHandler *Roughening_Motor_Controller;

DispacherController *Roughening_DispacherController;

#define LeftMotorID 1
#define RightMotorID 2

void MotorCommandsLoop();
void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length);

void Roughening_Motor_Controller_intialize(FDCANHandler *Handler)
{
	//初始化

	Roughening_Motor_Controller = Handler;
	Roughening_Motor_Controller->CAN_Decode = Roughening_MotorDecodeCAN;

	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,
	        "ZQ_CAN_ID2_LeftMotor", 0, ComError_ZQ_LeftMotor);
	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,
	        "ZQ_CAN_ID3_RightMotor", 0, ComError_ZQ_RightMotor);

	Roughening_DispacherController = Handler->dispacherController;
	Roughening_DispacherController->DispacherCallTime = 2;
	Roughening_DispacherController->Add_Dispatcher_List(
	        Roughening_DispacherController, MotorCommandsLoop);

	LOGFF(DL_WARN,"TT_Motors_intialize");

}
void MotorCommandsLoop()
{

	if (TT_Motor_Need_To_Activate == 1)
	{
//		ActivateMotor(LeftMotorID, Roughening_Motor_Controller, 6000);


		ActivateMotor(LeftMotorID, Roughening_Motor_Controller, 2000);
		ActivateMotor(RightMotorID, Roughening_Motor_Controller, 2000);



		SpeedModeSetup(LeftMotorID, Roughening_Motor_Controller, 6, 150, 150, 0);
		SpeedModeSetup(RightMotorID, Roughening_Motor_Controller, 6, 150, 150, 0);
//		SpeedModeSetup(LeftMotorID, Roughening_Motor_Controller, 6, 3500, 450, 0);
//		SpeedModeSetup(RightMotorID, Roughening_Motor_Controller, 6, 3500, 450, 0);
		TT_Motor_Need_To_Activate = 2;
	}
	else if (TT_Motor_Need_To_Activate == 2)
	{
		for (int i = 1; i < 3; i++)
		{
			TT_Request_Position(i, Roughening_Motor_Controller, 6);
			TT_Request_Velocity(i, Roughening_Motor_Controller, 6);
			TT_Request_Current(i, Roughening_Motor_Controller, 6);
			TT_Request_Fault(i, Roughening_Motor_Controller, 6);
			TT_Request_Tempature(i, Roughening_Motor_Controller, 6);
		}

		TT_SpeedMode_Set_TargetSpeed(LeftMotorID, Roughening_Motor_Controller, 6,
		        GV.LeftMotor.Target_Velcity);
		TT_SpeedMode_Set_TargetSpeed(RightMotorID, Roughening_Motor_Controller, 6,
		        -GV.RightMotor.Target_Velcity);

	}

}

char a[10];
void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length)
{

	memcpy(a,buffer,8);
	switch (canID - 0x580)
	{

		case LeftMotorID:
		{
			HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
			        "ZQ_CAN_ID2_LeftMotor", 1);
			TT_Analytic_Fun(LeftMotorID, buffer);
		}
			break;
		case RightMotorID:
		{
			HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
			        "ZQ_CAN_ID3_RightMotor", 1);
			TT_Analytic_Fun(RightMotorID, buffer);
		}
			break;
	}

}