/*
 * 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 "../../BASE/Inc/BSP/bsp_include.h"
#include "../../BASE/Inc/BSP/bsp_FDCAN.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 void ActivateMotor(int MotorID, FDCANHandler *ZQ_Motor_Controller,
        int32_t WaitTime) ;

extern void CANSendMessageSDO_ADD_To_SendList(int 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(int MotorID, int TargetPosition,
        int TargetSpeed, int AccTime, int DecTime,
        FDCANHandler *ZQ_Motor_Controller, int32_t WaitTime);
extern void Postion_Velcocity_Set_Position(int MotorID, int TargetPosition,
        FDCANHandler *ZQ_Motor_Controller, int32_t WaitTime) ;

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

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

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


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 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 SpeedModeSetup(int 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);
#endif /* INC_MSP_MSP_TTMOTOR_ZQ_H_ */