diff --git a/Bingoo/common/CMakeLists.txt b/Bingoo/CANmotor/CMakeLists.txt similarity index 72% rename from Bingoo/common/CMakeLists.txt rename to Bingoo/CANmotor/CMakeLists.txt index b746c1d..06cceae 100644 --- a/Bingoo/common/CMakeLists.txt +++ b/Bingoo/CANmotor/CMakeLists.txt @@ -9,7 +9,7 @@ else() endif() if(TARGET base) - target_link_libraries(common PUBLIC base) + target_link_libraries(CANmotor PUBLIC base) else() - message(WARNING "[common] Dependency 'base' not found.") + message(WARNING "[CANmotor] Dependency 'base' not found.") endif() \ No newline at end of file diff --git a/Bingoo/common/bsp_FDCAN.c b/Bingoo/CANmotor/bsp_FDCAN.c similarity index 99% rename from Bingoo/common/bsp_FDCAN.c rename to Bingoo/CANmotor/bsp_FDCAN.c index 762fbc3..1326a75 100644 --- a/Bingoo/common/bsp_FDCAN.c +++ b/Bingoo/CANmotor/bsp_FDCAN.c @@ -4,7 +4,7 @@ * Created on: Oct 26, 2023 * Author: shiya */ -#ifdef BUILD_COMMON +#ifdef BUILD_CANMOTOR #include "bsp_FDCAN.h" #include "main.h" #include @@ -326,4 +326,4 @@ void CANHandlerAddTxList(FDCANHandler *handler, uint32_t CAN_ID, } -#endif \ No newline at end of file +#endif diff --git a/Bingoo/common/include/bsp_FDCAN.h b/Bingoo/CANmotor/include/bsp_FDCAN.h similarity index 99% rename from Bingoo/common/include/bsp_FDCAN.h rename to Bingoo/CANmotor/include/bsp_FDCAN.h index 7459ce6..5d5e68b 100644 --- a/Bingoo/common/include/bsp_FDCAN.h +++ b/Bingoo/CANmotor/include/bsp_FDCAN.h @@ -4,7 +4,7 @@ * Created on: Oct 26, 2023 * Author: shiya */ -#ifdef BUILD_COMMON +#ifdef BUILD_CANMOTOR #ifndef INC_BSP_FDCAN_H_ #define INC_BSP_FDCAN_H_ #include "bsp_Error.pb.h" diff --git a/Bingoo/CANmotor/include/motor.h b/Bingoo/CANmotor/include/motor.h new file mode 100644 index 0000000..c51d6dc --- /dev/null +++ b/Bingoo/CANmotor/include/motor.h @@ -0,0 +1,45 @@ +/* + * motor.h + * + * Created on: 2026年4月20日 + * Author: Lizongdi + */ +#ifdef BUILD_CANMOTOR +#ifndef FSM_INC_MOTOR_H_ +#define FSM_INC_MOTOR_H_ + +#include "bsp_FDCAN.h" +#include "BHBF_ROBOT.h" + +typedef enum +{ + M_TYPE_MOVELEFT, + M_TYPE_MOVERIGHT, + M_TYPE_SWING +}EMotorType; + +typedef struct +{ + int32_t MotorID; + EMotorType MotorType; + int32_t HeartbeatID; + FDCANHandler *fHandler; + TT_MotorParameters *pMotorParam; +}TMotorCtrl; + +/*待适配接口*/ +void MotorGetState(TMotorCtrl *ptMotorCtrl, char *buffer); + +void WheelFuncInit(TMotorCtrl *ptLeftMotorCtrl, TMotorCtrl *ptRightMotorCtrl); +void WheelFuncProc(TMotorCtrl *ptLeftMotorCtrl, TMotorCtrl *ptRightMotorCtrl); +void WheelSetVelcity(TMotorCtrl *ptMotorCtrl, float fSpeed_M_min); + +void SwingFuncInit(TMotorCtrl *ptMotorCtrl); +void SwingFuncProc(TMotorCtrl *ptMotorCtrl); +void SwingSetPosition(TMotorCtrl *ptMotorCtrl, float fPositionAngle, int32_t iVelcityDegreeS); + +/*对外接口声明*/ +void MotorInit(void); + +#endif +#endif /* FSM_INC_MOTOR_H_ */ diff --git a/Bingoo/CANmotor/motor.c b/Bingoo/CANmotor/motor.c new file mode 100644 index 0000000..6bb6d3c --- /dev/null +++ b/Bingoo/CANmotor/motor.c @@ -0,0 +1,141 @@ +/* + * motor.c + * + * Created on: 2026年4月20日 + * Author: Lizongdi + */ +#ifdef BUILD_CANMOTOR + +#include "motor.h" + +TMotorCtrl *g_leftmotor; +TMotorCtrl *g_rightmotor; +TMotorCtrl *g_swingmotor; + +char a[10]; +int32_t fluat_flag=0; + +static void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length) +{ + + memcpy(a,buffer,8); + fluat_flag = canID - 0x580; + switch (fluat_flag) + { + case 1: + { + HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID1_LeftMotor", 1); + MotorGetState(g_leftmotor, buffer); + } + break; + case 2: + { + HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID2_RightMotor", 1); + MotorGetState(g_rightmotor, buffer); + } + break; + case 3: + { + HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "ZQ_CAN_ID3_SwingMotor", 1); + MotorGetState(g_swingmotor, buffer); + } + break; + } + +} + +static void MotorCommandsLoop() +{ + static char TT_Motor_Need_To_Activate = 1; + WheelSetVelcity(g_leftmotor, GV.Left_Speed_M_min); + WheelSetVelcity(g_rightmotor, GV.Right_Speed_M_min); + + if (TT_Motor_Need_To_Activate == 1) + { + WheelFuncInit(g_leftmotor, g_rightmotor); + TT_Motor_Need_To_Activate = 0; + } + else + { + WheelFuncProc(g_leftmotor, g_rightmotor); + } +} + +static void MotorCommandsLoop_2_Position() +{ + static char TT_Motor_Need_To_Activate_1 = 1; + SwingSetPosition(g_swingmotor, GV.Tar_Position_angle, GV.Tar_Position_Velcity_Degree_S); + + if (TT_Motor_Need_To_Activate_1 == 1) + { + SwingFuncInit(g_swingmotor); + TT_Motor_Need_To_Activate_1 = 0; + } + else + { + SwingFuncProc(g_swingmotor); + } +} + +static TMotorCtrl *MotorControllerInit(int32_t MotorID, EMotorType MotorType, + int32_t HeartbeatID, FDCANHandler *fHandler) +{ + TMotorCtrl *ptMotorCtrl = malloc(sizeof(TMotorCtrl)); + + if (ptMotorCtrl == NULL) + { + return NULL; + } + + ptMotorCtrl->pMotorParam = malloc(sizeof(TT_MotorParameters)); + + if (ptMotorCtrl->pMotorParam == NULL) + { + return NULL; + } + + if (M_TYPE_MOVELEFT == MotorType) + { + ptMotorCtrl->pMotorParam = &GV.LeftMotor; + HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID1_LeftMotor", 1, ComError_ZQ_CAN_ID1_LeftMotor); + } + else if (M_TYPE_MOVERIGHT == MotorType) + { + ptMotorCtrl->pMotorParam = &GV.RightMotor; + HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID2_RightMotor", 1, ComError_ZQ_CAN_ID2_RightMotor); + } + else if (M_TYPE_SWING == MotorType) + { + ptMotorCtrl->pMotorParam = &GV.SwingMotor; + HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID3_SwingMotor", 1, ComError_ZQ_CAN_ID3_SwingMotor); + } + + ptMotorCtrl->pMotorParam->MotorID = MotorID; + ptMotorCtrl->MotorID = MotorID; + ptMotorCtrl->MotorType = MotorType; + ptMotorCtrl->HeartbeatID = HeartbeatID; + ptMotorCtrl->fHandler = fHandler; + + ptMotorCtrl->fHandler->CAN_Decode = Roughening_MotorDecodeCAN; + ptMotorCtrl->fHandler->dispacherController->DispacherCallTime = 2; + + if (M_TYPE_SWING == MotorType) + { + ptMotorCtrl->fHandler->dispacherController->Add_Dispatcher_List(ptMotorCtrl->fHandler->dispacherController, MotorCommandsLoop_2_Position); + } + else if (M_TYPE_MOVELEFT == MotorType || M_TYPE_MOVERIGHT == MotorType) + { + ptMotorCtrl->fHandler->dispacherController->Add_Dispatcher_List(ptMotorCtrl->fHandler->dispacherController, MotorCommandsLoop); + } + + return ptMotorCtrl; +} + +void MotorInit(void) +{ + g_leftmotor = MotorControllerInit(1, M_TYPE_MOVELEFT, 0x707, &FD_CAN_1_Handler); + g_rightmotor = MotorControllerInit(2, M_TYPE_MOVERIGHT, 0x708, &FD_CAN_1_Handler); + g_swingmotor = MotorControllerInit(3, M_TYPE_SWING, 0x709, &FD_CAN_2_Handler); +} + +#endif diff --git a/Bingoo/library/CMakeLists.txt b/Bingoo/library/CMakeLists.txt index c49b03b..7d4ea94 100644 --- a/Bingoo/library/CMakeLists.txt +++ b/Bingoo/library/CMakeLists.txt @@ -178,7 +178,11 @@ else() if(MY_BUILD_STATIC) add_library(${TARGET_NAME} STATIC ${MODULE_SRCS}) else() - add_library(${TARGET_NAME} SHARED ${MODULE_SRCS}) + if(A_BUILD_MAIN_AS_STATIC_LIB) + add_library(${TARGET_NAME} OBJECT ${MODULE_SRCS}) + else() + add_library(${TARGET_NAME} SHARED ${MODULE_SRCS}) + endif() endif() if(CMAKE_BUILD_TYPE STREQUAL "Debug") diff --git a/Bingoo/main/CMakeLists.txt b/Bingoo/main/CMakeLists.txt index 4b98222..a12b9e7 100644 --- a/Bingoo/main/CMakeLists.txt +++ b/Bingoo/main/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.10) -option(A_BUILD_MAIN_AS_STATIC_LIB "Build as a static library instead of an executable" OFF) - get_filename_component(TARGET_NAME ${CMAKE_CURRENT_SOURCE_DIR} NAME) +option(A_BUILD_MAIN_AS_STATIC_LIB "Build as a static library instead of an executable" OFF) + set(MAIN_SRCS main.c ) diff --git a/Bingoo/motor/CMakeLists.txt b/Bingoo/motor/CMakeLists.txt index 5f3c0e1..deab195 100644 --- a/Bingoo/motor/CMakeLists.txt +++ b/Bingoo/motor/CMakeLists.txt @@ -14,8 +14,8 @@ else() message(WARNING "[motor] Dependency 'base' not found.") endif() -if(TARGET common) - target_link_libraries(motor PUBLIC common) +if(TARGET CANmotor) + target_link_libraries(motor PUBLIC CANmotor) else() - message(WARNING "[motor] Dependency 'common' not found.") + message(WARNING "[motor] Dependency 'CANmotor' not found.") endif() \ No newline at end of file diff --git a/Bingoo/motor/include/motor.h b/Bingoo/motor/include/motor.h deleted file mode 100644 index 54bf76f..0000000 --- a/Bingoo/motor/include/motor.h +++ /dev/null @@ -1,35 +0,0 @@ -/* - * motor.h - * - * Created on: 2026年4月20日 - * Author: Lizongdi - */ -#ifdef BUILD_MOTOR -#ifndef FSM_INC_MOTOR_H_ -#define FSM_INC_MOTOR_H_ - -#include "bsp_FDCAN.h" - -typedef enum -{ - M_TYPE_MOVELEFT, - M_TYPE_MOVERIGHT, - M_TYPE_SWING -}EMotorType; - -typedef struct -{ - int32_t MotorID; - EMotorType MotorType; - int32_t HeartbeatID; - FDCANHandler *fHandler; -}TMotorCtrl; - -typedef struct -{ - void (*MotorFuncInit)(TMotorCtrl *ptMotorCtrl); -}TMotorInterface; - -void MotorInit(void); -#endif -#endif /* FSM_INC_MOTOR_H_ */ diff --git a/Bingoo/motor/include/msp_TTMotor_ZQ.h b/Bingoo/motor/include/msp_TTMotor_ZQ.h index 55c2f6c..5cebc5d 100644 --- a/Bingoo/motor/include/msp_TTMotor_ZQ.h +++ b/Bingoo/motor/include/msp_TTMotor_ZQ.h @@ -66,6 +66,11 @@ extern "C"{ /*==============================================* * constants or macros define * *----------------------------------------------*/ +#define Move_Base_Speed_Count_m_Min 201.7*7.64//天太电机1m/min + +#define Swing_Speed_Deg_Sencond_motor 201.7//HJ32-121 +#define middle_position_motor -944334 +#define TT_One_Deg_Count_motor 11014///32768*121/360(减速比121)=11014 /*==============================================* @@ -85,8 +90,6 @@ extern void Configure_Asynchronous_Mode(TMotorCtrl *ptMotorCtrl, int32_t Node_Nu extern void Consumer_Or_microcontroller_Heartbeat(TMotorCtrl *ptMotorCtrl, int32_t WaitTime); extern void Driver_ReadError(TMotorCtrl *ptMotorCtrl, int32_t WaitTime); extern void Enable_NMT(TMotorCtrl *ptMotorCtrl, int32_t WaitTime); -extern void MotorMoveCAN2ParamInit(void); -extern void MotorMoveParamInit(void); extern void Position_Immediately_Setting(TMotorCtrl *ptMotorCtrl, int32_t Deri_Position, int32_t Deri_Speed, int32_t WaitTime); extern void Position_Lag_Setting(TMotorCtrl *ptMotorCtrl, int32_t Deri_Position, int32_t Deri_Speed, int32_t WaitTime); extern void Position_Lay_Setting(TMotorCtrl *ptMotorCtrl, int32_t Deri_Position,int32_t Deri_Speed, int32_t WaitTime); @@ -97,7 +100,7 @@ extern void Postion_Velcocity_Stop(TMotorCtrl *ptMotorCtrl, int32_t WaitTime); extern void SetMotorTargetPosition(TMotorCtrl *ptMotorCtrl, int32_t TargetPosition, int32_t WaitTime); extern void Set_Current_Positon_Zero(TMotorCtrl *ptMotorCtrl, int32_t WaitTime); extern void SpeedModeSetup(TMotorCtrl *ptMotorCtrl, 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_Analytic_Fun(TMotorCtrl *ptMotorCtrl, char *buffer); extern void TT_Request_Current(TMotorCtrl *ptMotorCtrl, int32_t WaitTime); extern void TT_Request_Fault(TMotorCtrl *ptMotorCtrl, int32_t WaitTime); extern void TT_Request_Position(TMotorCtrl *ptMotorCtrl, int32_t WaitTime); diff --git a/Bingoo/motor/motor.c b/Bingoo/motor/motor.c deleted file mode 100644 index 6a70d4e..0000000 --- a/Bingoo/motor/motor.c +++ /dev/null @@ -1,198 +0,0 @@ -/* - * motor.c - * - * Created on: 2026年4月20日 - * Author: Lizongdi - */ -#ifdef BUILD_MOTOR - -#include "bsp_FDCAN.h" -#include "motor.h" -#include "BHBF_ROBOT.h" -#include "msp_TTMotor_ZQ.h" - -TMotorCtrl *g_leftmotor; -TMotorCtrl *g_rightmotor; -TMotorCtrl *g_swingmotor; - -char a[10]; -int32_t fluat_flag=0; - -extern TT_MotorParameters *TT_Motor[7]; - -static void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length) -{ - - memcpy(a,buffer,8); - fluat_flag = canID - 0x580; - switch (fluat_flag) - { - - case 1: - { - HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID1_LeftMotor", 1); - TT_Analytic_Fun(1, buffer); - } - break; - case 2: - { - HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID2_RightMotor", 1); - TT_Analytic_Fun(2, buffer); - } - break; - case 3: - { - HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "ZQ_CAN_ID3_SwingMotor", 1); - TT_Analytic_Fun(3, buffer); - } - break; - } - -} - -static void MotorCommandsLoop() -{ - static char TT_Motor_Need_To_Activate = 1; -// -// if(start_time<15000) -// { -// HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID1_LeftMotor", 1); -// HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID2_RightMotor", 1); -// } - static int Heartbeat_Flag_01; - MotorMoveParamInit(); - - if (TT_Motor_Need_To_Activate == 1) - { - ActivateMotor(g_leftmotor, 6000); - ActivateMotor(g_rightmotor, 2000); - ActivateMotor(g_leftmotor, 2000); - ActivateMotor(g_rightmotor, 2000); - ActivateMotor(g_leftmotor, 2000); - ActivateMotor(g_rightmotor, 2000); - ActivateMotor(g_leftmotor, 2000); - ActivateMotor(g_rightmotor, 2000); - - Enable_NMT(g_leftmotor,1000); - Enable_NMT(g_rightmotor,1000); - - Configure_Asynchronous_Mode(g_leftmotor, 7,600); - Consumer_Or_microcontroller_Heartbeat(g_leftmotor, 4); - Configure_Asynchronous_Mode(g_rightmotor, 8,600); - Consumer_Or_microcontroller_Heartbeat(g_leftmotor, 4); - SpeedModeSetup(g_leftmotor, 6, 500, 500, 0); - Consumer_Or_microcontroller_Heartbeat(g_rightmotor, 4); - SpeedModeSetup(g_rightmotor, 6, 500, 500, 0); - Consumer_Or_microcontroller_Heartbeat(g_leftmotor, 4); - Consumer_Or_microcontroller_Heartbeat(g_rightmotor, 4); - TT_Motor_Need_To_Activate = 0; - } - else - { - TT_Request_Position(g_leftmotor, 6); - TT_Request_Velocity(g_leftmotor, 6); - TT_Request_Current(g_leftmotor, 6); - TT_Request_Fault(g_leftmotor, 6); - - TT_Request_Position(g_rightmotor, 6); - TT_Request_Velocity(g_rightmotor, 6); - TT_Request_Current(g_rightmotor, 6); - TT_Request_Fault(g_rightmotor, 6); - - TT_SpeedMode_Set_TargetSpeed(g_leftmotor, 6, GV.LeftMotor.Target_Velcity); - TT_SpeedMode_Set_TargetSpeed(g_rightmotor, 6, GV.RightMotor.Target_Velcity); - } - - Heartbeat_Flag_01++; - if(Heartbeat_Flag_01%7==0) - { - Consumer_Or_microcontroller_Heartbeat(g_leftmotor, 4); - Consumer_Or_microcontroller_Heartbeat(g_rightmotor, 4); - Heartbeat_Flag_01=0; - } -} - -static void MotorCommandsLoop_2_Position() -{ - static char TT_Motor_Need_To_Activate_1 = 1; -// if(start_time<15000) -// { -// HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "ZQ_CAN_ID3_SwingMotor", 1); -// } - MotorMoveCAN2ParamInit(); - - if (TT_Motor_Need_To_Activate_1 == 1) - { - ActivateMotor(g_swingmotor, 6000); - ActivateMotor(g_swingmotor, 2000); - ActivateMotor(g_swingmotor, 2000); - ActivateMotor(g_swingmotor, 2000); - ActivateMotor(g_swingmotor, 2000); - Postion_Velcocity_Run_SetParameter(g_swingmotor, 0, 0, 500, 500, 100); - TT_Motor_Need_To_Activate_1 = 0; - } - else - { - TT_Request_Position(g_swingmotor, 10); - TT_Request_Fault(g_swingmotor, 10); - - switch(GV.SwingMotor.Position_immediately1_Lag2) - { - case 1: - Position_Immediately_Setting(g_swingmotor, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,30); - break; - case 2: - Position_Lag_Setting(g_swingmotor, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,30); - break; - } - } -} - -static TMotorCtrl *MotorControllerInit(int32_t MotorID, EMotorType MotorType, - int32_t HeartbeatID, FDCANHandler *fHandler) -{ - TMotorCtrl *ptMotorCtrl = malloc(sizeof(TMotorCtrl)); - if (M_TYPE_MOVELEFT == MotorType) - { - TT_Motor[M_TYPE_MOVELEFT] = &GV.LeftMotor; - HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID1_LeftMotor", 1, ComError_ZQ_CAN_ID1_LeftMotor); - } - else if (M_TYPE_MOVERIGHT == MotorType) - { - TT_Motor[M_TYPE_MOVERIGHT] = &GV.RightMotor; - HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID2_RightMotor", 1, ComError_ZQ_CAN_ID2_RightMotor); - } - else if (M_TYPE_SWING == MotorType) - { - TT_Motor[M_TYPE_SWING] = &GV.SwingMotor; - HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID3_SwingMotor", 1, ComError_ZQ_CAN_ID3_SwingMotor); - } - TT_Motor[MotorType]->MotorID = MotorID; - ptMotorCtrl->MotorID = MotorID; - ptMotorCtrl->MotorType = MotorType; - ptMotorCtrl->HeartbeatID = HeartbeatID; - ptMotorCtrl->fHandler = fHandler; - - ptMotorCtrl->fHandler->CAN_Decode = Roughening_MotorDecodeCAN; - - if (M_TYPE_SWING == MotorType) - { - ptMotorCtrl->fHandler->dispacherController->Add_Dispatcher_List(ptMotorCtrl->fHandler->dispacherController, MotorCommandsLoop_2_Position); - } - else - { - ptMotorCtrl->fHandler->dispacherController->Add_Dispatcher_List(ptMotorCtrl->fHandler->dispacherController, MotorCommandsLoop); - } - ptMotorCtrl->fHandler->dispacherController->DispacherCallTime = 2; - - return ptMotorCtrl; -} - -void MotorInit(void) -{ - g_leftmotor = MotorControllerInit(1, M_TYPE_MOVELEFT, 0x707, &FD_CAN_1_Handler); - g_rightmotor = MotorControllerInit(2, M_TYPE_MOVERIGHT, 0x708, &FD_CAN_1_Handler); - g_swingmotor = MotorControllerInit(3, M_TYPE_SWING, 0x709, &FD_CAN_2_Handler); -} - -#endif diff --git a/Bingoo/motor/motor_adapter.c b/Bingoo/motor/motor_adapter.c new file mode 100644 index 0000000..ec02d20 --- /dev/null +++ b/Bingoo/motor/motor_adapter.c @@ -0,0 +1,151 @@ +/****************************************************************************** + + 版权所有 (C), 2018-2099, Radkil + + ****************************************************************************** + 文 件 名 : motor_adapter.c + 版 本 号 : 初稿 + 作 者 : radkil + 生成日期 : 2026年4月21日 + 最近修改 : + 功能描述 : 电机适配层 + + 修改历史 : + 1.日 期 : 2026年4月21日 + 作 者 : radkil + 修改内容 : 创建文件 + +******************************************************************************/ +#ifdef BUILD_MOTOR +#include "motor.h" +#include "msp_TTMotor_ZQ.h" + +/*----------------------------------------------* + * 外部变量说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 外部函数原型说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 内部函数原型说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 全局变量 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 模块级变量 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 常量定义 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 宏定义 * + *----------------------------------------------*/ + +static void WheelSendHeartbeat(TMotorCtrl *ptLeftMotorCtrl, TMotorCtrl *ptRightMotorCtrl) +{ + static int Heartbeat_Flag_01; + Heartbeat_Flag_01++; + if(Heartbeat_Flag_01%7==0) + { + Consumer_Or_microcontroller_Heartbeat(ptLeftMotorCtrl, 4); + Consumer_Or_microcontroller_Heartbeat(ptRightMotorCtrl, 4); + Heartbeat_Flag_01=0; + } +} + +void MotorGetState(TMotorCtrl *ptMotorCtrl, char *buffer) +{ + TT_Analytic_Fun(ptMotorCtrl, buffer); +} + +void WheelFuncInit(TMotorCtrl *ptLeftMotorCtrl, TMotorCtrl *ptRightMotorCtrl) +{ + ActivateMotor(ptLeftMotorCtrl, 6000); + ActivateMotor(ptRightMotorCtrl, 2000); + ActivateMotor(ptLeftMotorCtrl, 2000); + ActivateMotor(ptRightMotorCtrl, 2000); + ActivateMotor(ptLeftMotorCtrl, 2000); + ActivateMotor(ptRightMotorCtrl, 2000); + ActivateMotor(ptLeftMotorCtrl, 2000); + ActivateMotor(ptRightMotorCtrl, 2000); + + Enable_NMT(ptLeftMotorCtrl,1000); + Enable_NMT(ptRightMotorCtrl,1000); + + Configure_Asynchronous_Mode(ptLeftMotorCtrl, 7,600); + Consumer_Or_microcontroller_Heartbeat(ptLeftMotorCtrl, 4); + Configure_Asynchronous_Mode(ptRightMotorCtrl, 8,600); + Consumer_Or_microcontroller_Heartbeat(ptLeftMotorCtrl, 4); + SpeedModeSetup(ptLeftMotorCtrl, 6, 500, 500, 0); + Consumer_Or_microcontroller_Heartbeat(ptRightMotorCtrl, 4); + SpeedModeSetup(ptRightMotorCtrl, 6, 500, 500, 0); + Consumer_Or_microcontroller_Heartbeat(ptLeftMotorCtrl, 4); + Consumer_Or_microcontroller_Heartbeat(ptRightMotorCtrl, 4); + + WheelSendHeartbeat(ptLeftMotorCtrl, ptRightMotorCtrl); +} + +void WheelFuncProc(TMotorCtrl *ptLeftMotorCtrl, TMotorCtrl *ptRightMotorCtrl) +{ + TT_Request_Position(ptLeftMotorCtrl, 6); + TT_Request_Velocity(ptLeftMotorCtrl, 6); + TT_Request_Current(ptLeftMotorCtrl, 6); + TT_Request_Fault(ptLeftMotorCtrl, 6); + + TT_Request_Position(ptRightMotorCtrl, 6); + TT_Request_Velocity(ptRightMotorCtrl, 6); + TT_Request_Current(ptRightMotorCtrl, 6); + TT_Request_Fault(ptRightMotorCtrl, 6); + + TT_SpeedMode_Set_TargetSpeed(ptLeftMotorCtrl, 6, GV.LeftMotor.Target_Velcity); + TT_SpeedMode_Set_TargetSpeed(ptRightMotorCtrl, 6, GV.RightMotor.Target_Velcity); + + WheelSendHeartbeat(ptLeftMotorCtrl, ptRightMotorCtrl); +} + +void WheelSetVelcity(TMotorCtrl *ptMotorCtrl, float fSpeed_M_min) +{ + ptMotorCtrl->pMotorParam->Target_Velcity = (int32_t)(fSpeed_M_min * Move_Base_Speed_Count_m_Min); +} + +void SwingFuncInit(TMotorCtrl *ptMotorCtrl) +{ + ActivateMotor(ptMotorCtrl, 6000); + ActivateMotor(ptMotorCtrl, 2000); + ActivateMotor(ptMotorCtrl, 2000); + ActivateMotor(ptMotorCtrl, 2000); + ActivateMotor(ptMotorCtrl, 2000); + Postion_Velcocity_Run_SetParameter(ptMotorCtrl, 0, 0, 500, 500, 100); +} + +void SwingFuncProc(TMotorCtrl *ptMotorCtrl) +{ + TT_Request_Position(ptMotorCtrl, 10); + TT_Request_Fault(ptMotorCtrl, 10); + + switch(GV.SwingMotor.Position_immediately1_Lag2) + { + case 1: + Position_Immediately_Setting(ptMotorCtrl, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,30); + break; + case 2: + Position_Lag_Setting(ptMotorCtrl, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,30); + break; + } +} + +void SwingSetPosition(TMotorCtrl *ptMotorCtrl, float fPositionAngle, int32_t iVelcityDegreeS) +{ + ptMotorCtrl->pMotorParam->Tar_Position_count = (int32_t)(middle_position_motor - fPositionAngle * TT_One_Deg_Count_motor); + ptMotorCtrl->pMotorParam->Tar_Position_Velcity_RPM = iVelcityDegreeS*Swing_Speed_Deg_Sencond_motor; +} + +#endif + diff --git a/Bingoo/motor/msp_TTMotor_ZQ.c b/Bingoo/motor/msp_TTMotor_ZQ.c index c20f148..5c6ddcd 100644 --- a/Bingoo/motor/msp_TTMotor_ZQ.c +++ b/Bingoo/motor/msp_TTMotor_ZQ.c @@ -7,27 +7,6 @@ #ifdef BUILD_MOTOR #include "msp_TTMotor_ZQ.h" #include "BHBF_ROBOT.h" -#include "motor.h" - -#define Move_Base_Speed_Count_m_Min 201.7*7.64//天太电机1m/min - -#define Swing_Speed_Deg_Sencond_motor 201.7//HJ32-121 -#define middle_position_motor -944334 -#define TT_One_Deg_Count_motor 11014///32768*121/360(减速比121)=11014 - -TT_MotorParameters *TT_Motor[7]; - -void MotorMoveParamInit(void) -{ - GV.LeftMotor.Target_Velcity=(int)(GV.Left_Speed_M_min*Move_Base_Speed_Count_m_Min); - GV.RightMotor.Target_Velcity=(int)(GV.Right_Speed_M_min*Move_Base_Speed_Count_m_Min); -} - -void MotorMoveCAN2ParamInit(void) -{ - GV.SwingMotor.Tar_Position_count=(int32_t)(middle_position_motor-GV.Tar_Position_angle*TT_One_Deg_Count_motor); - GV.SwingMotor.Tar_Position_Velcity_RPM=GV.Tar_Position_Velcity_Degree_S*Swing_Speed_Deg_Sencond_motor; -} void ActivateMotor(TMotorCtrl *ptMotorCtrl, int32_t WaitTime) { @@ -283,7 +262,7 @@ void TT_Request_Current(TMotorCtrl *ptMotorCtrl, int32_t WaitTime) WaitTime); } -void TT_Analytic_Fun(int32_t ID_A_T_A, char *buffer) +void TT_Analytic_Fun(TMotorCtrl *ptMotorCtrl, char *buffer) { int Function_code = (int) ((int16_t) (buffer[1] | buffer[2] << 8)); @@ -291,71 +270,71 @@ void TT_Analytic_Fun(int32_t ID_A_T_A, char *buffer) { case 24672: - TT_Motor[ID_A_T_A]->Cont_Posi_Suc = 1; + ptMotorCtrl->pMotorParam->Cont_Posi_Suc = 1; break; case 24707: - TT_Motor[ID_A_T_A]->Acc_Suc = 1; + ptMotorCtrl->pMotorParam->Acc_Suc = 1; break; case 24708: - TT_Motor[ID_A_T_A]->Dec_Suc = 1; + ptMotorCtrl->pMotorParam->Dec_Suc = 1; break; case 24698: - TT_Motor[ID_A_T_A]->Target_Posi_Suc = 1; + ptMotorCtrl->pMotorParam->Target_Posi_Suc = 1; break; case 24705: - TT_Motor[ID_A_T_A]->Run_Speed_Suc = 1; + ptMotorCtrl->pMotorParam->Run_Speed_Suc = 1; break; case 24640: - TT_Motor[ID_A_T_A]->Clear_Suc = 1; + ptMotorCtrl->pMotorParam->Clear_Suc = 1; break; case 8220: - TT_Motor[ID_A_T_A]->Acc_Suc_Speed = 1; + ptMotorCtrl->pMotorParam->Acc_Suc_Speed = 1; break; case 24831: - TT_Motor[ID_A_T_A]->Suc_Speed_S = 1; + ptMotorCtrl->pMotorParam->Suc_Speed_S = 1; break; case 8221: - TT_Motor[ID_A_T_A]->Dec_Suc_Speed = 1; + ptMotorCtrl->pMotorParam->Dec_Suc_Speed = 1; break; case 24676: { - TT_Motor[ID_A_T_A]->Real_Position = (int) (buffer[4] + ptMotorCtrl->pMotorParam->Real_Position = (int) (buffer[4] | (buffer[5]) << 8 | (buffer[6]) << 16 | (buffer[7] << 24)); - if (TT_Motor[ID_A_T_A]->Start_Measuring == 1) + if (ptMotorCtrl->pMotorParam->Start_Measuring == 1) { - TT_Motor[ID_A_T_A]->Start_Measuring =0; - TT_Motor[ID_A_T_A]->Number_Of_Rounds=0; + ptMotorCtrl->pMotorParam->Start_Measuring =0; + ptMotorCtrl->pMotorParam->Number_Of_Rounds=0; - TT_Motor[ID_A_T_A]->Start_Measuring_Position=TT_Motor[ID_A_T_A]->Real_Position; - TT_Motor[ID_A_T_A]->Last_Real_Position=TT_Motor[ID_A_T_A]->Real_Position; + ptMotorCtrl->pMotorParam->Start_Measuring_Position=ptMotorCtrl->pMotorParam->Real_Position; + ptMotorCtrl->pMotorParam->Last_Real_Position=ptMotorCtrl->pMotorParam->Real_Position; } else { if (abs( - TT_Motor[ID_A_T_A]->Real_Position - - TT_Motor[ID_A_T_A]->Last_Real_Position) + ptMotorCtrl->pMotorParam->Real_Position + - ptMotorCtrl->pMotorParam->Last_Real_Position) >= 30000) { - if (TT_Motor[ID_A_T_A]->Real_Position > 0) + if (ptMotorCtrl->pMotorParam->Real_Position > 0) { - TT_Motor[ID_A_T_A]->Number_Of_Rounds -= 1; + ptMotorCtrl->pMotorParam->Number_Of_Rounds -= 1; } else { - TT_Motor[ID_A_T_A]->Number_Of_Rounds += 1; + ptMotorCtrl->pMotorParam->Number_Of_Rounds += 1; } } } - TT_Motor[ID_A_T_A]->Last_Real_Position = - TT_Motor[ID_A_T_A]->Real_Position; + ptMotorCtrl->pMotorParam->Last_Real_Position = + ptMotorCtrl->pMotorParam->Real_Position; double CircleLength = 3.14 * 280/1000 ; //pi*d //m - double _tempDeltCounts = TT_Motor[ID_A_T_A]->Real_Position - - TT_Motor[ID_A_T_A]->Start_Measuring_Position; - TT_Motor[ID_A_T_A]->Real_Disatnce = (_tempDeltCounts/ 32768/101 - + (double) TT_Motor[ID_A_T_A]->Number_Of_Rounds + double _tempDeltCounts = ptMotorCtrl->pMotorParam->Real_Position + - ptMotorCtrl->pMotorParam->Start_Measuring_Position; + ptMotorCtrl->pMotorParam->Real_Disatnce = (_tempDeltCounts/ 32768/101 + + (double) ptMotorCtrl->pMotorParam->Number_Of_Rounds ) * CircleLength; @@ -363,16 +342,16 @@ void TT_Analytic_Fun(int32_t ID_A_T_A, char *buffer) break; case 24684: - TT_Motor[ID_A_T_A]->Real_Velcity = (int) (buffer[4] + ptMotorCtrl->pMotorParam->Real_Velcity = (int) (buffer[4] | (buffer[5]) << 8 | (buffer[6]) << 16 | (buffer[7] << 24)); break; case 24696: - TT_Motor[ID_A_T_A]->Real_Current = (int) (buffer[4] + ptMotorCtrl->pMotorParam->Real_Current = (int) (buffer[4] | (buffer[5]) << 8 | (buffer[6]) << 16 | (buffer[7] << 24)); //0.1A break; case 12298://0x3001 - TT_Motor[ID_A_T_A]->TT_Motor_Fault = (int) (buffer[4] + ptMotorCtrl->pMotorParam->TT_Motor_Fault = (int) (buffer[4] | (buffer[5]) << 8 | (buffer[6]) << 16 | (buffer[7] << 24)); break; @@ -380,7 +359,7 @@ void TT_Analytic_Fun(int32_t ID_A_T_A, char *buffer) default: if ((buffer[0] == (0XAA)) | (buffer[1] == (0XBB))) { - TT_Motor[ID_A_T_A]->Act_Suc = 1; + ptMotorCtrl->pMotorParam->Act_Suc = 1; } break; } diff --git a/CMakePresets.json b/CMakePresets.json index e6ce463..80933b3 100644 --- a/CMakePresets.json +++ b/CMakePresets.json @@ -22,7 +22,8 @@ "cacheVariables": { "BUILD_CONTROLLER": "ON", "BUILD_PAINT": "ON", - "BUILD_MOTOR": "ON" + "BUILD_MOTOR": "ON", + "BUILD_CANMOTOR": "ON" } }, { @@ -33,6 +34,7 @@ "cacheVariables": { "BUILD_CTRL2": "ON", "BUILD_MOTORTT": "ON", + "BUILD_CANMOTOR": "ON", "BUILD_PAINT": "ON", "BUILD_SWING": "ON" } diff --git a/Core/Src/main.c b/Core/Src/main.c index acbf43a..2a6e815 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -39,7 +39,6 @@ #include #include "motor.h" #include "bsp_FDCAN.h" -#include "msp_TTMotor_ZQ.h" /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/