From 371443751f49ec80c0b717ba0739d13b230cb0be Mon Sep 17 00:00:00 2001 From: Lizongdi <1210855344@qq.com> Date: Mon, 20 Apr 2026 17:04:11 +0800 Subject: [PATCH] =?UTF-8?q?=E5=B0=86FDCAN=E6=94=BE=E5=88=B0common=E6=A8=A1?= =?UTF-8?q?=E5=9D=97=EF=BC=8Cmotor=E4=B8=8ETTmotor=E6=94=BE=E5=88=B0motor?= =?UTF-8?q?=E6=A8=A1=E5=9D=97=E5=B9=B6=E7=BC=96=E8=AF=91=E9=80=9A=E8=BF=87?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Bingoo/CMakeLists.txt | 3 +- Bingoo/base/BHBF_ROBOT.c | 1 - Bingoo/base/bsp_UART.c | 1 + Bingoo/base/include/bsp_fsm.h | 13 + Bingoo/base/include/bsp_include.h | 2 +- Bingoo/base/include/com.pb.h | 57 ++ Bingoo/base/include/motor.h | 21 - Bingoo/base/include/motors.h | 13 +- Bingoo/base/include/msp_TTMotor_ZQ.h | 105 ---- Bingoo/base/motor.c | 279 --------- Bingoo/base/msp_TTMotor_ZQ.c | 428 -------------- Bingoo/common/CMakeLists.txt | 15 + Bingoo/{base => common}/bsp_FDCAN.c | 25 +- Bingoo/{base => common}/include/bsp_FDCAN.h | 6 +- Bingoo/controller/controller.c | 2 + Bingoo/ctrl2/controller.c | 2 + Bingoo/motor/CMakeLists.txt | 6 + Bingoo/motor/include/motor.h | 35 ++ Bingoo/motor/include/msp_TTMotor_ZQ.h | 114 ++++ Bingoo/motor/motor.c | 244 ++++++-- Bingoo/motor/msp_TTMotor_ZQ.c | 389 ++++++++++++ Bingoo/motorTT/motor.c | 2 + Bingoo/paint/paint.c | 2 + Bingoo/swing/swing.c | 2 + Bingoo/tools/cmake_deps.py | 617 -------------------- Core/Src/main.c | 7 +- Core/Src/stm32h7xx_it.c | 53 +- 27 files changed, 892 insertions(+), 1552 deletions(-) create mode 100644 Bingoo/base/include/bsp_fsm.h create mode 100644 Bingoo/base/include/com.pb.h delete mode 100644 Bingoo/base/include/motor.h delete mode 100644 Bingoo/base/include/msp_TTMotor_ZQ.h delete mode 100644 Bingoo/base/motor.c delete mode 100644 Bingoo/base/msp_TTMotor_ZQ.c create mode 100644 Bingoo/common/CMakeLists.txt rename Bingoo/{base => common}/bsp_FDCAN.c (95%) rename Bingoo/{base => common}/include/bsp_FDCAN.h (98%) create mode 100644 Bingoo/motor/include/motor.h create mode 100644 Bingoo/motor/include/msp_TTMotor_ZQ.h create mode 100644 Bingoo/motor/msp_TTMotor_ZQ.c delete mode 100644 Bingoo/tools/cmake_deps.py diff --git a/Bingoo/CMakeLists.txt b/Bingoo/CMakeLists.txt index b19f7e7..e89e87b 100644 --- a/Bingoo/CMakeLists.txt +++ b/Bingoo/CMakeLists.txt @@ -58,7 +58,8 @@ function(add_module MOD_NAME MOD_PATH) endif() endfunction() -add_module(base base ON ON) +add_module(base base ON OFF) +add_module(common common ON OFF) add_module(controller controller OFF ON) add_module(motor motor OFF ON) add_module(ctrl2 ctrl2 OFF ON) diff --git a/Bingoo/base/BHBF_ROBOT.c b/Bingoo/base/BHBF_ROBOT.c index 068e22b..ae9606f 100644 --- a/Bingoo/base/BHBF_ROBOT.c +++ b/Bingoo/base/BHBF_ROBOT.c @@ -9,7 +9,6 @@ #include -#include "bsp_FDCAN.h" diff --git a/Bingoo/base/bsp_UART.c b/Bingoo/base/bsp_UART.c index d634b95..a481e21 100644 --- a/Bingoo/base/bsp_UART.c +++ b/Bingoo/base/bsp_UART.c @@ -1,4 +1,5 @@ #include "bsp_UART.h" +#include "bsp_com_helper.h" #include "main.h" #include diff --git a/Bingoo/base/include/bsp_fsm.h b/Bingoo/base/include/bsp_fsm.h new file mode 100644 index 0000000..e7a93a3 --- /dev/null +++ b/Bingoo/base/include/bsp_fsm.h @@ -0,0 +1,13 @@ +/* + * bsp_fsm.h + * + * Created on: Oct 18, 2024 + * Author: akeguo + */ + +#ifndef INC_BSP_FSM_H_ +#define INC_BSP_FSM_H_ + + + +#endif /* INC_BSP_FSM_H_ */ diff --git a/Bingoo/base/include/bsp_include.h b/Bingoo/base/include/bsp_include.h index a8d831f..85894ab 100644 --- a/Bingoo/base/include/bsp_include.h +++ b/Bingoo/base/include/bsp_include.h @@ -18,7 +18,7 @@ #include "bsp_EEPROM.h" #include "bsp_GPIO.h" -#include "bsp_FDCAN.h" +//#include "bsp_FDCAN.h" #include "bsp_TIMER.h" #include "bsp_PV.pb.h" diff --git a/Bingoo/base/include/com.pb.h b/Bingoo/base/include/com.pb.h new file mode 100644 index 0000000..0904289 --- /dev/null +++ b/Bingoo/base/include/com.pb.h @@ -0,0 +1,57 @@ +/* Automatically generated nanopb header */ +/* Generated by nanopb-0.4.8 */ + +#ifndef PB_COM_PB_H_INCLUDED +#define PB_COM_PB_H_INCLUDED +#include "BSP/pb.h" + +#if PB_PROTO_HEADER_VERSION != 40 +#error Regenerate this file with the current version of nanopb generator. +#endif + +/* Struct definitions */ +typedef struct _UplaodtoSerial { + int32_t Speed_1; + int32_t Speed_2; + int32_t Speed_3; + int32_t Speed_4; +} UplaodtoSerial; + + +#ifdef __cplusplus +extern "C" { +#endif + +/* Initializer values for message structs */ +#define UplaodtoSerial_init_default {0, 0, 0, 0} +#define UplaodtoSerial_init_zero {0, 0, 0, 0} + +/* Field tags (for use in manual encoding/decoding) */ +#define UplaodtoSerial_Speed_1_tag 1 +#define UplaodtoSerial_Speed_2_tag 2 +#define UplaodtoSerial_Speed_3_tag 3 +#define UplaodtoSerial_Speed_4_tag 4 + +/* Struct field encoding specification for nanopb */ +#define UplaodtoSerial_FIELDLIST(X, a) \ +X(a, STATIC, SINGULAR, INT32, Speed_1, 1) \ +X(a, STATIC, SINGULAR, INT32, Speed_2, 2) \ +X(a, STATIC, SINGULAR, INT32, Speed_3, 3) \ +X(a, STATIC, SINGULAR, INT32, Speed_4, 4) +#define UplaodtoSerial_CALLBACK NULL +#define UplaodtoSerial_DEFAULT NULL + +extern const pb_msgdesc_t UplaodtoSerial_msg; + +/* Defines for backwards compatibility with code written before nanopb-0.4.0 */ +#define UplaodtoSerial_fields &UplaodtoSerial_msg + +/* Maximum encoded size of messages (where known) */ +#define COM_PB_H_MAX_SIZE UplaodtoSerial_size +#define UplaodtoSerial_size 44 + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif diff --git a/Bingoo/base/include/motor.h b/Bingoo/base/include/motor.h deleted file mode 100644 index 4fc052f..0000000 --- a/Bingoo/base/include/motor.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * motor.h - * - * Created on: 2026年1月29日 - * Author: bm673 - */ - -#ifndef FSM_INC_MOTOR_H_ -#define FSM_INC_MOTOR_H_ - - - - - - - - - - - -#endif /* FSM_INC_MOTOR_H_ */ diff --git a/Bingoo/base/include/motors.h b/Bingoo/base/include/motors.h index bcfabf8..593e3a5 100644 --- a/Bingoo/base/include/motors.h +++ b/Bingoo/base/include/motors.h @@ -9,18 +9,7 @@ #define SRC_LS_CAN_MOTOR_FRAMES_H_ #include "BHBF_ROBOT.h" -#include "bsp_FDCAN.h" +//#include "bsp_FDCAN.h" #include "bsp_Error_Detect.h" - - -extern char LS_Motor_Need_To_Activate; -void LS_Motor_Controller_intialize(FDCANHandler *Handler); -void Motor_Controller_intialize_CAN2(FDCANHandler *Handler); - -void LS_Motor_DOSet(uint8_t state); -int32_t speed_m_per_min_to_pulse_s(int32_t speed_m_per_min); - -int32_t RunTime_DistanceCm_SpeedE_2MPMin(); - #endif /* SRC_LS_CAN_MOTOR_FRAMES_H_ */ diff --git a/Bingoo/base/include/msp_TTMotor_ZQ.h b/Bingoo/base/include/msp_TTMotor_ZQ.h deleted file mode 100644 index d98f8e9..0000000 --- a/Bingoo/base/include/msp_TTMotor_ZQ.h +++ /dev/null @@ -1,105 +0,0 @@ -/* - * 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_ */ diff --git a/Bingoo/base/motor.c b/Bingoo/base/motor.c deleted file mode 100644 index 1838c7f..0000000 --- a/Bingoo/base/motor.c +++ /dev/null @@ -1,279 +0,0 @@ -/* - * motor.c - * - * Created on: 2026年1月29日 - * Author: bm673 - */ -#include "motors.h" -#include "msp_TTMotor_ZQ.h" - - -FDCANHandler *Roughening_Motor_Controller; -FDCANHandler *Roughening_Motor_Controller_CAN2; -DispacherController *Roughening_DispacherController; -DispacherController *Roughening_DispacherController_CAN2; - - -char TT_Motor_Need_To_Activate = 1; -char TT_Motor_Need_To_Activate_1 = 1; - -float Move_Base_Speed_Count_m_Min=201.7*7.64;//天太电机1m/min -float Swing_Speed_Deg_Sencond_motor=201.7;//HJ32-121 -int middle_position_motor=-944334; -#define TT_One_Deg_Count_motor 11014///32768*121/360(减速比121)=11014 - -//MotorParameters *TT_Motor[7]; - - -void MotorCommandsLoop(); - -void MotorCommandsLoop_2(); -void MotorCommandsLoop_2_Position(); -void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length); -void Roughening_MotorDecodeCAN2(uint32_t canID, uint8_t *buffer, uint32_t length); -extern int32_t start_time; - -void Motor_Controller_intialize(FDCANHandler *Handler) -{ - //初始化 - Roughening_Motor_Controller = Handler; - Roughening_Motor_Controller->CAN_Decode = Roughening_MotorDecodeCAN; - HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID1_LeftMotor", 1, ComError_ZQ_CAN_ID1_LeftMotor); - HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID2_RightMotor", 1, ComError_ZQ_CAN_ID2_RightMotor); - Roughening_DispacherController = Handler->dispacherController; - Roughening_DispacherController->DispacherCallTime = 2; - Roughening_DispacherController->Add_Dispatcher_List(Roughening_DispacherController, MotorCommandsLoop); - - - - - //电机绑定 - TT_Motor[1]=&GV.LeftMotor; - TT_Motor[2]=&GV.RightMotor; - - TT_Motor[1]->MotorID=1; - TT_Motor[2]->MotorID=2; -} - - -void Motor_Controller_intialize_CAN2(FDCANHandler *Handler) -{ - //初始化 - Roughening_Motor_Controller_CAN2 = Handler; - Roughening_Motor_Controller_CAN2->CAN_Decode = Roughening_MotorDecodeCAN2; - HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID3_SwingMotor", 1, ComError_ZQ_CAN_ID3_SwingMotor); - Roughening_DispacherController_CAN2 = Handler->dispacherController; - Roughening_DispacherController_CAN2->DispacherCallTime = 2; - Roughening_DispacherController_CAN2->Add_Dispatcher_List(Roughening_DispacherController_CAN2, MotorCommandsLoop_2_Position); - - - TT_Motor[3]=&GV.SwingMotor; - TT_Motor[3]->MotorID=3; -} - - -void MotorCommandsLoop() -{ -// -// if(start_time<15000) -// { -// HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID1_LeftMotor", 1); -// HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID2_RightMotor", 1); -// } - 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); - - static int Heartbeat_Flag_01; - if (TT_Motor_Need_To_Activate == 1) - { - ActivateMotor(1, Roughening_Motor_Controller, 6000); - ActivateMotor(2, Roughening_Motor_Controller, 2000); - ActivateMotor(1, Roughening_Motor_Controller, 2000); - ActivateMotor(2, Roughening_Motor_Controller, 2000); - ActivateMotor(1, Roughening_Motor_Controller, 2000); - ActivateMotor(2, Roughening_Motor_Controller, 2000); - ActivateMotor(1, Roughening_Motor_Controller, 2000); - ActivateMotor(2, Roughening_Motor_Controller, 2000); - - - - - Enable_NMT(000,Roughening_Motor_Controller,01,1000); - Enable_NMT(000,Roughening_Motor_Controller,02,1000); - - Configure_Asynchronous_Mode(1, Roughening_Motor_Controller, 7,600); - Consumer_Or_microcontroller_Heartbeat(0x707, Roughening_Motor_Controller, 4); - Configure_Asynchronous_Mode(2, Roughening_Motor_Controller, 8,600); - Consumer_Or_microcontroller_Heartbeat(0x707, Roughening_Motor_Controller, 4); - SpeedModeSetup(1, Roughening_Motor_Controller, 6, 500, 500, 0); - Consumer_Or_microcontroller_Heartbeat(0x708, Roughening_Motor_Controller, 4); - SpeedModeSetup(2, Roughening_Motor_Controller, 6, 500, 500, 0); - Consumer_Or_microcontroller_Heartbeat(0x707, Roughening_Motor_Controller, 4); - Consumer_Or_microcontroller_Heartbeat(0x708, Roughening_Motor_Controller, 4); - TT_Motor_Need_To_Activate = 0; - } - else - { - 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_SpeedMode_Set_TargetSpeed(1, Roughening_Motor_Controller, 6,GV.LeftMotor.Target_Velcity); - TT_SpeedMode_Set_TargetSpeed(2, Roughening_Motor_Controller, 6,GV.RightMotor.Target_Velcity); - } - - Heartbeat_Flag_01++; - if(Heartbeat_Flag_01%7==0) - { - Consumer_Or_microcontroller_Heartbeat(0x707, Roughening_Motor_Controller, 4); - Consumer_Or_microcontroller_Heartbeat(0x708, Roughening_Motor_Controller, 4); - Heartbeat_Flag_01=0; - } -} - - - -void MotorCommandsLoop_2_Position() -{ -// if(start_time<15000) -// { -// HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "ZQ_CAN_ID3_SwingMotor", 1); -// } - 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; - - if (TT_Motor_Need_To_Activate_1 == 1) - { - ActivateMotor(3, Roughening_Motor_Controller_CAN2, 6000); - ActivateMotor(3, Roughening_Motor_Controller_CAN2, 2000); - ActivateMotor(3, Roughening_Motor_Controller_CAN2, 2000); - ActivateMotor(3, Roughening_Motor_Controller_CAN2, 2000); - ActivateMotor(3, Roughening_Motor_Controller_CAN2, 2000); - Postion_Velcocity_Run_SetParameter( 3, 0, 0, 500, 500, Roughening_Motor_Controller_CAN2, 100 ); - TT_Motor_Need_To_Activate_1 = 0; - } - else - { - for (int i = 3; i <4 ; i++)//前两个电机 - { - TT_Request_Position(3, Roughening_Motor_Controller_CAN2, 10); - - TT_Request_Fault(3, Roughening_Motor_Controller_CAN2, 10); - } - - switch(GV.SwingMotor.Position_immediately1_Lag2) - { - case 1: - Position_Immediately_Setting(3,Roughening_Motor_Controller_CAN2, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,30); - break; - case 2: - Position_Lag_Setting(3,Roughening_Motor_Controller_CAN2, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,30); - break; - } - } -} - - - -void MotorCommandsLoop_2() -{ - static int Heartbeat_Flag; - if (TT_Motor_Need_To_Activate_1 == 1) - { - - ActivateMotor(3, Roughening_Motor_Controller_CAN2, 6000); - ActivateMotor(3, Roughening_Motor_Controller_CAN2, 2000); - ActivateMotor(3, Roughening_Motor_Controller_CAN2, 2000); - Enable_NMT(000,Roughening_Motor_Controller_CAN2,03,1000); - Configure_Asynchronous_Mode(3, Roughening_Motor_Controller_CAN2, 9,1000); - SpeedModeSetup(3, Roughening_Motor_Controller_CAN2, 12, 500, 500, 0); - TT_Motor_Need_To_Activate_1 = 0; - } - else - { - for (int i = 3; i <4 ; i++)//前两个电机 - { - TT_Request_Position(3, Roughening_Motor_Controller_CAN2, 6); - TT_Request_Current(3, Roughening_Motor_Controller_CAN2, 6); - TT_Request_Fault(3, Roughening_Motor_Controller_CAN2, 6); - } -// TT_SpeedMode_Set_TargetSpeed(3, Roughening_Motor_Controller_CAN2, 6,GV.SwingMotor.Target_Velcity); - Heartbeat_Flag++; - if(Heartbeat_Flag%15==0) - { - Consumer_Or_microcontroller_Heartbeat(0x709, Roughening_Motor_Controller_CAN2, 4); - Heartbeat_Flag=0; - } - } -} - - - -char a[10]; -int32_t fluat_flag=0; - -void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length) -{ - - memcpy(a,buffer,8); - switch (canID - 0x580) - { - - 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: - { - fluat_flag=1; - } - break; - - - } - -} - - -void Roughening_MotorDecodeCAN2(uint32_t canID, uint8_t *buffer, uint32_t length) -{ - - memcpy(a,buffer,8); - switch (canID - 0x580) - { - - case 1: - { - fluat_flag=2; - } - break; - case 2: - { - fluat_flag=3; - } - break; - case 3: - { - HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "ZQ_CAN_ID3_SwingMotor", 1); - TT_Analytic_Fun(3, buffer); - } - break; - - - } - -} - - - diff --git a/Bingoo/base/msp_TTMotor_ZQ.c b/Bingoo/base/msp_TTMotor_ZQ.c deleted file mode 100644 index d863e0e..0000000 --- a/Bingoo/base/msp_TTMotor_ZQ.c +++ /dev/null @@ -1,428 +0,0 @@ -/* - * msp_TTMotor_ZQ.c - * - * Created on: Oct 10, 2024 - * Author: akeguo - */ -#include "msp_TTMotor_ZQ.h" -#include "BHBF_ROBOT.h" - - -TT_MotorParameters *TT_Motor[7]; - - -void ActivateMotor(int32_t MotorID, FDCANHandler *ZQ_Motor_Controller, - int32_t WaitTime) -{ - ZQ_Motor_Controller->Tx_Buf[0] = 0x10; - ZQ_Motor_Controller->Tx_Buf[1] = 0x10; - ZQ_Motor_Controller->AddCANSendList(ZQ_Motor_Controller, MotorID, 2, - ZQ_Motor_Controller->Tx_Buf, WaitTime, NULL); //wait for 5 seconds to send - - //CANSendMessageSDO_ADD_To_SendList(MotorID, 0x23, 0x6083, 0x00, AccTime,ZQ_Motor_Controller,WaitTime); -} -void Enable_NMT(int32_t MotorID, FDCANHandler *ZQ_Motor_Controller, int32_t Node_Number, int32_t WaitTime) -{ - ZQ_Motor_Controller->Tx_Buf[0] = 0x01; - ZQ_Motor_Controller->Tx_Buf[1] = Node_Number; - ZQ_Motor_Controller->AddCANSendList(ZQ_Motor_Controller, MotorID, 2, ZQ_Motor_Controller->Tx_Buf, WaitTime, NULL); //wait for 5 seconds to send -} - -void Configure_Asynchronous_Mode(int32_t MotorID, FDCANHandler *ZQ_Motor_Controller, int32_t Node_Number, int32_t WaitTime) -{ - ZQ_Motor_Controller->Tx_Buf[0] = 0x23; - ZQ_Motor_Controller->Tx_Buf[1] = 0x16; - ZQ_Motor_Controller->Tx_Buf[2] = 0x10; - ZQ_Motor_Controller->Tx_Buf[3] = 0x01; - ZQ_Motor_Controller->Tx_Buf[4] = 0xe8; - ZQ_Motor_Controller->Tx_Buf[5] = 0x03; - ZQ_Motor_Controller->Tx_Buf[6] = Node_Number; - ZQ_Motor_Controller->Tx_Buf[7] = 0x00; - ZQ_Motor_Controller->AddCANSendList(ZQ_Motor_Controller, 0x600 + MotorID, 8, ZQ_Motor_Controller->Tx_Buf, WaitTime, NULL); -} - -void Consumer_Or_microcontroller_Heartbeat(int32_t MotorID, FDCANHandler *ZQ_Motor_Controller, int32_t WaitTime) -{ - ZQ_Motor_Controller->Tx_Buf[0] = 0x05; - ZQ_Motor_Controller->AddCANSendList(ZQ_Motor_Controller, MotorID, 1, ZQ_Motor_Controller->Tx_Buf, WaitTime, NULL); //wait for 5 seconds to send -} - - -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) -{ - //copy the corresponsiding data to the send Array - ZQ_Motor_Controller->Tx_Buf[0] = Function; - memcpy(&ZQ_Motor_Controller->Tx_Buf[1], &ControlWord, 2); - ZQ_Motor_Controller->Tx_Buf[3] = subWord; - memcpy(&ZQ_Motor_Controller->Tx_Buf[4], &ControlWordValue, 4); - //send 8 bytes data - - ZQ_Motor_Controller->AddCANSendList(ZQ_Motor_Controller, 0x600 + MotorID, 8, - ZQ_Motor_Controller->Tx_Buf, WaitTime, NULL); -} - -//位置速度模式 -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) -{ - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2f, 0x6060, 0x00, 1, - ZQ_Motor_Controller, WaitTime); // 1：设置操作模式，向索引0x6060:00写入0x01 - if (TargetSpeed >= 3500) //the highest is 3500 - { - TargetSpeed = 3500; - } - if (TargetSpeed < 0) - { - TargetSpeed = 0; - } - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x23, 0x6083, 0x00, AccTime, - ZQ_Motor_Controller, WaitTime); //2：设置加速时间，向索引0x6083:00写入4字节数值 - // Thread.Sleep(2); - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x23, 0x6084, 0x00, DecTime, - ZQ_Motor_Controller, WaitTime); //3：设置减速时间，向索引0x6084:00写入4字节数值 - //Thread.Sleep(2); - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x23, 0x607A, 0x00, - TargetPosition, ZQ_Motor_Controller, WaitTime); //4：设置目标位置，向索引 0x607A:00写入4字节数值 - // Thread.Sleep(2); - - //23：写4字节数据； - //816000:索引号为0x6081,子索引号为0x00； - //E80300 00:设置运行速度为1000（单位0.1rpm） - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x23, 0x6081, 0x00, - TargetSpeed * 10, ZQ_Motor_Controller, WaitTime); //5：设置运行速度，向索引 0x6081:00写入4字节数值 - - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x80, - ZQ_Motor_Controller, WaitTime); ///6:清除异常，向控制字0x6040:00写入0x80 - - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x06, - ZQ_Motor_Controller, WaitTime); //7：伺服准备，向控制字 0x6040:00写入0x06 - - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x07, - ZQ_Motor_Controller, WaitTime); //8：伺服等待使能，向控制字0x6040:00写入0x07 - - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x2f, - ZQ_Motor_Controller, WaitTime); //9：伺服使能，向控制字0x6040:00写入0x0F - - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x3f, - ZQ_Motor_Controller, WaitTime); //10：伺服启动,向控制字 0x6040:00写入0x1F ///： -} - -void Postion_Velcocity_Set_Position(int32_t MotorID, int32_t TargetPosition, - FDCANHandler *ZQ_Motor_Controller, int32_t WaitTime) -{ -// 3、立即更新时，重复执行步骤4、步骤9(控制字(6040)写入2F)和步骤10(控制字(6040) -// 写入3F)，驱动器还没到达当前目标位置时，支持接收新的目标位置，最终到达的目标位 -// 置是：新的目标位置。 - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x23, 0x607a, 0x00, - TargetPosition, ZQ_Motor_Controller, WaitTime); //设置目标位置 - //Thread.Sleep(2); - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x2f, - ZQ_Motor_Controller, WaitTime); ///： - - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x3f, - ZQ_Motor_Controller, WaitTime); ///： -} - -void Driver_ReadError(int32_t MotorID, FDCANHandler *ZQ_Motor_Controller, - int32_t WaitTime) -{ - //CANSendMessageSDO_ADD_To_SendList(CAN_ID, new byte[8] { 0x40, 0x0A, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00 });//40 0A 30 00 00 00 00 00 - - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x40, 0x300a, 0x00, 0, - ZQ_Motor_Controller, WaitTime); //40 0A 30 00 00 00 00 00 -} -void SetMotorTargetPosition(int32_t MotorID, int32_t TargetPosition, - FDCANHandler *ZQ_Motor_Controller, int32_t WaitTime) -{ - Postion_Velcocity_Set_Position(MotorID, TargetPosition, ZQ_Motor_Controller, - WaitTime); -} - -void Postion_Velcocity_Stop(int32_t MotorID, FDCANHandler *ZQ_Motor_Controller, - int32_t WaitTime) -{ - - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6085, 0x00, 200, - ZQ_Motor_Controller, WaitTime); // 6085 的减速时间100ms - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x605a, 0x00, 6, - ZQ_Motor_Controller, WaitTime); // 0x06：按索引 6085 的减速时间进行减速，停下时电机 轴处于锁轴状态。 - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x02, - ZQ_Motor_Controller, WaitTime); //快速停机 -} - -//void SetCurrentPositionZero(int32_t MotorID,FDCANHandler *ZQ_Motor_Controller,int32_t WaitTime) -//{ -// CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2f, 0x6098, 0x00, 0x23,ZQ_Motor_Controller,WaitTime); //1：设置定位的方式,向索 引0x6098:00写入0X23 -// CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2f, 0x6060, 0x00, 6,ZQ_Motor_Controller,WaitTime); //2：设置操作模式，向索引0x6060:00写入0x06 -// CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x80,ZQ_Motor_Controller,WaitTime); //3:清除异常，向控制字 0x6040:00写入0x80 -// CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x06,ZQ_Motor_Controller,WaitTime); //4：伺服准备，向控制字 0x6040:00写入0x06 -// CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x07,ZQ_Motor_Controller,WaitTime); //5：伺服等待使能，向控制字0x6040:00写入0x07 -// CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x0F,ZQ_Motor_Controller,WaitTime); //6：伺服使能，向控制字0x6040:00写入0x0F -// CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2f, 0x6040, 0x00, 0x1F,ZQ_Motor_Controller,WaitTime); //7：启动原点定位,向控制字0x6040:00写入0x1F -//} - -void SpeedModeSetup(int32_t MotorID, FDCANHandler *ZQ_Motor_Controller, - int32_t WaitTime, int32_t Acc, int32_t Dec, int32_t TargetVelocity) //设定速度模式，并更改相关速度 -{ - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x02, - ZQ_Motor_Controller, 100); - //设置操作模式，向索引0x6060：00写入0x03 - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2f, 0x6060, 0x00, 0x3, - ZQ_Motor_Controller, WaitTime); - //2:清除异常， 向控制字 0x6040:00 写入 0x80 - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x80, - ZQ_Motor_Controller, WaitTime); - //3： 伺服准备, 向控制字 0x6040:00 写入 0x06 - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x06, - ZQ_Motor_Controller, WaitTime); - //4： 伺服等待使能, 向控制字 0x6040:00 写入 0x07 - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x07, - ZQ_Motor_Controller, WaitTime); - //5： 伺服使能,向控制字0x6040:00 写入 0x0F - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x0f, - ZQ_Motor_Controller, WaitTime); - //6:设置加速时间， 向索引 0x201C:00写入4字节数值 //不关心加速时间的模式下， 加减速时间建议设置为 1500（ms） 以上 - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x23, 0x201c, 0x00, Acc, - ZQ_Motor_Controller, WaitTime); - //7： 设置减速时间， 向索引0x201D:00写入4字节数据 - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x23, 0x201d, 0x00, Dec, - ZQ_Motor_Controller, WaitTime); - //8： 设定目标速度值， 向索引 0x60FF:00 写入 4 字节数值（有符号） - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x23, 0x60ff, 0x00, - TargetVelocity, ZQ_Motor_Controller, WaitTime); ///6:清除异常，向控制字 - - //修改步骤8 可以实现速度的改变 - // 23:写 4 字节数据；FF 60 00:索引号为 0x60FF， - // 子索引号为 0x00;E8 03 00 00： - // 设置目标速度 1000（单位 0.1rpm） -} -void TT_SpeedMode_Set_TargetSpeed(uint32_t MotorID, - FDCANHandler *ZQ_Motor_Controller, int32_t WaitTime, - int32_t TargetSpeed) -{ - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x23, 0x60ff, 0x00, TargetSpeed, - ZQ_Motor_Controller, WaitTime); -} - -#if 0 -void Swing_Motor_Set_Target_Position() -{ - CANSendMessageSDO(SwingMotorID, 0x40, 0x6041, 0x00, 0x00); ///：是否到达目的位置 -} - -void Swing_Motor_Read_ReachedEnd() -{ - CANSendMessageSDO(SwingMotorID, 0x40, 0x6041, 0x00, 0x00); ///：是否到达目的位置 - -} -#endif - -void Set_Current_Positon_Zero(uint8_t MotorID, - FDCANHandler *ZQ_Motor_Controller, int32_t WaitTime) // Home 设置当前位置为零点 -{ - -// CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x2f,ZQ_Motor_Controller,WaitTime); ///： -// CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x3f,ZQ_Motor_Controller,WaitTime); ///： - - //CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x2008, 0x00, 0x01,ZQ_Motor_Controller,1000); - //CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x02,ZQ_Motor_Controller,100); //1. 停止执行 - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2f, 0x6098, 0x00, 0x23, - ZQ_Motor_Controller, WaitTime); //1. 设置定位的方式,向索引0x6098:00写入0X23 - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2f, 0x6060, 0x00, 0x06, - ZQ_Motor_Controller, WaitTime); //2：设置操作模式，向索引0x6060:00写入0x06 - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x80, - ZQ_Motor_Controller, WaitTime); //3:清除异常，向控制字0x6040:00写入0x80 - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x06, - ZQ_Motor_Controller, WaitTime); //4.伺服准备，向控制字0x6040:00写入0x06 - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x07, - ZQ_Motor_Controller, WaitTime); //5. 伺服等待使能，向控制字0x6040:00写入0x07 - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x0f, - ZQ_Motor_Controller, WaitTime); //6. 伺服使能 向控制字 0x6040:00写入0x0F - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2f, 0x6040, 0x00, 0x1f, - ZQ_Motor_Controller, WaitTime); //7. ：启动原点定位,向控制字0x6040:00写入0x1F - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x40, 0x6041, 0x00, 0x0, - ZQ_Motor_Controller, 100); //8. ：启动原点定位,向控制字0x6040:00写入0x1F - -} - -void TT_Request_Position(uint32_t Motor_ID_1, FDCANHandler *ZQ_Motor_Controller, - int32_t WaitTime) // Home 设置当前位置为零点 -{ - CANSendMessageSDO_ADD_To_SendList(Motor_ID_1, 0x40, 0x6064, 0x00, 0x00, - ZQ_Motor_Controller, WaitTime); //用户实际速度反馈(0.1rpm) -} - - -void Position_Immediately_Setting(uint8_t MotorID, FDCANHandler *ZQ_Motor_Controller,int32_t Deri_Position,int32_t Deri_Speed, int32_t WaitTime) // Home 设置当前位置为零点 -{ - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x23, 0x607A, 0x00, Deri_Position, ZQ_Motor_Controller, WaitTime); //设置位置 - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x23, 0x6081, 0x00, Deri_Speed, ZQ_Motor_Controller, WaitTime); //设置速度 - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2B, 0x6040, 0x00, 0x2f, ZQ_Motor_Controller, WaitTime); - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2B, 0x6040, 0x00, 0x3f, ZQ_Motor_Controller, WaitTime); -} - -void Position_Lag_Setting(uint8_t MotorID, FDCANHandler *ZQ_Motor_Controller,int32_t Deri_Position,int32_t Deri_Speed, int32_t WaitTime) // Home 设置当前位置为零点 -{ - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x23, 0x607A, 0x00, Deri_Position, ZQ_Motor_Controller, WaitTime); //设置位置 - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x23, 0x6081, 0x00, Deri_Speed, ZQ_Motor_Controller, WaitTime); //设置速度 - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2B, 0x6040, 0x00, 0x0f, ZQ_Motor_Controller, WaitTime); - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2B, 0x6040, 0x00, 0x1f, ZQ_Motor_Controller, WaitTime); -} - - - - -void Position_Lay_Setting(uint8_t MotorID, FDCANHandler *ZQ_Motor_Controller,int32_t Deri_Position,int32_t Deri_Speed, int32_t WaitTime) // Home 设置当前位置为零点 -{ - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2f, 0x6098, 0x00, 0x23, - ZQ_Motor_Controller, WaitTime); //1. 设置定位的方式,向索引0x6098:00写入0X23 - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2f, 0x6060, 0x00, 0x06, - ZQ_Motor_Controller, WaitTime); //2：设置操作模式，向索引0x6060:00写入0x06 - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x80, - ZQ_Motor_Controller, WaitTime); //3:清除异常，向控制字0x6040:00写入0x80 - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x06, - ZQ_Motor_Controller, WaitTime); //4.伺服准备，向控制字0x6040:00写入0x06 - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x07, - ZQ_Motor_Controller, WaitTime); //5. 伺服等待使能，向控制字0x6040:00写入0x07 - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2b, 0x6040, 0x00, 0x0f, - ZQ_Motor_Controller, WaitTime); //6. 伺服使能 向控制字 0x6040:00写入0x0F - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x2f, 0x6040, 0x00, 0x1f, - ZQ_Motor_Controller, WaitTime); //7. ：启动原点定位,向控制字0x6040:00写入0x1F - CANSendMessageSDO_ADD_To_SendList(MotorID, 0x40, 0x6041, 0x00, 0x0, - ZQ_Motor_Controller, 100); //8. ：启动原点定位,向控制字0x6040:00写入0x1F -} - - - - - -void TT_Request_Velocity(uint32_t Motor_ID_1, FDCANHandler *ZQ_Motor_Controller, - int32_t WaitTime) -{ - CANSendMessageSDO_ADD_To_SendList(Motor_ID_1, 0x40, 0x606c, 0x00, 0x00, - ZQ_Motor_Controller, WaitTime); //用户实际速度反馈(0.1rpm) - -} -// -// -void TT_Request_Fault(uint32_t Motor_ID_1, FDCANHandler *ZQ_Motor_Controller, - int32_t WaitTime) -{ - CANSendMessageSDO_ADD_To_SendList(Motor_ID_1, 0x40, 0x300A, 0x00, 0x00, - ZQ_Motor_Controller, WaitTime); - -} - -void TT_Request_Current(uint32_t Motor_ID_1, FDCANHandler *ZQ_Motor_Controller, - int32_t WaitTime) -{ - CANSendMessageSDO_ADD_To_SendList(Motor_ID_1, 0x40, 0x6078, 0x00, 0x00, - ZQ_Motor_Controller, WaitTime); -} - -void TT_Analytic_Fun(int32_t ID_A_T_A, char *buffer) -{ - - int Function_code = (int) ((int16_t) (buffer[1] | buffer[2] << 8)); - switch (Function_code) - { - - case 24672: - TT_Motor[ID_A_T_A]->Cont_Posi_Suc = 1; - break; - case 24707: - TT_Motor[ID_A_T_A]->Acc_Suc = 1; - break; - case 24708: - TT_Motor[ID_A_T_A]->Dec_Suc = 1; - break; - case 24698: - TT_Motor[ID_A_T_A]->Target_Posi_Suc = 1; - break; - case 24705: - TT_Motor[ID_A_T_A]->Run_Speed_Suc = 1; - break; - case 24640: - TT_Motor[ID_A_T_A]->Clear_Suc = 1; - break; - case 8220: - TT_Motor[ID_A_T_A]->Acc_Suc_Speed = 1; - break; - case 24831: - TT_Motor[ID_A_T_A]->Suc_Speed_S = 1; - break; - case 8221: - TT_Motor[ID_A_T_A]->Dec_Suc_Speed = 1; - break; - case 24676: - { - TT_Motor[ID_A_T_A]->Real_Position = (int) (buffer[4] - | (buffer[5]) << 8 | (buffer[6]) << 16 | (buffer[7] << 24)); - if (TT_Motor[ID_A_T_A]->Start_Measuring == 1) - { - TT_Motor[ID_A_T_A]->Start_Measuring =0; - TT_Motor[ID_A_T_A]->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; - } - else - { - if (abs( - TT_Motor[ID_A_T_A]->Real_Position - - TT_Motor[ID_A_T_A]->Last_Real_Position) - >= 30000) - { - if (TT_Motor[ID_A_T_A]->Real_Position > 0) - { - TT_Motor[ID_A_T_A]->Number_Of_Rounds -= 1; - } - else - { - TT_Motor[ID_A_T_A]->Number_Of_Rounds += 1; - } - } - } - - TT_Motor[ID_A_T_A]->Last_Real_Position = - TT_Motor[ID_A_T_A]->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 - ) - * CircleLength; - - } - - break; - case 24684: - TT_Motor[ID_A_T_A]->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] - | (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] - | (buffer[5]) << 8 | (buffer[6]) << 16 | (buffer[7] << 24)); - - break; - - default: - if ((buffer[0] == (0XAA)) | (buffer[1] == (0XBB))) - { - TT_Motor[ID_A_T_A]->Act_Suc = 1; - } - break; - } -} - diff --git a/Bingoo/common/CMakeLists.txt b/Bingoo/common/CMakeLists.txt new file mode 100644 index 0000000..b746c1d --- /dev/null +++ b/Bingoo/common/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.10) + +set(COMMON_CMAKE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/../library/CMakeLists.txt") + +if(EXISTS ${COMMON_CMAKE_PATH}) + include(${COMMON_CMAKE_PATH}) +else() + message(FATAL_ERROR "Cannot find common build logic at ${COMMON_CMAKE_PATH}") +endif() + +if(TARGET base) + target_link_libraries(common PUBLIC base) +else() + message(WARNING "[common] Dependency 'base' not found.") +endif() \ No newline at end of file diff --git a/Bingoo/base/bsp_FDCAN.c b/Bingoo/common/bsp_FDCAN.c similarity index 95% rename from Bingoo/base/bsp_FDCAN.c rename to Bingoo/common/bsp_FDCAN.c index 5e3843e..762fbc3 100644 --- a/Bingoo/base/bsp_FDCAN.c +++ b/Bingoo/common/bsp_FDCAN.c @@ -4,6 +4,7 @@ * Created on: Oct 26, 2023 * Author: shiya */ +#ifdef BUILD_COMMON #include "bsp_FDCAN.h" #include "main.h" #include @@ -13,13 +14,7 @@ FDCANHandler FD_CAN_1_Handler; FDCANHandler FD_CAN_2_Handler; -FDCAN_RxHeaderTypeDef CAN_RX_HDR; - -uint8_t CAN_Buf[8]; -uint8_t CAN_Buf_2[8]; - -int32_t CAN_ID; -int32_t CAN_ID_2; +static DispacherController *can_dispacherController; uint8_t GF_BSP_FDCAN_Init(void) //can初始化 { @@ -67,12 +62,15 @@ void HAL_FDCAN_ErrorStatusCallback(FDCAN_HandleTypeDef *hfdcan, void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs)//can接收 { + FDCAN_RxHeaderTypeDef CAN_RX_HDR; + uint8_t CAN_Buf[8]; + uint8_t CAN_Buf_2[8]; if (hfdcan->Instance == FDCAN1) { if (HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO0, &CAN_RX_HDR, CAN_Buf)//把数据存CAN_Buf里 == HAL_OK) { - CAN_ID = FD_CAN_1_Handler.ReceivedFrameID = + FD_CAN_1_Handler.ReceivedFrameID = (uint32_t) CAN_RX_HDR.Identifier; //ID #if NewCANSendVersion @@ -94,7 +92,7 @@ void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs) if (HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO0, &CAN_RX_HDR, CAN_Buf_2) == HAL_OK) ////把数据存CAN_Buf2里 { - CAN_ID_2 = FD_CAN_2_Handler.ReceivedFrameID = + FD_CAN_2_Handler.ReceivedFrameID = (uint32_t) CAN_RX_HDR.Identifier; #if NewCANSendVersion @@ -114,9 +112,6 @@ void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs) } } -FDCAN_TxHeaderTypeDef TXHeader1; -FDCAN_TxHeaderTypeDef TXHeader2; - void GF_BSP_CANHandler_Init(int can1_sendListPeriod, int can1_DispacherPeriod, int can2_sendListPeriod, int can2_DispacherPeriod) { @@ -158,8 +153,6 @@ void GF_BSP_CANHandler_Init_CAN(FDCANHandler *handler, } -DispacherController *can_dispacherController; -HardWareController *can_HardWareController; //2ms void GF_BSP_CAN_Timer() { @@ -244,7 +237,8 @@ void CAN_Send_Data_t(struct _FDCANHandler *fd) void CAN_Send_t(struct _FDCANHandler *fd, uint32_t FrameID, uint8_t DataLength, uint8_t *Txdata) { - + FDCAN_TxHeaderTypeDef TXHeader1; + FDCAN_TxHeaderTypeDef TXHeader2; //Function_code = Txdata[0]; if (DataLength > 8) @@ -332,3 +326,4 @@ void CANHandlerAddTxList(FDCANHandler *handler, uint32_t CAN_ID, } +#endif \ No newline at end of file diff --git a/Bingoo/base/include/bsp_FDCAN.h b/Bingoo/common/include/bsp_FDCAN.h similarity index 98% rename from Bingoo/base/include/bsp_FDCAN.h rename to Bingoo/common/include/bsp_FDCAN.h index 7796c5b..7459ce6 100644 --- a/Bingoo/base/include/bsp_FDCAN.h +++ b/Bingoo/common/include/bsp_FDCAN.h @@ -4,7 +4,7 @@ * Created on: Oct 26, 2023 * Author: shiya */ - +#ifdef BUILD_COMMON #ifndef INC_BSP_FDCAN_H_ #define INC_BSP_FDCAN_H_ #include "bsp_Error.pb.h" @@ -79,6 +79,6 @@ void DecodeMotorCAN(uint32_t canID, uint8_t *buffer, uint32_t length); extern FDCANHandler FD_CAN_1_Handler; extern FDCANHandler FD_CAN_2_Handler; -extern int32_t CAN_ID; -extern int32_t CAN_ID_2; + #endif /* INC_BSP_FDCAN_H_ */ +#endif diff --git a/Bingoo/controller/controller.c b/Bingoo/controller/controller.c index f069fb4..e7ef9e4 100644 --- a/Bingoo/controller/controller.c +++ b/Bingoo/controller/controller.c @@ -16,6 +16,7 @@ 修改内容 : 创建文件 ******************************************************************************/ +#ifdef BUILD_CONTROLLER #include "robot.h" /*----------------------------------------------* @@ -67,3 +68,4 @@ void ControllerDisplayPressure(int _iValue) printf("This is controller. display pressure[%d]!\n", _iValue); } +#endif diff --git a/Bingoo/ctrl2/controller.c b/Bingoo/ctrl2/controller.c index bcd15e1..fb92d54 100644 --- a/Bingoo/ctrl2/controller.c +++ b/Bingoo/ctrl2/controller.c @@ -16,6 +16,7 @@ 修改内容 : 创建文件 ******************************************************************************/ +#ifdef BUILD_CTRL2 #include "robot.h" /*----------------------------------------------* @@ -67,3 +68,4 @@ void ControllerDisplayPressure(int _iValue) printf("This is ctrl2. display pressure[%d]!\n", _iValue); } +#endif diff --git a/Bingoo/motor/CMakeLists.txt b/Bingoo/motor/CMakeLists.txt index b0d543d..5f3c0e1 100644 --- a/Bingoo/motor/CMakeLists.txt +++ b/Bingoo/motor/CMakeLists.txt @@ -12,4 +12,10 @@ if(TARGET base) target_link_libraries(motor PUBLIC base) else() message(WARNING "[motor] Dependency 'base' not found.") +endif() + +if(TARGET common) + target_link_libraries(motor PUBLIC common) +else() + message(WARNING "[motor] Dependency 'common' not found.") endif() \ No newline at end of file diff --git a/Bingoo/motor/include/motor.h b/Bingoo/motor/include/motor.h new file mode 100644 index 0000000..54bf76f --- /dev/null +++ b/Bingoo/motor/include/motor.h @@ -0,0 +1,35 @@ +/* + * 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 new file mode 100644 index 0000000..55c2f6c --- /dev/null +++ b/Bingoo/motor/include/msp_TTMotor_ZQ.h @@ -0,0 +1,114 @@ +/****************************************************************************** + + 版权所有 (C), 2018-2099, Radkil + + ****************************************************************************** + 文 件 名 : msp_TTMotor_ZQ.h + 版 本 号 : 初稿 + 作 者 : radkil + 生成日期 : 2026年4月20日 + 最近修改 : + 功能描述 : msp_TTMotor_ZQ.c 的头文件 + + 修改历史 : + 1.日 期 : 2026年4月20日 + 作 者 : radkil + 修改内容 : 创建文件 + +******************************************************************************/ + +/*----------------------------------------------* + * 外部变量说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 外部函数原型说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 内部函数原型说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 全局变量 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 模块级变量 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 常量定义 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 宏定义 * + *----------------------------------------------*/ +#ifdef BUILD_MOTOR +#ifndef __MSP_TTMOTOR_ZQ_H__ +#define __MSP_TTMOTOR_ZQ_H__ + + +#ifdef __cplusplus +#if __cplusplus +extern "C"{ +#endif +#endif /* __cplusplus */ + + +/*==============================================* + * include header files * + *----------------------------------------------*/ +#include "motor.h" + + + +/*==============================================* + * constants or macros define * + *----------------------------------------------*/ + + +/*==============================================* + * project-wide global variables * + *----------------------------------------------*/ + + + +/*==============================================* + * routines' or functions' implementations * + *----------------------------------------------*/ + +extern void ActivateMotor(TMotorCtrl *ptMotorCtrl, int32_t WaitTime); +extern void CANSendMessageSDO_ADD_To_SendList(TMotorCtrl *ptMotorCtrl, uint8_t Function, + uint16_t ControlWord, uint8_t subWord, int32_t ControlWordValue, int32_t WaitTime); +extern void Configure_Asynchronous_Mode(TMotorCtrl *ptMotorCtrl, int32_t Node_Number, int32_t WaitTime); +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); +extern void Postion_Velcocity_Run_SetParameter(TMotorCtrl *ptMotorCtrl, int32_t TargetPosition, + int32_t TargetSpeed, int32_t AccTime, int32_t DecTime, int32_t WaitTime); +extern void Postion_Velcocity_Set_Position(TMotorCtrl *ptMotorCtrl, int32_t TargetPosition, int32_t WaitTime); +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_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); +extern void TT_Request_Velocity(TMotorCtrl *ptMotorCtrl, int32_t WaitTime); +extern void TT_SpeedMode_Set_TargetSpeed(TMotorCtrl *ptMotorCtrl, int32_t WaitTime, int32_t TargetSpeed); + +#ifdef __cplusplus +#if __cplusplus +} +#endif +#endif /* __cplusplus */ + +#endif +#endif /* __MSP_TTMOTOR_ZQ_H__ */ diff --git a/Bingoo/motor/motor.c b/Bingoo/motor/motor.c index cd85f93..6a70d4e 100644 --- a/Bingoo/motor/motor.c +++ b/Bingoo/motor/motor.c @@ -1,78 +1,198 @@ -/****************************************************************************** - - 版权所有 (C), 2018-2099, Radkil - - ****************************************************************************** - 文 件 名 : motor.c - 版 本 号 : 初稿 - 作 者 : radkil - 生成日期 : 2026年4月14日 - 最近修改 : - 功能描述 : 轮电机具体实现 - - 修改历史 : - 1.日 期 : 2026年4月14日 - 作 者 : radkil - 修改内容 : 创建文件 - -******************************************************************************/ -#include "robot.h" - -/*----------------------------------------------* - * 外部变量说明 * - *----------------------------------------------*/ - -/*----------------------------------------------* - * 外部函数原型说明 * - *----------------------------------------------*/ - -/*----------------------------------------------* - * 内部函数原型说明 * - *----------------------------------------------*/ - -/*----------------------------------------------* - * 全局变量 * - *----------------------------------------------*/ - -/*----------------------------------------------* - * 模块级变量 * - *----------------------------------------------*/ - -/*----------------------------------------------* - * 常量定义 * - *----------------------------------------------*/ - -/*----------------------------------------------* - * 宏定义 * - *----------------------------------------------*/ -void WheelMotorStrong(void) -{ - // 用于强符号覆盖的锚点 -} +/* + * 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; -int WheelMotorInit(void) +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) { - printf("This is motor.\n"); - return RB_SUCCESS; + + 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; + } + } -void WheelMotorDirForward(void) +static void MotorCommandsLoop() { - printf("This is motor. forward!\n"); + 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; + } } -void WheelMotorDirBackward(void) +static void MotorCommandsLoop_2_Position() { - printf("This is motor. backward!\n"); + 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; + } + } } -void WheelMotorStop(void) +static TMotorCtrl *MotorControllerInit(int32_t MotorID, EMotorType MotorType, + int32_t HeartbeatID, FDCANHandler *fHandler) { - printf("This is motor. stop!\n"); + 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 WheelMotorSetSpeed(int _iValue) +void MotorInit(void) { - printf("This is motor. set speed[%d]!\n", _iValue); + 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/msp_TTMotor_ZQ.c b/Bingoo/motor/msp_TTMotor_ZQ.c new file mode 100644 index 0000000..c20f148 --- /dev/null +++ b/Bingoo/motor/msp_TTMotor_ZQ.c @@ -0,0 +1,389 @@ +/* + * msp_TTMotor_ZQ.c + * + * Created on: 2026年4月20日 + * Author: Lizongdi + */ +#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) +{ + ptMotorCtrl->fHandler->Tx_Buf[0] = 0x10; + ptMotorCtrl->fHandler->Tx_Buf[1] = 0x10; + ptMotorCtrl->fHandler->AddCANSendList(ptMotorCtrl->fHandler, ptMotorCtrl->MotorID, 2, + ptMotorCtrl->fHandler->Tx_Buf, WaitTime, NULL); //wait for 5 seconds to send +} + +void Enable_NMT(TMotorCtrl *ptMotorCtrl, int32_t WaitTime) +{ + ptMotorCtrl->fHandler->Tx_Buf[0] = 0x01; + ptMotorCtrl->fHandler->Tx_Buf[1] = ptMotorCtrl->MotorID; + ptMotorCtrl->fHandler->AddCANSendList(ptMotorCtrl->fHandler, 0, 2, ptMotorCtrl->fHandler->Tx_Buf, WaitTime, NULL); //wait for 5 seconds to send +} + +void Configure_Asynchronous_Mode(TMotorCtrl *ptMotorCtrl, int32_t Node_Number, int32_t WaitTime) +{ + ptMotorCtrl->fHandler->Tx_Buf[0] = 0x23; + ptMotorCtrl->fHandler->Tx_Buf[1] = 0x16; + ptMotorCtrl->fHandler->Tx_Buf[2] = 0x10; + ptMotorCtrl->fHandler->Tx_Buf[3] = 0x01; + ptMotorCtrl->fHandler->Tx_Buf[4] = 0xe8; + ptMotorCtrl->fHandler->Tx_Buf[5] = 0x03; + ptMotorCtrl->fHandler->Tx_Buf[6] = Node_Number; + ptMotorCtrl->fHandler->Tx_Buf[7] = 0x00; + ptMotorCtrl->fHandler->AddCANSendList(ptMotorCtrl->fHandler, 0x600 + ptMotorCtrl->MotorID, 8, ptMotorCtrl->fHandler->Tx_Buf, WaitTime, NULL); +} + +void Consumer_Or_microcontroller_Heartbeat(TMotorCtrl *ptMotorCtrl, int32_t WaitTime) +{ + ptMotorCtrl->fHandler->Tx_Buf[0] = 0x05; + ptMotorCtrl->fHandler->AddCANSendList(ptMotorCtrl->fHandler, ptMotorCtrl->HeartbeatID, 1, ptMotorCtrl->fHandler->Tx_Buf, WaitTime, NULL); //wait for 5 seconds to send +} + +void CANSendMessageSDO_ADD_To_SendList(TMotorCtrl *ptMotorCtrl, uint8_t Function, + uint16_t ControlWord, uint8_t subWord, int32_t ControlWordValue, int32_t WaitTime) +{ + //copy the corresponsiding data to the send Array + ptMotorCtrl->fHandler->Tx_Buf[0] = Function; + memcpy(&ptMotorCtrl->fHandler->Tx_Buf[1], &ControlWord, 2); + ptMotorCtrl->fHandler->Tx_Buf[3] = subWord; + memcpy(&ptMotorCtrl->fHandler->Tx_Buf[4], &ControlWordValue, 4); + //send 8 bytes data + + ptMotorCtrl->fHandler->AddCANSendList(ptMotorCtrl->fHandler, 0x600 + ptMotorCtrl->MotorID, 8, + ptMotorCtrl->fHandler->Tx_Buf, WaitTime, NULL); +} + +//位置速度模式 +void Postion_Velcocity_Run_SetParameter(TMotorCtrl *ptMotorCtrl, int32_t TargetPosition, + int32_t TargetSpeed, int32_t AccTime, int32_t DecTime, int32_t WaitTime) +{ + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2f, 0x6060, 0x00, 1, + WaitTime); // 1：设置操作模式，向索引0x6060:00写入0x01 + if (TargetSpeed >= 3500) //the highest is 3500 + { + TargetSpeed = 3500; + } + if (TargetSpeed < 0) + { + TargetSpeed = 0; + } + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x23, 0x6083, 0x00, AccTime, + WaitTime); //2：设置加速时间，向索引0x6083:00写入4字节数值 + // Thread.Sleep(2); + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x23, 0x6084, 0x00, DecTime, + WaitTime); //3：设置减速时间，向索引0x6084:00写入4字节数值 + //Thread.Sleep(2); + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x23, 0x607A, 0x00, + TargetPosition, WaitTime); //4：设置目标位置，向索引 0x607A:00写入4字节数值 + // Thread.Sleep(2); + + //23：写4字节数据； + //816000:索引号为0x6081,子索引号为0x00； + //E80300 00:设置运行速度为1000（单位0.1rpm） + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x23, 0x6081, 0x00, + TargetSpeed * 10, WaitTime); //5：设置运行速度，向索引 0x6081:00写入4字节数值 + + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x80, + WaitTime); ///6:清除异常，向控制字0x6040:00写入0x80 + + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x06, + WaitTime); //7：伺服准备，向控制字 0x6040:00写入0x06 + + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x07, + WaitTime); //8：伺服等待使能，向控制字0x6040:00写入0x07 + + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x2f, + WaitTime); //9：伺服使能，向控制字0x6040:00写入0x0F + + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x3f, + WaitTime); //10：伺服启动,向控制字 0x6040:00写入0x1F ///： +} + +void Postion_Velcocity_Set_Position(TMotorCtrl *ptMotorCtrl, int32_t TargetPosition, int32_t WaitTime) +{ +// 3、立即更新时，重复执行步骤4、步骤9(控制字(6040)写入2F)和步骤10(控制字(6040) +// 写入3F)，驱动器还没到达当前目标位置时，支持接收新的目标位置，最终到达的目标位 +// 置是：新的目标位置。 + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x23, 0x607a, 0x00, + TargetPosition, WaitTime); //设置目标位置 + //Thread.Sleep(2); + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x2f, + WaitTime); ///： + + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x3f, + WaitTime); ///： +} + +void Driver_ReadError(TMotorCtrl *ptMotorCtrl, int32_t WaitTime) +{ + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x40, 0x300a, 0x00, 0, + WaitTime); //40 0A 30 00 00 00 00 00 +} +void SetMotorTargetPosition(TMotorCtrl *ptMotorCtrl, int32_t TargetPosition, int32_t WaitTime) +{ + Postion_Velcocity_Set_Position(ptMotorCtrl, TargetPosition, WaitTime); +} + +void Postion_Velcocity_Stop(TMotorCtrl *ptMotorCtrl, int32_t WaitTime) +{ + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6085, 0x00, 200, + WaitTime); // 6085 的减速时间100ms + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x605a, 0x00, 6, + WaitTime); // 0x06：按索引 6085 的减速时间进行减速，停下时电机 轴处于锁轴状态。 + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x02, + WaitTime); //快速停机 +} + +void SpeedModeSetup(TMotorCtrl *ptMotorCtrl, int32_t WaitTime, int32_t Acc, int32_t Dec, int32_t TargetVelocity) //设定速度模式，并更改相关速度 +{ + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x02, + 100); + //设置操作模式，向索引0x6060：00写入0x03 + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2f, 0x6060, 0x00, 0x3, + WaitTime); + //2:清除异常， 向控制字 0x6040:00 写入 0x80 + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x80, + WaitTime); + //3： 伺服准备, 向控制字 0x6040:00 写入 0x06 + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x06, + WaitTime); + //4： 伺服等待使能, 向控制字 0x6040:00 写入 0x07 + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x07, + WaitTime); + //5： 伺服使能,向控制字0x6040:00 写入 0x0F + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x0f, + WaitTime); + //6:设置加速时间， 向索引 0x201C:00写入4字节数值 //不关心加速时间的模式下， 加减速时间建议设置为 1500（ms） 以上 + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x23, 0x201c, 0x00, Acc, + WaitTime); + //7： 设置减速时间， 向索引0x201D:00写入4字节数据 + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x23, 0x201d, 0x00, Dec, + WaitTime); + //8： 设定目标速度值， 向索引 0x60FF:00 写入 4 字节数值（有符号） + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x23, 0x60ff, 0x00, + TargetVelocity, WaitTime); ///6:清除异常，向控制字 + + //修改步骤8 可以实现速度的改变 + // 23:写 4 字节数据；FF 60 00:索引号为 0x60FF， + // 子索引号为 0x00;E8 03 00 00： + // 设置目标速度 1000（单位 0.1rpm） +} +void TT_SpeedMode_Set_TargetSpeed(TMotorCtrl *ptMotorCtrl, int32_t WaitTime, int32_t TargetSpeed) +{ + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x23, 0x60ff, 0x00, TargetSpeed, + WaitTime); +} + +void Set_Current_Positon_Zero(TMotorCtrl *ptMotorCtrl, int32_t WaitTime) // Home 设置当前位置为零点 +{ + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2f, 0x6098, 0x00, 0x23, + WaitTime); //1. 设置定位的方式,向索引0x6098:00写入0X23 + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2f, 0x6060, 0x00, 0x06, + WaitTime); //2：设置操作模式，向索引0x6060:00写入0x06 + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x80, + WaitTime); //3:清除异常，向控制字0x6040:00写入0x80 + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x06, + WaitTime); //4.伺服准备，向控制字0x6040:00写入0x06 + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x07, + WaitTime); //5. 伺服等待使能，向控制字0x6040:00写入0x07 + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x0f, + WaitTime); //6. 伺服使能 向控制字 0x6040:00写入0x0F + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2f, 0x6040, 0x00, 0x1f, + WaitTime); //7. ：启动原点定位,向控制字0x6040:00写入0x1F + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x40, 0x6041, 0x00, 0x0, + 100); //8. ：启动原点定位,向控制字0x6040:00写入0x1F + +} + +void TT_Request_Position(TMotorCtrl *ptMotorCtrl, int32_t WaitTime) // Home 设置当前位置为零点 +{ + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x40, 0x6064, 0x00, 0x00, + WaitTime); //用户实际速度反馈(0.1rpm) +} + + +void Position_Immediately_Setting(TMotorCtrl *ptMotorCtrl, int32_t Deri_Position, int32_t Deri_Speed, int32_t WaitTime) // Home 设置当前位置为零点 +{ + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x23, 0x607A, 0x00, Deri_Position, WaitTime); //设置位置 + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x23, 0x6081, 0x00, Deri_Speed, WaitTime); //设置速度 + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2B, 0x6040, 0x00, 0x2f, WaitTime); + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2B, 0x6040, 0x00, 0x3f, WaitTime); +} + +void Position_Lag_Setting(TMotorCtrl *ptMotorCtrl, int32_t Deri_Position, int32_t Deri_Speed, int32_t WaitTime) // Home 设置当前位置为零点 +{ + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x23, 0x607A, 0x00, Deri_Position, WaitTime); //设置位置 + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x23, 0x6081, 0x00, Deri_Speed, WaitTime); //设置速度 + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2B, 0x6040, 0x00, 0x0f, WaitTime); + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2B, 0x6040, 0x00, 0x1f, WaitTime); +} + +void Position_Lay_Setting(TMotorCtrl *ptMotorCtrl, int32_t Deri_Position,int32_t Deri_Speed, int32_t WaitTime) // Home 设置当前位置为零点 +{ + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2f, 0x6098, 0x00, 0x23, + WaitTime); //1. 设置定位的方式,向索引0x6098:00写入0X23 + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2f, 0x6060, 0x00, 0x06, + WaitTime); //2：设置操作模式，向索引0x6060:00写入0x06 + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x80, + WaitTime); //3:清除异常，向控制字0x6040:00写入0x80 + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x06, + WaitTime); //4.伺服准备，向控制字0x6040:00写入0x06 + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x07, + WaitTime); //5. 伺服等待使能，向控制字0x6040:00写入0x07 + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2b, 0x6040, 0x00, 0x0f, + WaitTime); //6. 伺服使能 向控制字 0x6040:00写入0x0F + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x2f, 0x6040, 0x00, 0x1f, + WaitTime); //7. ：启动原点定位,向控制字0x6040:00写入0x1F + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x40, 0x6041, 0x00, 0x0, + 100); //8. ：启动原点定位,向控制字0x6040:00写入0x1F +} + +void TT_Request_Velocity(TMotorCtrl *ptMotorCtrl, int32_t WaitTime) +{ + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x40, 0x606c, 0x00, 0x00, + WaitTime); //用户实际速度反馈(0.1rpm) + +} +// +// +void TT_Request_Fault(TMotorCtrl *ptMotorCtrl, int32_t WaitTime) +{ + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x40, 0x300A, 0x00, 0x00, + WaitTime); + +} + +void TT_Request_Current(TMotorCtrl *ptMotorCtrl, int32_t WaitTime) +{ + CANSendMessageSDO_ADD_To_SendList(ptMotorCtrl, 0x40, 0x6078, 0x00, 0x00, + WaitTime); +} + +void TT_Analytic_Fun(int32_t ID_A_T_A, char *buffer) +{ + + int Function_code = (int) ((int16_t) (buffer[1] | buffer[2] << 8)); + switch (Function_code) + { + + case 24672: + TT_Motor[ID_A_T_A]->Cont_Posi_Suc = 1; + break; + case 24707: + TT_Motor[ID_A_T_A]->Acc_Suc = 1; + break; + case 24708: + TT_Motor[ID_A_T_A]->Dec_Suc = 1; + break; + case 24698: + TT_Motor[ID_A_T_A]->Target_Posi_Suc = 1; + break; + case 24705: + TT_Motor[ID_A_T_A]->Run_Speed_Suc = 1; + break; + case 24640: + TT_Motor[ID_A_T_A]->Clear_Suc = 1; + break; + case 8220: + TT_Motor[ID_A_T_A]->Acc_Suc_Speed = 1; + break; + case 24831: + TT_Motor[ID_A_T_A]->Suc_Speed_S = 1; + break; + case 8221: + TT_Motor[ID_A_T_A]->Dec_Suc_Speed = 1; + break; + case 24676: + { + TT_Motor[ID_A_T_A]->Real_Position = (int) (buffer[4] + | (buffer[5]) << 8 | (buffer[6]) << 16 | (buffer[7] << 24)); + if (TT_Motor[ID_A_T_A]->Start_Measuring == 1) + { + TT_Motor[ID_A_T_A]->Start_Measuring =0; + TT_Motor[ID_A_T_A]->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; + } + else + { + if (abs( + TT_Motor[ID_A_T_A]->Real_Position + - TT_Motor[ID_A_T_A]->Last_Real_Position) + >= 30000) + { + if (TT_Motor[ID_A_T_A]->Real_Position > 0) + { + TT_Motor[ID_A_T_A]->Number_Of_Rounds -= 1; + } + else + { + TT_Motor[ID_A_T_A]->Number_Of_Rounds += 1; + } + } + } + + TT_Motor[ID_A_T_A]->Last_Real_Position = + TT_Motor[ID_A_T_A]->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 + ) + * CircleLength; + + } + + break; + case 24684: + TT_Motor[ID_A_T_A]->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] + | (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] + | (buffer[5]) << 8 | (buffer[6]) << 16 | (buffer[7] << 24)); + + break; + + default: + if ((buffer[0] == (0XAA)) | (buffer[1] == (0XBB))) + { + TT_Motor[ID_A_T_A]->Act_Suc = 1; + } + break; + } +} + +#endif diff --git a/Bingoo/motorTT/motor.c b/Bingoo/motorTT/motor.c index 65a5ffd..a3815b5 100644 --- a/Bingoo/motorTT/motor.c +++ b/Bingoo/motorTT/motor.c @@ -16,6 +16,7 @@ 修改内容 : 创建文件 ******************************************************************************/ +#ifdef BUILD_MOTORTT #include "robot.h" /*----------------------------------------------* @@ -76,3 +77,4 @@ void WheelMotorSetSpeed(int _iValue) printf("This is motorTT. set speed[%d]!\n", _iValue); } +#endif diff --git a/Bingoo/paint/paint.c b/Bingoo/paint/paint.c index ba69b8e..2a5d864 100644 --- a/Bingoo/paint/paint.c +++ b/Bingoo/paint/paint.c @@ -16,6 +16,7 @@ 修改内容 : 创建文件 ******************************************************************************/ +#ifdef BUILD_PAINT #include "robot.h" /*----------------------------------------------* @@ -67,3 +68,4 @@ int PaintGetParam(void) return 0; } +#endif diff --git a/Bingoo/swing/swing.c b/Bingoo/swing/swing.c index 2c326a9..d7a9a30 100644 --- a/Bingoo/swing/swing.c +++ b/Bingoo/swing/swing.c @@ -16,6 +16,7 @@ 修改内容 : 创建文件 ******************************************************************************/ +#ifdef BUILD_SWING #include "robot.h" #include "paint_if.h" @@ -70,3 +71,4 @@ int SwingArmGetParam(void) return 0; } +#endif diff --git a/Bingoo/tools/cmake_deps.py b/Bingoo/tools/cmake_deps.py deleted file mode 100644 index 4c9b77e..0000000 --- a/Bingoo/tools/cmake_deps.py +++ /dev/null @@ -1,617 +0,0 @@ -import os -import re -import sys -import json -from collections import defaultdict -from typing import Dict, Set, List, Tuple - -# ================= 配置区域 ================= -IGNORE_DIRS = {'library', 'main', 'build', 'bin', '.git', 'cmake'} -TARGET_FILE = 'CMakeLists.txt' -OUTPUT_MD = 'Mermaid.md' -OUTPUT_HTML = 'Mermaid.html' -# =========================================== - -def find_cmake_files(root_path: str) -> Dict[str, str]: - valid_modules = {} - print(f"🔍 开始扫描目录: {root_path} ...") - - for dirpath, dirnames, filenames in os.walk(root_path): - current_dir_name = os.path.basename(dirpath) - if dirpath == root_path: - continue - if current_dir_name in IGNORE_DIRS: - continue - - cmake_count = filenames.count(TARGET_FILE) - if cmake_count == 1: - valid_modules[dirpath] = current_dir_name - elif cmake_count > 1: - print(f"⚠️ 跳过异常目录 (存在多个 {TARGET_FILE}): {dirpath}") - - print(f"✅ 找到 {len(valid_modules)} 个有效模块。\n") - return valid_modules - -def parse_dependencies(modules: Dict[str, str]) -> Dict[str, Set[str]]: - dependencies = defaultdict(set) - module_names = set(modules.values()) - - tll_pattern = re.compile(r'target_link_libraries\s*\(\s*[\w\d_]+\s*(?:PUBLIC|PRIVATE|INTERFACE)?\s*(.*?)\)', re.IGNORECASE | re.DOTALL) - lib_name_pattern = re.compile(r'[\w\d_\-\.]+') - - print("📝 正在分析依赖关系...") - - for path, mod_name in modules.items(): - file_path = os.path.join(path, TARGET_FILE) - try: - with open(file_path, 'r', encoding='utf-8', errors='ignore') as f: - content = f.read() - - matches = tll_pattern.findall(content) - for match_group in matches: - libs = lib_name_pattern.findall(match_group) - for lib in libs: - if lib in module_names and lib != mod_name: - dependencies[mod_name].add(lib) - except Exception as e: - print(f"❌ 读取文件失败 {file_path}: {e}") - - return dependencies - -def generate_mermaid_content(dependencies: Dict[str, Set[str]], all_modules: Set[str]) -> str: - graph_lines = ["graph TD"] - nodes_in_edges = set() - edges = [] - - for src, targets in dependencies.items(): - nodes_in_edges.add(src) - for tgt in targets: - nodes_in_edges.add(tgt) - edges.append(f" {src} --> {tgt}") - - if edges: - graph_lines.extend(edges) - - isolated_nodes = all_modules - nodes_in_edges - if isolated_nodes: - for node in sorted(isolated_nodes): - graph_lines.append(f" {node}") - - mermaid_graph_code = "\n".join(graph_lines) - header = "# CMake 模块依赖图\n\n以下是自动生成的模块依赖关系图：\n\n" - block_start = "```mermaid\n" - block_end = "\n```\n" - footer = f"\n> 提示：已生成离线交互图表 `{OUTPUT_HTML}`。\n" - - return header + block_start + mermaid_graph_code + block_end + footer - -def generate_offline_html(dependencies: Dict[str, Set[str]], all_modules: Set[str]): - nodes_list = sorted(list(all_modules)) - edges_list = [] - - # 统计每个节点的入度和出度，用于 Tooltip 显示 - in_degree = defaultdict(int) - out_degree = defaultdict(int) - - for src, targets in dependencies.items(): - out_degree[src] = len(targets) - for tgt in targets: - in_degree[tgt] += 1 - edges_list.append({"source": src, "target": tgt}) - - # 构建包含统计信息的节点数据 - nodes_data = [] - for name in nodes_list: - nodes_data.append({ - "id": name, - "in": in_degree.get(name, 0), - "out": out_degree.get(name, 0) - }) - - data_json = json.dumps({ - "nodes": nodes_data, - "edges": edges_list - }) - - html_template = """ - - - - - CMake 依赖图 (终极版) - - - -
- -
-
-

📦 CMake 依赖图

-

💡 点击节点查看对外依赖

- - -
- -
- - - - - -
-
- - - - -""" - final_html = html_template.replace("{data_json_placeholder}", data_json) - return final_html - -def print_tree_view(dependencies: Dict[str, Set[str]], all_modules: Set[str]): - # (保持原有逻辑不变) - print("\n" + "="*50) - print("🌳 终端依赖树状图:") - print("="*50) - if not all_modules: - print(" (无模块)") - return - children = set() - for targets in dependencies.values(): children.update(targets) - roots = all_modules - children - if not roots and all_modules: roots = {sorted(all_modules)[0]} - sorted_roots = sorted(roots) - if not sorted_roots and all_modules: sorted_roots = [sorted(all_modules)[0]] - for i, root in enumerate(sorted_roots): - is_last = (i == len(sorted_roots) - 1) - visited_in_tree = set() - def _recurse(n: str, pre: str, last: bool, visited: set): - conn = "└── " if last else "├── " - print(f"{pre}{conn}{n}") - if n in visited: - print(f"{pre}{' ' if last else '│ '} (↑ 循环引用)") - return - visited.add(n) - sub_deps = sorted(dependencies.get(n, set())) - if not sub_deps: return - new_pre = pre + (" " if last else "│ ") - for idx, sub in enumerate(sub_deps): - _recurse(sub, new_pre, idx == len(sub_deps)-1, visited) - _recurse(root, "", is_last, visited_in_tree) - print("="*50 + "\n") - -def main(): - root_dir = os.path.abspath(sys.argv[1]) if len(sys.argv) > 1 else os.getcwd() - if not os.path.isdir(root_dir): - print(f"错误：目录不存在 - {root_dir}") - sys.exit(1) - - modules = find_cmake_files(root_dir) - deps_map = parse_dependencies(modules) if modules else {} - all_mods = set(modules.values()) if modules else set() - - print_tree_view(deps_map, all_mods) - - md_content = generate_mermaid_content(deps_map, all_mods) - md_path = os.path.join(root_dir, OUTPUT_MD) - with open(md_path, 'w', encoding='utf-8') as f: - f.write(md_content) - - html_content = generate_offline_html(deps_map, all_mods) - html_path = os.path.join(root_dir, OUTPUT_HTML) - with open(html_path, 'w', encoding='utf-8') as f: - f.write(html_content) - - print(f"🎉 完成！") - print(f" 📄 文档图表：{md_path}") - print(f" 🌐 交互图表：{html_path}") - print(f" ✨ 新增功能：搜索框、悬停详情、一键导出 PNG") - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/Core/Src/main.c b/Core/Src/main.c index 2479996..acbf43a 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -31,12 +31,14 @@ /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ #include "BHBF_ROBOT.h" -#include "bsp_FDCAN.h" +//#include "bsp_FDCAN.h" #include #include "msp_ground_management.h" #include "msp_strain_gauge.h" #include +#include "motor.h" +#include "bsp_FDCAN.h" #include "msp_TTMotor_ZQ.h" /* USER CODE END Includes */ @@ -361,8 +363,7 @@ void GF_Robot_Init() GF_BSP_CANHandler_Init(can1_sendListPeriod, can1_DispacherPeriod, can2_sendListPeriod, can2_DispacherPeriod); - Motor_Controller_intialize(&FD_CAN_1_Handler); - Motor_Controller_intialize_CAN2(&FD_CAN_2_Handler); + MotorInit(); // LS_Motor_Controller_intialize(&FD_CAN_1_Handler); diff --git a/Core/Src/stm32h7xx_it.c b/Core/Src/stm32h7xx_it.c index b573731..e811b78 100644 --- a/Core/Src/stm32h7xx_it.c +++ b/Core/Src/stm32h7xx_it.c @@ -86,16 +86,58 @@ extern TIM_HandleTypeDef htim8; /** * @brief This function handles Non maskable interrupt. */ +uint8_t NMI_Trigger_Source = 0; void NMI_Handler(void) { /* USER CODE BEGIN NonMaskableInt_IRQn 0 */ /* USER CODE END NonMaskableInt_IRQn 0 */ - HAL_RCC_NMI_IRQHandler(); + //HAL_RCC_NMI_IRQHandler(); /* USER CODE BEGIN NonMaskableInt_IRQn 1 */ - while (1) - { - } + // 1. ȡں˹״̬Ĵ + uint32_t icsr = SCB->ICSR; // жϿ״̬Ĵ + uint32_t cfsr = SCB->CFSR; // ù״̬Ĵ + uint32_t hfsr = SCB->HFSR; // Ӳ fault ״̬Ĵ + uint32_t shcsr = SCB->SHCSR; // ϵͳ handler ״̬Ĵ + + // 2. ж NMI Դ + //if(SCB->ICSR & (1U << 31)) // λ31 = NMI PENDING + //{ + // -------------------------- + // 1ʱӹ CSS + // -------------------------- + if(RCC->CIFR & (1U << 8)) // RCC_CIR Ĵ CSSF λ = ʱӹ + { + // ԭⲿ HSE ʧЧ / û + NMI_Trigger_Source=1; + __NOP(); + } + + // -------------------------- + // 2ⲿ NMI Ŵ + // -------------------------- + else if( (RCC->CIFR & (1U << 8)) == 0 ) + { + // ԭⲿ NMI ŵƽ + NMI_Trigger_Source=2; + __NOP(); + } + + // -------------------------- + // 3ں/ + // -------------------------- + else + { + // ں˴󡢷Ƿַߴ + NMI_Trigger_Source=3; + __NOP(); + } + //} + + + // ѭ + while(1); + /* USER CODE END NonMaskableInt_IRQn 1 */ } @@ -106,6 +148,9 @@ void HardFault_Handler(void) { /* USER CODE BEGIN HardFault_IRQn 0 */ + + + /* USER CODE END HardFault_IRQn 0 */ while (1) {