LiZongdi
/
BingooRobot_pro
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Compare commits

base: LiZongdi:aee8b28fb7a7aba47f424a2bcf610b45ed010b96
Branches Tags
LiZongdi:master
...
compare: LiZongdi:1a12b72164b8e30f1d0a251417c12c9ca7499a50
Branches Tags
LiZongdi:master

3 Commits
aee8b28fb7 ... 1a12b72164

Author SHA1 Message Date
Lizongdi 1a12b72164 Merge branch 'master' of http://123.207.52.103:3000/LiZongdi/BingooRobot_pro
3 weeks ago
Lizongdi ac46050020 【待合】解决宏定义可见性问题
3 weeks ago
Lizongdi 371443751f 将FDCAN放到common模块，motor与TTmotor放到motor模块并编译通过
3 weeks ago
28 changed files with 894 additions and 1554 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats
  1. 5
      Bingoo/CMakeLists.txt
  2. 1
      Bingoo/base/BHBF_ROBOT.c
  3. 1
      Bingoo/base/bsp_UART.c
  4. 13
      Bingoo/base/include/bsp_fsm.h
  5. 2
      Bingoo/base/include/bsp_include.h
  6. 57
      Bingoo/base/include/com.pb.h
  7. 21
      Bingoo/base/include/motor.h
  8. 13
      Bingoo/base/include/motors.h
  9. 105
      Bingoo/base/include/msp_TTMotor_ZQ.h
  10. 279
      Bingoo/base/motor.c
  11. 428
      Bingoo/base/msp_TTMotor_ZQ.c
  12. 15
      Bingoo/common/CMakeLists.txt
  13. 25
      Bingoo/common/bsp_FDCAN.c
  14. 6
      Bingoo/common/include/bsp_FDCAN.h
  15. 2
      Bingoo/controller/controller.c
  16. 2
      Bingoo/ctrl2/controller.c
  17. 2
      Bingoo/main/CMakeLists.txt
  18. 6
      Bingoo/motor/CMakeLists.txt
  19. 35
      Bingoo/motor/include/motor.h
  20. 114
      Bingoo/motor/include/msp_TTMotor_ZQ.h
  21. 244
      Bingoo/motor/motor.c
  22. 389
      Bingoo/motor/msp_TTMotor_ZQ.c
  23. 2
      Bingoo/motorTT/motor.c
  24. 2
      Bingoo/paint/paint.c
  25. 2
      Bingoo/swing/swing.c
  26. 617
      Bingoo/tools/cmake_deps.py
  27. 7
      Core/Src/main.c
  28. 53
      Core/Src/stm32h7xx_it.c

5
Bingoo/CMakeLists.txt
View File

@ -49,16 +49,17 @@ function(add_module MOD_NAME MOD_PATH)
if(${BUILD_${MOD_UPPER}})
set(BUILD_${MOD_UPPER}_HAS_TEST ${HAS_TEST} CACHE INTERNAL "Has test support")
add_subdirectory(${MOD_PATH})
list(APPEND GLOBAL_ENABLED_MODULES ${MOD_NAME})
list(APPEND GLOBAL_ENABLED_MACROS BUILD_${MOD_UPPER})
message(STATUS "[Root] Enabled module(TestSupport: ${HAS_TEST}): ${MOD_NAME} (Default: ${DEFAULT_STATE}) -> Macro: BUILD_${MOD_UPPER}")
set(GLOBAL_ENABLED_MODULES ${GLOBAL_ENABLED_MODULES} PARENT_SCOPE)
set(GLOBAL_ENABLED_MACROS ${GLOBAL_ENABLED_MACROS} PARENT_SCOPE)
add_subdirectory(${MOD_PATH})
endif()
endfunction()
add_module(base base ON ON)
add_module(base base ON OFF)
add_module(CANmotor CANmotor ON OFF)
add_module(controller controller OFF ON)
add_module(motor motor OFF ON)
add_module(ctrl2 ctrl2 OFF ON)

1
Bingoo/base/BHBF_ROBOT.c
View File

@ -9,7 +9,6 @@
#include <string.h>
#include "bsp_FDCAN.h"

1
Bingoo/base/bsp_UART.c
View File

@ -1,4 +1,5 @@
#include "bsp_UART.h"
#include "bsp_com_helper.h"
#include "main.h"
#include <stdlib.h>

13
Bingoo/base/include/bsp_fsm.h
View File

@ -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_ */

2
Bingoo/base/include/bsp_include.h
View File

@ -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"

57
Bingoo/base/include/com.pb.h
View File

@ -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

21
Bingoo/base/include/motor.h
View File

@ -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_ */

13
Bingoo/base/include/motors.h
View File

@ -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_ */

105
Bingoo/base/include/msp_TTMotor_ZQ.h
View File

@ -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_ */

279
Bingoo/base/motor.c
View File

@ -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;
}
}

428
Bingoo/base/msp_TTMotor_ZQ.c
View File

@ -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;
}
}

15
Bingoo/common/CMakeLists.txt
View File

@ -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()

25
Bingoo/base/bsp_FDCAN.c → Bingoo/common/bsp_FDCAN.c
View File

@ -4,6 +4,7 @@
* Created on: Oct 26, 2023
* Author: shiya
*/
#ifdef BUILD_COMMON
#include "bsp_FDCAN.h"
#include "main.h"
#include <stdlib.h>
@ -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

6
Bingoo/base/include/bsp_FDCAN.h → Bingoo/common/include/bsp_FDCAN.h
View File

@ -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

2
Bingoo/controller/controller.c
View File

@ -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

2
Bingoo/ctrl2/controller.c
View File

@ -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

2
Bingoo/main/CMakeLists.txt
View File

@ -42,7 +42,7 @@ endif()
if(DEFINED GLOBAL_ENABLED_MACROS AND NOT GLOBAL_ENABLED_MACROS STREQUAL "")
message(STATUS "[Main] Defining macros: ${GLOBAL_ENABLED_MACROS}")
target_compile_definitions(${TARGET_NAME} PRIVATE ${GLOBAL_ENABLED_MACROS})
target_compile_definitions(${TARGET_NAME} PUBLIC ${GLOBAL_ENABLED_MACROS})
endif()
target_compile_options(${TARGET_NAME}

6
Bingoo/motor/CMakeLists.txt
View File

@ -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()

35
Bingoo/motor/include/motor.h
View File

@ -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_ */

114
Bingoo/motor/include/msp_TTMotor_ZQ.h
View File

@ -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__ */

244
Bingoo/motor/motor.c
View File

@ -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

389
Bingoo/motor/msp_TTMotor_ZQ.c
View File

@ -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

2
Bingoo/motorTT/motor.c
View File

@ -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

2
Bingoo/paint/paint.c
View File

@ -16,6 +16,7 @@
修改内容 : 创建文件
******************************************************************************/
#ifdef BUILD_PAINT
#include "robot.h"
/*----------------------------------------------*
@ -67,3 +68,4 @@ int PaintGetParam(void)
return 0;
}
#endif

2
Bingoo/swing/swing.c
View File

@ -16,6 +16,7 @@
修改内容 : 创建文件
******************************************************************************/
#ifdef BUILD_SWING
#include "robot.h"
#include "paint_if.h"
@ -70,3 +71,4 @@ int SwingArmGetParam(void)
return 0;
}
#endif

617
Bingoo/tools/cmake_deps.py
View File

@ -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 = """<!DOCTYPE html>
<html lang="zh-CN">
<head>
<meta charset="UTF-8">
<meta name="viewport" content="width=device-width, initial-scale=1.0">
<title>CMake 依赖图 (终极版)</title>
<style>
body { margin: 0; overflow: hidden; background-color: #f4f6f8; font-family: 'Segoe UI', sans-serif; }
#container { width: 100vw; height: 100vh; position: relative; }
svg { width: 100%; height: 100%; cursor: grab; display: block; }
svg:active { cursor: grabbing; }
/* 节点样式 */
.node circle { fill: #fff; stroke: #555; stroke-width: 2px; transition: all 0.3s; }
.node text { font-size: 13px; font-weight: 600; pointer-events: none; text-anchor: middle; dy: 5; fill: #333; user-select: none; }
/* 连线样式 */
.link { stroke: #ccc; stroke-opacity: 0.4; stroke-width: 1.5px; fill: none; }
.link-arrow { fill: #ccc; opacity: 0.4; }
/* 交互状态 */
.node.highlight circle { fill: #d4edda; stroke: #28a745; stroke-width: 3px; r: 35; }
.node.highlight text { fill: #155724; font-weight: bold; }
.node.dimmed { opacity: 0.15; }
.node.dimmed circle { stroke: #999; }
.link.dimmed { opacity: 0.05; stroke-width: 1px; }
.link.highlight { stroke: #28a745; stroke-width: 2.5px; opacity: 1; }
.link-arrow.highlight { fill: #28a745; opacity: 1; }
/* UI 控件 */
#ui-layer {
position: absolute; top: 20px; left: 20px; z-index: 10;
display: flex; flex-direction: column; gap: 10px;
}
.panel {
background: rgba(255,255,255,0.95);
padding: 15px; border-radius: 8px;
box-shadow: 0 4px 15px rgba(0,0,0,0.1);
backdrop-filter: blur(5px);
}
h2 { margin: 0 0 5px 0; font-size: 16px; color: #333; }
p { margin: 0; font-size: 12px; color: #666; line-height: 1.4; }
/* 搜索框 */
#search-box {
padding: 8px 12px; border: 1px solid #ddd; border-radius: 4px;
width: 200px; font-size: 14px; outline: none; transition: border 0.2s;
}
#search-box:focus { border-color: #28a745; }
/* 按钮 */
.btn {
background: #007bff; color: white; border: none;
padding: 8px 15px; border-radius: 4px; cursor: pointer;
font-size: 13px; margin-top: 5px; width: 100%;
transition: background 0.2s;
}
.btn:hover { background: #0056b3; }
.btn-secondary { background: #6c757d; }
.btn-secondary:hover { background: #545b62; }
/* Tooltip */
#tooltip {
position: absolute; background: rgba(0,0,0,0.8); color: #fff;
padding: 8px 12px; border-radius: 4px; font-size: 12px;
pointer-events: none; opacity: 0; transition: opacity 0.2s;
z-index: 20; white-space: pre-line; max-width: 200px;
box-shadow: 0 2px 10px rgba(0,0,0,0.2);
}
#tooltip strong { color: #4facfe; }
</style>
</head>
<body>
<div id="container">
<!-- UI 层 -->
<div id="ui-layer">
<div class="panel">
<h2>📦 CMake 依赖图</h2>
<p>💡 点击节点查看<strong>对外依赖</strong></p>
<input type="text" id="search-box" placeholder="🔍 搜索模块名称...">
<button class="btn btn-secondary" id="btn-export">📷 导出 PNG 图片</button>
</div>
<div class="panel" id="stats-panel" style="display:none;">
<p id="stats-content"></p>
</div>
</div>
<!-- 画布 -->
<svg id="graph-svg"></svg>
<!-- 悬浮提示 -->
<div id="tooltip"></div>
</div>
<script>
const rawData = {data_json_placeholder};
const svg = document.getElementById('graph-svg');
const tooltip = document.getElementById('tooltip');
const searchBox = document.getElementById('search-box');
const btnExport = document.getElementById('btn-export');
const statsPanel = document.getElementById('stats-panel');
const statsContent = document.getElementById('stats-content');
let width = window.innerWidth;
let height = window.innerHeight;
// 视图变换
let transform = { x: 0, y: 0, k: 1 };
let isDragging = false;
let startPan = { x: 0, y: 0 };
const gLinks = document.createElementNS("http://www.w3.org/2000/svg", "g");
const gNodes = document.createElementNS("http://www.w3.org/2000/svg", "g");
const gArrows = document.createElementNS("http://www.w3.org/2000/svg", "defs");
// 定义箭头标记
gArrows.innerHTML = `
<marker id="arrow-default" markerWidth="10" markerHeight="10" refX="28" refY="3" orient="auto" markerUnits="strokeWidth">
<path d="M0,0 L0,6 L9,3 z" fill="#ccc" opacity="0.4" />
</marker>
<marker id="arrow-highlight" markerWidth="10" markerHeight="10" refX="33" refY="3" orient="auto" markerUnits="strokeWidth">
<path d="M0,0 L0,6 L9,3 z" fill="#28a745" />
</marker>
`;
svg.appendChild(gArrows);
svg.appendChild(gLinks);
svg.appendChild(gNodes);
// 初始化节点数据
const nodes = rawData.nodes.map(n => ({
id: n.id,
inDeg: n.in,
outDeg: n.out,
x: Math.random() * (width - 100) + 50,
y: Math.random() * (height - 100) + 50,
vx: 0, vy: 0,
radius: 25
}));
const nodeMap = {};
nodes.forEach(n => nodeMap[n.id] = n);
const links = rawData.edges.map(e => ({
source: nodeMap[e.source],
target: nodeMap[e.target]
}));
// === 物理引擎参数 ===
const REPULSION = 6000;
const SPRING_LENGTH = 250;
const SPRING_STRENGTH = 0.04;
const DAMPING = 0.85;
const CENTER_GRAVITY = 0.01;
function tick() {
// 斥力
for (let i = 0; i < nodes.length; i++) {
for (let j = i + 1; j < nodes.length; j++) {
const a = nodes[i], b = nodes[j];
let dx = a.x - b.x, dy = a.y - b.y;
let dist = Math.sqrt(dx*dx + dy*dy) || 1;
let force = REPULSION / (dist * dist);
a.vx += (dx / dist) * force;
a.vy += (dy / dist) * force;
b.vx -= (dx / dist) * force;
b.vy -= (dy / dist) * force;
}
}
// 引力
links.forEach(link => {
const a = link.source, b = link.target;
let dx = b.x - a.x, dy = b.y - a.y;
let dist = Math.sqrt(dx*dx + dy*dy) || 1;
let force = (dist - SPRING_LENGTH) * SPRING_STRENGTH;
let fx = (dx / dist) * force, fy = (dy / dist) * force;
a.vx += fx; a.vy += fy;
b.vx -= fx; b.vy -= fy;
});
// 更新位置
nodes.forEach(n => {
n.vx += (width/2 - n.x) * CENTER_GRAVITY;
n.vy += (height/2 - n.y) * CENTER_GRAVITY;
n.x += n.vx; n.y += n.vy;
n.vx *= DAMPING; n.vy *= DAMPING;
// 边界
n.x = Math.max(n.radius, Math.min(width - n.radius, n.x));
n.y = Math.max(n.radius, Math.min(height - n.radius, n.y));
});
}
let iterations = 0;
function animate() {
if (iterations < 400) {
tick();
iterations++;
render();
requestAnimationFrame(animate);
} else {
// 动画结束，不再每帧重绘 DOM，只在交互时更新
}
}
function render() {
// 渲染连线 (带箭头)
let linksHTML = '';
links.forEach((l, idx) => {
linksHTML += `<line x1="${l.source.x}" y1="${l.source.y}" x2="${l.target.x}" y2="${l.target.y}"
class="link" data-src="${l.source.id}" data-tgt="${l.target.id}"
marker-end="url(#arrow-default)" />`;
});
gLinks.innerHTML = linksHTML;
// 渲染节点
let nodesHTML = '';
nodes.forEach(n => {
nodesHTML += `
<g class="node" transform="translate(${n.x},${n.y})" data-id="${n.id}">
<circle r="${n.radius}" />
<text>${n.id}</text>
</g>
`;
});
gNodes.innerHTML = nodesHTML;
bindEvents();
}
function bindEvents() {
// 节点交互
document.querySelectorAll('.node').forEach(el => {
el.addEventListener('click', (e) => {
e.stopPropagation();
const id = el.getAttribute('data-id');
highlight(id);
});
el.addEventListener('mouseenter', (e) => {
const id = el.getAttribute('data-id');
const node = nodeMap[id];
tooltip.innerHTML = `<strong>${id}</strong><br>被依赖: ${node.inDeg}<br>依赖他人: ${node.outDeg}`;
tooltip.style.opacity = 1;
moveTooltip(e);
});
el.addEventListener('mousemove', moveTooltip);
el.addEventListener('mouseleave', () => {
tooltip.style.opacity = 0;
});
});
// 全局鼠标移动 (用于拖拽和 Tooltip)
document.addEventListener('mousemove', (e) => {
if(isDragging) {
transform.x = e.clientX - startPan.x;
transform.y = e.clientY - startPan.y;
updateTransform();
}
if(tooltip.style.opacity == 1) moveTooltip(e);
});
}
function moveTooltip(e) {
tooltip.style.left = (e.clientX + 15) + 'px';
tooltip.style.top = (e.clientY + 15) + 'px';
}
// === 核心高亮逻辑：只高亮输出 ===
function highlight(activeId) {
document.querySelectorAll('.node').forEach(n => n.classList.remove('highlight', 'dimmed'));
document.querySelectorAll('.link').forEach(l => {
l.classList.remove('highlight', 'dimmed');
l.setAttribute('marker-end', 'url(#arrow-default)');
});
const relatedIds = new Set([activeId]);
let count = 0;
document.querySelectorAll('.link').forEach(l => {
const src = l.getAttribute('data-src');
const tgt = l.getAttribute('data-tgt');
if (src === activeId) {
relatedIds.add(tgt);
l.classList.add('highlight');
l.setAttribute('marker-end', 'url(#arrow-highlight)'); // 变色箭头
count++;
} else {
l.classList.add('dimmed');
}
});
document.querySelectorAll('.node').forEach(n => {
const id = n.getAttribute('data-id');
if (relatedIds.has(id)) {
n.classList.add('highlight');
} else {
n.classList.add('dimmed');
}
});
// 更新统计面板
statsPanel.style.display = 'block';
statsContent.innerHTML = `选中：<strong>${activeId}</strong><br>正在展示其 <span style="color:#28a745;font-weight:bold">${count}</span> 个直接依赖项。`;
}
// === 搜索功能 ===
searchBox.addEventListener('input', (e) => {
const val = e.target.value.toLowerCase();
if (!val) {
// 清空搜索，重置视图
document.querySelectorAll('.node').forEach(n => n.classList.remove('highlight', 'dimmed'));
document.querySelectorAll('.link').forEach(l => {
l.classList.remove('highlight', 'dimmed');
l.setAttribute('marker-end', 'url(#arrow-default)');
});
statsPanel.style.display = 'none';
return;
}
// 简单搜索：高亮包含关键词的节点
let found = false;
document.querySelectorAll('.node').forEach(n => {
const id = n.getAttribute('data-id');
if (id.toLowerCase().includes(val)) {
n.classList.add('highlight');
n.classList.remove('dimmed');
found = true;
// 可选：自动平移到第一个匹配项 (略复杂，暂省略)
} else {
n.classList.add('dimmed');
n.classList.remove('highlight');
}
});
// 淡化所有连线
document.querySelectorAll('.link').forEach(l => l.classList.add('dimmed'));
if(found) {
statsPanel.style.display = 'block';
statsContent.innerHTML = `搜索结果："<strong>${val}</strong>"`;
} else {
statsPanel.style.display = 'none';
}
});
// === 导出图片功能 ===
btnExport.addEventListener('click', () => {
// 1. 克隆 SVG
const svgEl = document.getElementById('graph-svg');
const clone = svgEl.cloneNode(true);
// 2. 移除 transform，应用当前坐标到元素本身 (简化处理：直接序列化当前状态)
// 注意：为了简单，我们直接截取当前视口看到的区域可能更准确，
// 但这里我们导出全图。
const serializer = new XMLSerializer();
let source = serializer.serializeToString(clone);
// 添加命名空间
if(!source.match(/^<svg[^>]+xmlns="http\:\/\/www\.w3\.org\/2000\/svg"/)){
source = source.replace(/^<svg/, '<svg xmlns="http://www.w3.org/2000/svg"');
}
if(!source.match(/^<svg[^>]+xmlns:xlink="http\:\/\/www\.w3\.org\/1999\/xlink"/)){
source = source.replace(/^<svg/, '<svg xmlns:xlink="http://www.w3.org/1999/xlink"');
}
// 3. 创建 Canvas
const canvas = document.createElement('canvas');
canvas.width = width;
canvas.height = height;
const ctx = canvas.getContext('2d');
// 填充背景
ctx.fillStyle = '#f4f6f8';
ctx.fillRect(0, 0, canvas.width, canvas.height);
const img = new Image();
const svgBlob = new Blob([source], {type: 'image/svg+xml;charset=utf-8'});
const url = URL.createObjectURL(svgBlob);
img.onload = function() {
// 应用当前的缩放和平移
ctx.setTransform(transform.k, 0, 0, transform.k, transform.x, transform.y);
ctx.drawImage(img, 0, 0);
// 下载
const pngUrl = canvas.toDataURL('image/png');
const downloadLink = document.createElement('a');
downloadLink.href = pngUrl;
downloadLink.download = 'cmake_deps_graph.png';
document.body.appendChild(downloadLink);
downloadLink.click();
document.body.removeChild(downloadLink);
URL.revokeObjectURL(url);
};
img.src = url;
});
// 拖拽与缩放事件绑定
svg.addEventListener('mousedown', (e) => {
if (e.target.tagName === 'svg' || e.target.tagName === 'g') {
isDragging = true;
startPan = { x: e.clientX - transform.x, y: e.clientY - transform.y };
svg.style.cursor = 'grabbing';
}
});
window.addEventListener('mouseup', () => { isDragging = false; svg.style.cursor = 'grab'; });
svg.addEventListener('wheel', (e) => {
e.preventDefault();
const scaleAmount = -e.deltaY * 0.001;
transform.k = Math.min(Math.max(0.1, transform.k + scaleAmount), 5);
updateTransform();
});
function updateTransform() {
gLinks.setAttribute('transform', `translate(${transform.x},${transform.y}) scale(${transform.k})`);
gNodes.setAttribute('transform', `translate(${transform.x},${transform.y}) scale(${transform.k})`);
}
window.addEventListener('resize', () => {
width = window.innerWidth;
height = window.innerHeight;
});
// 启动
animate();
// 初始居中偏移
transform.x = width * 0.1;
transform.y = height * 0.1;
updateTransform();
</script>
</body>
</html>
"""
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()

7
Core/Src/main.c
View File

@ -31,12 +31,14 @@
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "BHBF_ROBOT.h"
#include "bsp_FDCAN.h"
//#include "bsp_FDCAN.h"
#include <bsp_client_setting.h>
#include "msp_ground_management.h"
#include "msp_strain_gauge.h"
#include <bsp_tempature.h>
#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);

53
Core/Src/stm32h7xx_it.c
View File

@ -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)
{

Write Preview
Loading…
Cancel
Save