15 changed files with 392 additions and 301 deletions
@ -0,0 +1,45 @@ |
|||||
|
/*
|
||||
|
* motor.h |
||||
|
* |
||||
|
* Created on: 2026年4月20日 |
||||
|
* Author: Lizongdi |
||||
|
*/ |
||||
|
#ifdef BUILD_CANMOTOR |
||||
|
#ifndef FSM_INC_MOTOR_H_ |
||||
|
#define FSM_INC_MOTOR_H_ |
||||
|
|
||||
|
#include "bsp_FDCAN.h" |
||||
|
#include "BHBF_ROBOT.h" |
||||
|
|
||||
|
typedef enum |
||||
|
{ |
||||
|
M_TYPE_MOVELEFT, |
||||
|
M_TYPE_MOVERIGHT, |
||||
|
M_TYPE_SWING |
||||
|
}EMotorType; |
||||
|
|
||||
|
typedef struct |
||||
|
{ |
||||
|
int32_t MotorID; |
||||
|
EMotorType MotorType; |
||||
|
int32_t HeartbeatID; |
||||
|
FDCANHandler *fHandler; |
||||
|
TT_MotorParameters *pMotorParam; |
||||
|
}TMotorCtrl; |
||||
|
|
||||
|
/*待适配接口*/ |
||||
|
void MotorGetState(TMotorCtrl *ptMotorCtrl, char *buffer); |
||||
|
|
||||
|
void WheelFuncInit(TMotorCtrl *ptLeftMotorCtrl, TMotorCtrl *ptRightMotorCtrl); |
||||
|
void WheelFuncProc(TMotorCtrl *ptLeftMotorCtrl, TMotorCtrl *ptRightMotorCtrl); |
||||
|
void WheelSetVelcity(TMotorCtrl *ptMotorCtrl, float fSpeed_M_min); |
||||
|
|
||||
|
void SwingFuncInit(TMotorCtrl *ptMotorCtrl); |
||||
|
void SwingFuncProc(TMotorCtrl *ptMotorCtrl); |
||||
|
void SwingSetPosition(TMotorCtrl *ptMotorCtrl, float fPositionAngle, int32_t iVelcityDegreeS); |
||||
|
|
||||
|
/*对外接口声明*/ |
||||
|
void MotorInit(void); |
||||
|
|
||||
|
#endif |
||||
|
#endif /* FSM_INC_MOTOR_H_ */ |
||||
@ -0,0 +1,141 @@ |
|||||
|
/*
|
||||
|
* motor.c |
||||
|
* |
||||
|
* Created on: 2026年4月20日 |
||||
|
* Author: Lizongdi |
||||
|
*/ |
||||
|
#ifdef BUILD_CANMOTOR |
||||
|
|
||||
|
#include "motor.h" |
||||
|
|
||||
|
TMotorCtrl *g_leftmotor; |
||||
|
TMotorCtrl *g_rightmotor; |
||||
|
TMotorCtrl *g_swingmotor; |
||||
|
|
||||
|
char a[10]; |
||||
|
int32_t fluat_flag=0; |
||||
|
|
||||
|
static void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length) |
||||
|
{ |
||||
|
|
||||
|
memcpy(a,buffer,8); |
||||
|
fluat_flag = canID - 0x580; |
||||
|
switch (fluat_flag) |
||||
|
{ |
||||
|
case 1: |
||||
|
{ |
||||
|
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID1_LeftMotor", 1); |
||||
|
MotorGetState(g_leftmotor, buffer); |
||||
|
} |
||||
|
break; |
||||
|
case 2: |
||||
|
{ |
||||
|
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID2_RightMotor", 1); |
||||
|
MotorGetState(g_rightmotor, buffer); |
||||
|
} |
||||
|
break; |
||||
|
case 3: |
||||
|
{ |
||||
|
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "ZQ_CAN_ID3_SwingMotor", 1); |
||||
|
MotorGetState(g_swingmotor, buffer); |
||||
|
} |
||||
|
break; |
||||
|
} |
||||
|
|
||||
|
} |
||||
|
|
||||
|
static void MotorCommandsLoop() |
||||
|
{ |
||||
|
static char TT_Motor_Need_To_Activate = 1; |
||||
|
WheelSetVelcity(g_leftmotor, GV.Left_Speed_M_min); |
||||
|
WheelSetVelcity(g_rightmotor, GV.Right_Speed_M_min); |
||||
|
|
||||
|
if (TT_Motor_Need_To_Activate == 1) |
||||
|
{ |
||||
|
WheelFuncInit(g_leftmotor, g_rightmotor); |
||||
|
TT_Motor_Need_To_Activate = 0; |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
WheelFuncProc(g_leftmotor, g_rightmotor); |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
static void MotorCommandsLoop_2_Position() |
||||
|
{ |
||||
|
static char TT_Motor_Need_To_Activate_1 = 1; |
||||
|
SwingSetPosition(g_swingmotor, GV.Tar_Position_angle, GV.Tar_Position_Velcity_Degree_S); |
||||
|
|
||||
|
if (TT_Motor_Need_To_Activate_1 == 1) |
||||
|
{ |
||||
|
SwingFuncInit(g_swingmotor); |
||||
|
TT_Motor_Need_To_Activate_1 = 0; |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
SwingFuncProc(g_swingmotor); |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
static TMotorCtrl *MotorControllerInit(int32_t MotorID, EMotorType MotorType, |
||||
|
int32_t HeartbeatID, FDCANHandler *fHandler) |
||||
|
{ |
||||
|
TMotorCtrl *ptMotorCtrl = malloc(sizeof(TMotorCtrl)); |
||||
|
|
||||
|
if (ptMotorCtrl == NULL) |
||||
|
{ |
||||
|
return NULL; |
||||
|
} |
||||
|
|
||||
|
ptMotorCtrl->pMotorParam = malloc(sizeof(TT_MotorParameters)); |
||||
|
|
||||
|
if (ptMotorCtrl->pMotorParam == NULL) |
||||
|
{ |
||||
|
return NULL; |
||||
|
} |
||||
|
|
||||
|
if (M_TYPE_MOVELEFT == MotorType) |
||||
|
{ |
||||
|
ptMotorCtrl->pMotorParam = &GV.LeftMotor; |
||||
|
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID1_LeftMotor", 1, ComError_ZQ_CAN_ID1_LeftMotor); |
||||
|
} |
||||
|
else if (M_TYPE_MOVERIGHT == MotorType) |
||||
|
{ |
||||
|
ptMotorCtrl->pMotorParam = &GV.RightMotor; |
||||
|
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID2_RightMotor", 1, ComError_ZQ_CAN_ID2_RightMotor); |
||||
|
} |
||||
|
else if (M_TYPE_SWING == MotorType) |
||||
|
{ |
||||
|
ptMotorCtrl->pMotorParam = &GV.SwingMotor; |
||||
|
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID3_SwingMotor", 1, ComError_ZQ_CAN_ID3_SwingMotor); |
||||
|
} |
||||
|
|
||||
|
ptMotorCtrl->pMotorParam->MotorID = MotorID; |
||||
|
ptMotorCtrl->MotorID = MotorID; |
||||
|
ptMotorCtrl->MotorType = MotorType; |
||||
|
ptMotorCtrl->HeartbeatID = HeartbeatID; |
||||
|
ptMotorCtrl->fHandler = fHandler; |
||||
|
|
||||
|
ptMotorCtrl->fHandler->CAN_Decode = Roughening_MotorDecodeCAN; |
||||
|
ptMotorCtrl->fHandler->dispacherController->DispacherCallTime = 2; |
||||
|
|
||||
|
if (M_TYPE_SWING == MotorType) |
||||
|
{ |
||||
|
ptMotorCtrl->fHandler->dispacherController->Add_Dispatcher_List(ptMotorCtrl->fHandler->dispacherController, MotorCommandsLoop_2_Position); |
||||
|
} |
||||
|
else if (M_TYPE_MOVELEFT == MotorType || M_TYPE_MOVERIGHT == MotorType) |
||||
|
{ |
||||
|
ptMotorCtrl->fHandler->dispacherController->Add_Dispatcher_List(ptMotorCtrl->fHandler->dispacherController, MotorCommandsLoop); |
||||
|
} |
||||
|
|
||||
|
return ptMotorCtrl; |
||||
|
} |
||||
|
|
||||
|
void MotorInit(void) |
||||
|
{ |
||||
|
g_leftmotor = MotorControllerInit(1, M_TYPE_MOVELEFT, 0x707, &FD_CAN_1_Handler); |
||||
|
g_rightmotor = MotorControllerInit(2, M_TYPE_MOVERIGHT, 0x708, &FD_CAN_1_Handler); |
||||
|
g_swingmotor = MotorControllerInit(3, M_TYPE_SWING, 0x709, &FD_CAN_2_Handler); |
||||
|
} |
||||
|
|
||||
|
#endif |
||||
@ -1,35 +0,0 @@ |
|||||
/*
|
|
||||
* motor.h |
|
||||
* |
|
||||
* Created on: 2026年4月20日 |
|
||||
* Author: Lizongdi |
|
||||
*/ |
|
||||
#ifdef BUILD_MOTOR |
|
||||
#ifndef FSM_INC_MOTOR_H_ |
|
||||
#define FSM_INC_MOTOR_H_ |
|
||||
|
|
||||
#include "bsp_FDCAN.h" |
|
||||
|
|
||||
typedef enum |
|
||||
{ |
|
||||
M_TYPE_MOVELEFT, |
|
||||
M_TYPE_MOVERIGHT, |
|
||||
M_TYPE_SWING |
|
||||
}EMotorType; |
|
||||
|
|
||||
typedef struct |
|
||||
{ |
|
||||
int32_t MotorID; |
|
||||
EMotorType MotorType; |
|
||||
int32_t HeartbeatID; |
|
||||
FDCANHandler *fHandler; |
|
||||
}TMotorCtrl; |
|
||||
|
|
||||
typedef struct |
|
||||
{ |
|
||||
void (*MotorFuncInit)(TMotorCtrl *ptMotorCtrl); |
|
||||
}TMotorInterface; |
|
||||
|
|
||||
void MotorInit(void); |
|
||||
#endif |
|
||||
#endif /* FSM_INC_MOTOR_H_ */ |
|
||||
@ -1,198 +0,0 @@ |
|||||
/*
|
|
||||
* motor.c |
|
||||
* |
|
||||
* Created on: 2026年4月20日 |
|
||||
* Author: Lizongdi |
|
||||
*/ |
|
||||
#ifdef BUILD_MOTOR |
|
||||
|
|
||||
#include "bsp_FDCAN.h" |
|
||||
#include "motor.h" |
|
||||
#include "BHBF_ROBOT.h" |
|
||||
#include "msp_TTMotor_ZQ.h" |
|
||||
|
|
||||
TMotorCtrl *g_leftmotor; |
|
||||
TMotorCtrl *g_rightmotor; |
|
||||
TMotorCtrl *g_swingmotor; |
|
||||
|
|
||||
char a[10]; |
|
||||
int32_t fluat_flag=0; |
|
||||
|
|
||||
extern TT_MotorParameters *TT_Motor[7]; |
|
||||
|
|
||||
static void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length) |
|
||||
{ |
|
||||
|
|
||||
memcpy(a,buffer,8); |
|
||||
fluat_flag = canID - 0x580; |
|
||||
switch (fluat_flag) |
|
||||
{ |
|
||||
|
|
||||
case 1: |
|
||||
{ |
|
||||
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID1_LeftMotor", 1); |
|
||||
TT_Analytic_Fun(1, buffer); |
|
||||
} |
|
||||
break; |
|
||||
case 2: |
|
||||
{ |
|
||||
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID2_RightMotor", 1); |
|
||||
TT_Analytic_Fun(2, buffer); |
|
||||
} |
|
||||
break; |
|
||||
case 3: |
|
||||
{ |
|
||||
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "ZQ_CAN_ID3_SwingMotor", 1); |
|
||||
TT_Analytic_Fun(3, buffer); |
|
||||
} |
|
||||
break; |
|
||||
} |
|
||||
|
|
||||
} |
|
||||
|
|
||||
static void MotorCommandsLoop() |
|
||||
{ |
|
||||
static char TT_Motor_Need_To_Activate = 1; |
|
||||
//
|
|
||||
// if(start_time<15000)
|
|
||||
// {
|
|
||||
// HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID1_LeftMotor", 1);
|
|
||||
// HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID2_RightMotor", 1);
|
|
||||
// }
|
|
||||
static int Heartbeat_Flag_01; |
|
||||
MotorMoveParamInit(); |
|
||||
|
|
||||
if (TT_Motor_Need_To_Activate == 1) |
|
||||
{ |
|
||||
ActivateMotor(g_leftmotor, 6000); |
|
||||
ActivateMotor(g_rightmotor, 2000); |
|
||||
ActivateMotor(g_leftmotor, 2000); |
|
||||
ActivateMotor(g_rightmotor, 2000); |
|
||||
ActivateMotor(g_leftmotor, 2000); |
|
||||
ActivateMotor(g_rightmotor, 2000); |
|
||||
ActivateMotor(g_leftmotor, 2000); |
|
||||
ActivateMotor(g_rightmotor, 2000); |
|
||||
|
|
||||
Enable_NMT(g_leftmotor,1000); |
|
||||
Enable_NMT(g_rightmotor,1000); |
|
||||
|
|
||||
Configure_Asynchronous_Mode(g_leftmotor, 7,600); |
|
||||
Consumer_Or_microcontroller_Heartbeat(g_leftmotor, 4); |
|
||||
Configure_Asynchronous_Mode(g_rightmotor, 8,600); |
|
||||
Consumer_Or_microcontroller_Heartbeat(g_leftmotor, 4); |
|
||||
SpeedModeSetup(g_leftmotor, 6, 500, 500, 0); |
|
||||
Consumer_Or_microcontroller_Heartbeat(g_rightmotor, 4); |
|
||||
SpeedModeSetup(g_rightmotor, 6, 500, 500, 0); |
|
||||
Consumer_Or_microcontroller_Heartbeat(g_leftmotor, 4); |
|
||||
Consumer_Or_microcontroller_Heartbeat(g_rightmotor, 4); |
|
||||
TT_Motor_Need_To_Activate = 0; |
|
||||
} |
|
||||
else |
|
||||
{ |
|
||||
TT_Request_Position(g_leftmotor, 6); |
|
||||
TT_Request_Velocity(g_leftmotor, 6); |
|
||||
TT_Request_Current(g_leftmotor, 6); |
|
||||
TT_Request_Fault(g_leftmotor, 6); |
|
||||
|
|
||||
TT_Request_Position(g_rightmotor, 6); |
|
||||
TT_Request_Velocity(g_rightmotor, 6); |
|
||||
TT_Request_Current(g_rightmotor, 6); |
|
||||
TT_Request_Fault(g_rightmotor, 6); |
|
||||
|
|
||||
TT_SpeedMode_Set_TargetSpeed(g_leftmotor, 6, GV.LeftMotor.Target_Velcity); |
|
||||
TT_SpeedMode_Set_TargetSpeed(g_rightmotor, 6, GV.RightMotor.Target_Velcity); |
|
||||
} |
|
||||
|
|
||||
Heartbeat_Flag_01++; |
|
||||
if(Heartbeat_Flag_01%7==0) |
|
||||
{ |
|
||||
Consumer_Or_microcontroller_Heartbeat(g_leftmotor, 4); |
|
||||
Consumer_Or_microcontroller_Heartbeat(g_rightmotor, 4); |
|
||||
Heartbeat_Flag_01=0; |
|
||||
} |
|
||||
} |
|
||||
|
|
||||
static void MotorCommandsLoop_2_Position() |
|
||||
{ |
|
||||
static char TT_Motor_Need_To_Activate_1 = 1; |
|
||||
// if(start_time<15000)
|
|
||||
// {
|
|
||||
// HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "ZQ_CAN_ID3_SwingMotor", 1);
|
|
||||
// }
|
|
||||
MotorMoveCAN2ParamInit(); |
|
||||
|
|
||||
if (TT_Motor_Need_To_Activate_1 == 1) |
|
||||
{ |
|
||||
ActivateMotor(g_swingmotor, 6000); |
|
||||
ActivateMotor(g_swingmotor, 2000); |
|
||||
ActivateMotor(g_swingmotor, 2000); |
|
||||
ActivateMotor(g_swingmotor, 2000); |
|
||||
ActivateMotor(g_swingmotor, 2000); |
|
||||
Postion_Velcocity_Run_SetParameter(g_swingmotor, 0, 0, 500, 500, 100); |
|
||||
TT_Motor_Need_To_Activate_1 = 0; |
|
||||
} |
|
||||
else |
|
||||
{ |
|
||||
TT_Request_Position(g_swingmotor, 10); |
|
||||
TT_Request_Fault(g_swingmotor, 10); |
|
||||
|
|
||||
switch(GV.SwingMotor.Position_immediately1_Lag2) |
|
||||
{ |
|
||||
case 1: |
|
||||
Position_Immediately_Setting(g_swingmotor, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,30); |
|
||||
break; |
|
||||
case 2: |
|
||||
Position_Lag_Setting(g_swingmotor, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,30); |
|
||||
break; |
|
||||
} |
|
||||
} |
|
||||
} |
|
||||
|
|
||||
static TMotorCtrl *MotorControllerInit(int32_t MotorID, EMotorType MotorType, |
|
||||
int32_t HeartbeatID, FDCANHandler *fHandler) |
|
||||
{ |
|
||||
TMotorCtrl *ptMotorCtrl = malloc(sizeof(TMotorCtrl)); |
|
||||
if (M_TYPE_MOVELEFT == MotorType) |
|
||||
{ |
|
||||
TT_Motor[M_TYPE_MOVELEFT] = &GV.LeftMotor; |
|
||||
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID1_LeftMotor", 1, ComError_ZQ_CAN_ID1_LeftMotor); |
|
||||
} |
|
||||
else if (M_TYPE_MOVERIGHT == MotorType) |
|
||||
{ |
|
||||
TT_Motor[M_TYPE_MOVERIGHT] = &GV.RightMotor; |
|
||||
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID2_RightMotor", 1, ComError_ZQ_CAN_ID2_RightMotor); |
|
||||
} |
|
||||
else if (M_TYPE_SWING == MotorType) |
|
||||
{ |
|
||||
TT_Motor[M_TYPE_SWING] = &GV.SwingMotor; |
|
||||
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID3_SwingMotor", 1, ComError_ZQ_CAN_ID3_SwingMotor); |
|
||||
} |
|
||||
TT_Motor[MotorType]->MotorID = MotorID; |
|
||||
ptMotorCtrl->MotorID = MotorID; |
|
||||
ptMotorCtrl->MotorType = MotorType; |
|
||||
ptMotorCtrl->HeartbeatID = HeartbeatID; |
|
||||
ptMotorCtrl->fHandler = fHandler; |
|
||||
|
|
||||
ptMotorCtrl->fHandler->CAN_Decode = Roughening_MotorDecodeCAN; |
|
||||
|
|
||||
if (M_TYPE_SWING == MotorType) |
|
||||
{ |
|
||||
ptMotorCtrl->fHandler->dispacherController->Add_Dispatcher_List(ptMotorCtrl->fHandler->dispacherController, MotorCommandsLoop_2_Position); |
|
||||
} |
|
||||
else |
|
||||
{ |
|
||||
ptMotorCtrl->fHandler->dispacherController->Add_Dispatcher_List(ptMotorCtrl->fHandler->dispacherController, MotorCommandsLoop); |
|
||||
} |
|
||||
ptMotorCtrl->fHandler->dispacherController->DispacherCallTime = 2; |
|
||||
|
|
||||
return ptMotorCtrl; |
|
||||
} |
|
||||
|
|
||||
void MotorInit(void) |
|
||||
{ |
|
||||
g_leftmotor = MotorControllerInit(1, M_TYPE_MOVELEFT, 0x707, &FD_CAN_1_Handler); |
|
||||
g_rightmotor = MotorControllerInit(2, M_TYPE_MOVERIGHT, 0x708, &FD_CAN_1_Handler); |
|
||||
g_swingmotor = MotorControllerInit(3, M_TYPE_SWING, 0x709, &FD_CAN_2_Handler); |
|
||||
} |
|
||||
|
|
||||
#endif |
|
||||
@ -0,0 +1,151 @@ |
|||||
|
/******************************************************************************
|
||||
|
|
||||
|
版权所有 (C), 2018-2099, Radkil |
||||
|
|
||||
|
****************************************************************************** |
||||
|
文 件 名 : motor_adapter.c |
||||
|
版 本 号 : 初稿 |
||||
|
作 者 : radkil |
||||
|
生成日期 : 2026年4月21日 |
||||
|
最近修改 : |
||||
|
功能描述 : 电机适配层 |
||||
|
|
||||
|
修改历史 : |
||||
|
1.日 期 : 2026年4月21日 |
||||
|
作 者 : radkil |
||||
|
修改内容 : 创建文件 |
||||
|
|
||||
|
******************************************************************************/ |
||||
|
#ifdef BUILD_MOTOR |
||||
|
#include "motor.h" |
||||
|
#include "msp_TTMotor_ZQ.h" |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 外部变量说明 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 外部函数原型说明 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 内部函数原型说明 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 全局变量 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 模块级变量 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 常量定义 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
/*----------------------------------------------*
|
||||
|
* 宏定义 * |
||||
|
*----------------------------------------------*/ |
||||
|
|
||||
|
static void WheelSendHeartbeat(TMotorCtrl *ptLeftMotorCtrl, TMotorCtrl *ptRightMotorCtrl) |
||||
|
{ |
||||
|
static int Heartbeat_Flag_01; |
||||
|
Heartbeat_Flag_01++; |
||||
|
if(Heartbeat_Flag_01%7==0) |
||||
|
{ |
||||
|
Consumer_Or_microcontroller_Heartbeat(ptLeftMotorCtrl, 4); |
||||
|
Consumer_Or_microcontroller_Heartbeat(ptRightMotorCtrl, 4); |
||||
|
Heartbeat_Flag_01=0; |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
void MotorGetState(TMotorCtrl *ptMotorCtrl, char *buffer) |
||||
|
{ |
||||
|
TT_Analytic_Fun(ptMotorCtrl, buffer); |
||||
|
} |
||||
|
|
||||
|
void WheelFuncInit(TMotorCtrl *ptLeftMotorCtrl, TMotorCtrl *ptRightMotorCtrl) |
||||
|
{ |
||||
|
ActivateMotor(ptLeftMotorCtrl, 6000); |
||||
|
ActivateMotor(ptRightMotorCtrl, 2000); |
||||
|
ActivateMotor(ptLeftMotorCtrl, 2000); |
||||
|
ActivateMotor(ptRightMotorCtrl, 2000); |
||||
|
ActivateMotor(ptLeftMotorCtrl, 2000); |
||||
|
ActivateMotor(ptRightMotorCtrl, 2000); |
||||
|
ActivateMotor(ptLeftMotorCtrl, 2000); |
||||
|
ActivateMotor(ptRightMotorCtrl, 2000); |
||||
|
|
||||
|
Enable_NMT(ptLeftMotorCtrl,1000); |
||||
|
Enable_NMT(ptRightMotorCtrl,1000); |
||||
|
|
||||
|
Configure_Asynchronous_Mode(ptLeftMotorCtrl, 7,600); |
||||
|
Consumer_Or_microcontroller_Heartbeat(ptLeftMotorCtrl, 4); |
||||
|
Configure_Asynchronous_Mode(ptRightMotorCtrl, 8,600); |
||||
|
Consumer_Or_microcontroller_Heartbeat(ptLeftMotorCtrl, 4); |
||||
|
SpeedModeSetup(ptLeftMotorCtrl, 6, 500, 500, 0); |
||||
|
Consumer_Or_microcontroller_Heartbeat(ptRightMotorCtrl, 4); |
||||
|
SpeedModeSetup(ptRightMotorCtrl, 6, 500, 500, 0); |
||||
|
Consumer_Or_microcontroller_Heartbeat(ptLeftMotorCtrl, 4); |
||||
|
Consumer_Or_microcontroller_Heartbeat(ptRightMotorCtrl, 4); |
||||
|
|
||||
|
WheelSendHeartbeat(ptLeftMotorCtrl, ptRightMotorCtrl); |
||||
|
} |
||||
|
|
||||
|
void WheelFuncProc(TMotorCtrl *ptLeftMotorCtrl, TMotorCtrl *ptRightMotorCtrl) |
||||
|
{ |
||||
|
TT_Request_Position(ptLeftMotorCtrl, 6); |
||||
|
TT_Request_Velocity(ptLeftMotorCtrl, 6); |
||||
|
TT_Request_Current(ptLeftMotorCtrl, 6); |
||||
|
TT_Request_Fault(ptLeftMotorCtrl, 6); |
||||
|
|
||||
|
TT_Request_Position(ptRightMotorCtrl, 6); |
||||
|
TT_Request_Velocity(ptRightMotorCtrl, 6); |
||||
|
TT_Request_Current(ptRightMotorCtrl, 6); |
||||
|
TT_Request_Fault(ptRightMotorCtrl, 6); |
||||
|
|
||||
|
TT_SpeedMode_Set_TargetSpeed(ptLeftMotorCtrl, 6, GV.LeftMotor.Target_Velcity); |
||||
|
TT_SpeedMode_Set_TargetSpeed(ptRightMotorCtrl, 6, GV.RightMotor.Target_Velcity); |
||||
|
|
||||
|
WheelSendHeartbeat(ptLeftMotorCtrl, ptRightMotorCtrl); |
||||
|
} |
||||
|
|
||||
|
void WheelSetVelcity(TMotorCtrl *ptMotorCtrl, float fSpeed_M_min) |
||||
|
{ |
||||
|
ptMotorCtrl->pMotorParam->Target_Velcity = (int32_t)(fSpeed_M_min * Move_Base_Speed_Count_m_Min); |
||||
|
} |
||||
|
|
||||
|
void SwingFuncInit(TMotorCtrl *ptMotorCtrl) |
||||
|
{ |
||||
|
ActivateMotor(ptMotorCtrl, 6000); |
||||
|
ActivateMotor(ptMotorCtrl, 2000); |
||||
|
ActivateMotor(ptMotorCtrl, 2000); |
||||
|
ActivateMotor(ptMotorCtrl, 2000); |
||||
|
ActivateMotor(ptMotorCtrl, 2000); |
||||
|
Postion_Velcocity_Run_SetParameter(ptMotorCtrl, 0, 0, 500, 500, 100); |
||||
|
} |
||||
|
|
||||
|
void SwingFuncProc(TMotorCtrl *ptMotorCtrl) |
||||
|
{ |
||||
|
TT_Request_Position(ptMotorCtrl, 10); |
||||
|
TT_Request_Fault(ptMotorCtrl, 10); |
||||
|
|
||||
|
switch(GV.SwingMotor.Position_immediately1_Lag2) |
||||
|
{ |
||||
|
case 1: |
||||
|
Position_Immediately_Setting(ptMotorCtrl, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,30); |
||||
|
break; |
||||
|
case 2: |
||||
|
Position_Lag_Setting(ptMotorCtrl, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,30); |
||||
|
break; |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
void SwingSetPosition(TMotorCtrl *ptMotorCtrl, float fPositionAngle, int32_t iVelcityDegreeS) |
||||
|
{ |
||||
|
ptMotorCtrl->pMotorParam->Tar_Position_count = (int32_t)(middle_position_motor - fPositionAngle * TT_One_Deg_Count_motor); |
||||
|
ptMotorCtrl->pMotorParam->Tar_Position_Velcity_RPM = iVelcityDegreeS*Swing_Speed_Deg_Sencond_motor; |
||||
|
} |
||||
|
|
||||
|
#endif |
||||
|
|
||||
Loading…
Reference in new issue