/*
 * msp_TTMotor_ZQ.h
 *
 *  Created on: Oct 10, 2024
 *      Author: akeguo
 */

#ifndef INC_MSP_MSP_TTMOTOR_ZQ_H_
#define INC_MSP_MSP_TTMOTOR_ZQ_H_
#include "bsp_include.h"
#include "bsp_FDCAN.h"
#include "msp_ZQ_MotorParameters.pb.h"


#include "fsm.h"

#define DF_MSP_TT_Motor_PositionMode 	01
#define DF_MSP_TT_Motor_SpeedMode 		02
#define DF_MSP_TT_Motor_CurrentMode 	03
#define DF_MSP_TT_Motor_Posi_Now 	04

#define TT_One_Deg_Count 	11014///32768*121/360(减速比121)=11014

#define TT_Left_limt    84080      //-775000+75*11014
#define TT_Right_limt  -1568020  //-775000-75*11014
//#define Sw_Rust_Mid_Posi  -775000  //-775000-75*11014

//double Sw_Rust_Mid_Posi=-775000;//需检测修改
#define DF_MSP_TT_Motor_StartID 2

void SwingMotorSetCheckTargetPositon();

void TT_Analytic_Fun(int32_t ID_A_T_A, char *buffer);

extern int SwingMotorID;
extern int LeftMotorID;
extern int RightMotorID;
extern TT_MotorParameters   *TT_Motor[7];

extern void ActivateMotor(int32_t MotorID, FDCANHandler *ZQ_Motor_Controller,
        int32_t WaitTime) ;

extern void CANSendMessageSDO_ADD_To_SendList(int32_t MotorID, uint8_t Function,
        uint16_t ControlWord, uint8_t subWord, int32_t ControlWordValue,
        FDCANHandler *ZQ_Motor_Controller, int32_t WaitTime);

//位置速度模式
extern void Postion_Velcocity_Run_SetParameter(int32_t MotorID, int32_t TargetPosition,
		int32_t TargetSpeed, int32_t AccTime, int32_t DecTime,
        FDCANHandler *ZQ_Motor_Controller, int32_t WaitTime);
extern void Postion_Velcocity_Set_Position(int32_t MotorID, int32_t TargetPosition,
        FDCANHandler *ZQ_Motor_Controller, int32_t WaitTime) ;

extern void Driver_ReadError(int32_t MotorID, FDCANHandler *ZQ_Motor_Controller,
        int32_t WaitTime);
extern void SetMotorTargetPosition(int32_t MotorID, int32_t TargetPosition,
        FDCANHandler *ZQ_Motor_Controller, int32_t WaitTime) ;

extern void Postion_Velcocity_Stop(int32_t MotorID, FDCANHandler *ZQ_Motor_Controller,
        int32_t WaitTime) ;

//extern void SetCurrentPositionZero(int MotorID, FDCANHandler *ZQ_Motor_Controller,
//        int32_t WaitTime) ;

extern void Motor_Controller_intialize(FDCANHandler *Handler);

extern void Swing_Motor_Set_Target_Position() ;

extern void Swing_Motor_Read_ReachedEnd() ;
extern void Set_Current_Positon_Zero(uint8_t MotorID,
        FDCANHandler *ZQ_Motor_Controller, int32_t WaitTime) ;

extern void TT_Request_Position(uint32_t Motor_ID_1,
        FDCANHandler *ZQ_Motor_Controller, int32_t WaitTime) ;
extern void TT_Request_Velocity(uint32_t Motor_ID_1,
        FDCANHandler *ZQ_Motor_Controller, int32_t WaitTime) ;


extern void Position_Immediately_Setting(uint8_t MotorID, FDCANHandler *ZQ_Motor_Controller,
		int32_t Deri_Position,int32_t Deri_Speed, int32_t WaitTime);  // Home  设置当前位置为零点

extern void Position_Lag_Setting(uint8_t MotorID, FDCANHandler *ZQ_Motor_Controller,
		int32_t Deri_Position,int32_t Deri_Speed, int32_t WaitTime) ; // Home  设置当前位置为零点

extern void TT_Request_Fault(uint32_t Motor_ID_1,
        FDCANHandler *ZQ_Motor_Controller, int32_t WaitTime) ;

extern void TT_Request_Current(uint32_t Motor_ID_1,
        FDCANHandler *ZQ_Motor_Controller, int32_t WaitTime);

extern void TT_Analytic_Fun(int32_t ID_A_T_A, char *buffer);


extern void TT_Analytic_Fun(int32_t ID_A_T_A, char *buffer);

extern void SpeedModeSetup(int32_t MotorID,FDCANHandler *ZQ_Motor_Controller,int32_t WaitTime,int32_t Acc, int32_t Dec, int32_t TargetVelocity); //设定速度模式，并更改相关速度

extern void TT_Analytic_Fun(int32_t ID_A_T_A, char *buffer);
extern void TT_SpeedMode_Set_TargetSpeed(uint32_t MotorID,FDCANHandler *ZQ_Motor_Controller,int32_t WaitTime,int32_t TargetSpeed);


extern void Enable_NMT(int32_t MotorID, FDCANHandler *ZQ_Motor_Controller, int32_t Node_Number,  int32_t WaitTime);
extern void Configure_Asynchronous_Mode(int32_t MotorID, FDCANHandler *ZQ_Motor_Controller, int32_t Node_Number,  int32_t WaitTime);
extern void Consumer_Or_microcontroller_Heartbeat(int32_t MotorID, FDCANHandler *ZQ_Motor_Controller,   int32_t WaitTime);
#endif /* INC_MSP_MSP_TTMOTOR_ZQ_H_ */