HFUT_Firmament_EC
/
sentry_gimbal_hzz
11
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add motor_task.c

This commit is contained in:
NeoZng 2022-10-20 21:14:17 +08:00
parent 78f85b7fda
commit a3a95768e6
12 changed files with 204 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
.vscode/c_cpp_properties.json vendored Normal file
View File

@ -0,0 +1,19 @@
{
"configurations": [
{
"name": "Win32",
"includePath": [
"${workspaceFolder}/**"
],
"defines": [
"_DEBUG",
"UNICODE",
"_UNICODE"
],
"cStandard": "c17",
"intelliSenseMode": "windows-msvc-x64",
"configurationProvider": "ms-vscode.makefile-tools"
}
],
"version": 4
}

5
.vscode/settings.json vendored
View File

@ -26,6 +26,9 @@
"can_receive.h": "c",
"can_process.h": "c",
"can.h": "c",
"stdlib.h": "c"
"stdlib.h": "c",
"lk9025.h": "c",
"dji_motor.h": "c",
"ht04.h": "c"
}
}

1
Src/stm32f4xx_it.c
View File

@ -199,6 +199,7 @@ void DMA1_Stream1_IRQHandler(void)
/* USER CODE BEGIN DMA1_Stream1_IRQn 0 */
/* USER CODE END DMA1_Stream1_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_usart3_rx);
/* USER CODE BEGIN DMA1_Stream1_IRQn 1 */
/* USER CODE END DMA1_Stream1_IRQn 1 */

6
basic_framework.ioc
View File

@ -154,7 +154,7 @@ MxDb.Version=DB.6.0.60
NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
NVIC.CAN1_RX0_IRQn=true\:6\:0\:true\:false\:true\:true\:true\:true\:true
NVIC.CAN2_RX0_IRQn=true\:6\:0\:true\:false\:true\:true\:true\:true\:true
NVIC.DMA1_Stream1_IRQn=true\:5\:0\:true\:false\:true\:true\:false\:false\:true
NVIC.DMA1_Stream1_IRQn=true\:5\:0\:true\:false\:true\:true\:false\:true\:true
NVIC.DMA2_Stream1_IRQn=true\:5\:0\:true\:false\:true\:true\:false\:true\:true
NVIC.DMA2_Stream2_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA2_Stream6_IRQn=true\:5\:0\:true\:false\:true\:true\:false\:true\:true
@ -309,8 +309,8 @@ ProjectManager.MainLocation=Src
ProjectManager.NoMain=false
ProjectManager.PreviousToolchain=
ProjectManager.ProjectBuild=false
ProjectManager.ProjectFileName=abstract_test.ioc
ProjectManager.ProjectName=abstract_test
ProjectManager.ProjectFileName=basic_framework.ioc
ProjectManager.ProjectName=basic_framework
ProjectManager.RegisterCallBack=
ProjectManager.StackSize=0x4000
ProjectManager.TargetToolchain=Makefile

26
modules/motor/HT04.c
View File

@ -1,7 +1,19 @@
#include "HT04.h"
#include "memory.h"
joint_instance joint_motor_info[HT_MOTOR_CNT];
joint_instance* joint_motor_info[HT_MOTOR_CNT];
void HTMotorInit(joint_instance* _instance,CAN_HandleTypeDef* _hcan,uint8_t tx_id,uint8_t rx_id)
{
static uint8_t idx;
_instance->motor_can_instace.can_handle=_hcan;
_instance->motor_can_instace.tx_id=tx_id;
_instance->motor_can_instace.rx_id=rx_id;
_instance->motor_can_instace.can_module_callback=DecodeJoint;
CANRegister(&_instance->motor_can_instace);
joint_motor_info[idx++]=_instance;
}
static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
{
@ -24,7 +36,7 @@ void JointControl(joint_instance* _instance,float current)
tmp = float_to_uint(current, T_MIN, T_MAX, 12);
_instance->motor_can_instace.rx_buff[6] = tmp>>8;
_instance->motor_can_instace.rx_buff[7] = tmp&0xff;
CANTransmit(_instance);
CANTransmit(&_instance->motor_can_instace);
}
void SetJointMode(joint_mode cmd,joint_instance* _instance)
@ -40,15 +52,15 @@ void DecodeJoint(can_instance* motor_instance)
uint16_t tmp;
for (size_t i = 0; i < HT_MOTOR_CNT; i++)
{
if(&joint_motor_info[i].motor_can_instace==motor_instance)
if(&joint_motor_info[i]->motor_can_instace==motor_instance)
{
tmp = (motor_instance->rx_buff[1] << 8) | motor_instance->rx_buff[2];
joint_motor_info[i].last_ecd=joint_motor_info[i].ecd;
joint_motor_info[i].ecd=uint_to_float(tmp,P_MAX,P_MIN,16);
joint_motor_info[i]->last_ecd=joint_motor_info[i]->ecd;
joint_motor_info[i]->ecd=uint_to_float(tmp,P_MAX,P_MIN,16);
tmp = (motor_instance->rx_buff[3] << 4) | (motor_instance->rx_buff[4] >> 4);
joint_motor_info[i].speed_rpm= uint_to_float(tmp,V_MAX,V_MIN,12);
joint_motor_info[i]->speed_rpm= uint_to_float(tmp,V_MAX,V_MIN,12);
tmp=((motor_instance->rx_buff[4]&0xf)<<8) | motor_instance->rx_buff[5];
joint_motor_info[i].given_current=uint_to_float(tmp,T_MAX,T_MIN,12);
joint_motor_info[i]->given_current=uint_to_float(tmp,T_MAX,T_MIN,12);
break;
}
}

4
modules/motor/HT04.h
View File

@ -3,6 +3,7 @@
#include "struct_typedef.h"
#include "bsp_can.h"
#include "controller.h"
#define HT_MOTOR_CNT 4
@ -19,6 +20,8 @@ typedef struct //HT04
float ecd;
float speed_rpm;
float given_current;
PID_t pid;
can_instance motor_can_instace;
} joint_instance;
@ -29,6 +32,7 @@ typedef enum
CMD_ZERO_POSITION = 0xfe
} joint_mode;
void HTMotorInit(joint_instance* _instance,CAN_HandleTypeDef* _hcan,uint8_t tx_id,uint8_t rx_id);
void JointControl(joint_instance* _instance,float current);

60
modules/motor/LK9025.c
View File

@ -1,26 +1,43 @@
#include"LK9025.h"
static driven_instance driven_motor_info[2];
static driven_instance* driven_motor_info[LK_MOTOR_CNT];
void LKMotroInit(uint16_t motor_id,uint16_t rx_id,CAN_HandleTypeDef* hcan)
static void DecodeDriven(can_instance* _instance)
{
for (size_t i = 0; i < LK_MOTOR_CNT; i++)
{
if(&driven_motor_info[i]->motor_can_instance==_instance)
{
driven_motor_info[i]->last_ecd = driven_motor_info[i]->ecd;
driven_motor_info[i]->ecd = (uint16_t)((_instance->rx_buff[7]<<8) | _instance->rx_buff[6]);
driven_motor_info[i]->speed_rpm = (uint16_t)(_instance->rx_buff[5] << 8 | _instance->rx_buff[4]);
driven_motor_info[i]->given_current = (uint16_t)(_instance->rx_buff[3] << 8 | _instance->rx_buff[2]);
driven_motor_info[i]->temperate = _instance->rx_buff[1];
break;
}
}
}
void LKMotroInit(driven_instance* instance,CAN_HandleTypeDef* _hcan,uint8_t tx_id,uint8_t rx_id)
{
static uint8_t idx;
driven_motor_info[idx].motor_can_instance.can_handle=hcan;
driven_motor_info[idx].motor_can_instance.can_module_callback=DecodeDriven;
driven_motor_info[idx].motor_can_instance.rx_id=rx_id;
driven_motor_info[idx].motor_can_instance.tx_id=motor_id;
CANRegister(&driven_motor_info[idx].motor_can_instance);
instance->motor_can_instance.can_module_callback=DecodeDriven;
instance->motor_can_instance.can_handle=_hcan;
instance->motor_can_instance.tx_id=tx_id;
instance->motor_can_instance.rx_id=rx_id;
CANRegister(&instance->motor_can_instance);
driven_motor_info[idx++]=instance;
}
void DrivenControl(int16_t motor1_current,int16_t motor2_current)
{
// LIMIT_MIN_MAX(motor1_current, I_MIN, I_MAX);
// LIMIT_MIN_MAX(motor2_current, I_MIN, I_MAX);
driven_motor_info[0].motor_can_instance.tx_buff[0] = motor1_current;
driven_motor_info[0].motor_can_instance.tx_buff[1] = motor1_current>>8;
driven_motor_info[0].motor_can_instance.tx_buff[2] = motor2_current;
driven_motor_info[0].motor_can_instance.tx_buff[3] = motor2_current>>8;
CANTransmit(&driven_motor_info[0].motor_can_instance);
LIMIT_MIN_MAX(motor1_current, I_MIN, I_MAX);
LIMIT_MIN_MAX(motor2_current, I_MIN, I_MAX);
driven_motor_info[0]->motor_can_instance.tx_buff[0] = motor1_current;
driven_motor_info[0]->motor_can_instance.tx_buff[1] = motor1_current>>8;
driven_motor_info[0]->motor_can_instance.tx_buff[2] = motor2_current;
driven_motor_info[0]->motor_can_instance.tx_buff[3] = motor2_current>>8;
CANTransmit(&driven_motor_info[0]->motor_can_instance);
}
void SetDrivenMode(driven_mode cmd,uint16_t motor_id)
@ -31,18 +48,3 @@ void SetDrivenMode(driven_mode cmd,uint16_t motor_id)
// CANTransmit(driven_mode)
}
void DecodeDriven(can_instance* _instance)
{
for (size_t i = 0; i < LK_MOTOR_CNT; i++)
{
if(&driven_motor_info[i].motor_can_instance==_instance)
{
driven_motor_info[i].last_ecd = driven_motor_info[i].ecd;
driven_motor_info[i].ecd = (uint16_t)((_instance->rx_buff[7]<<8) | _instance->rx_buff[6]);
driven_motor_info[i].speed_rpm = (uint16_t)(_instance->rx_buff[5] << 8 | _instance->rx_buff[4]);
driven_motor_info[i].given_current = (uint16_t)(_instance->rx_buff[3] << 8 | _instance->rx_buff[2]);
driven_motor_info[i].temperate = _instance->rx_buff[1];
break;
}
}
}

8
modules/motor/LK9025.h
View File

@ -3,6 +3,7 @@
#include "struct_typedef.h"
#include "bsp_can.h"
#include "controller.h"
#define LK_MOTOR_CNT 2
#define I_MIN -2000
@ -17,7 +18,10 @@ typedef struct //9025
int16_t speed_rpm;
int16_t given_current;
uint8_t temperate;
PID_t pid;
can_instance motor_can_instance;
} driven_instance;
typedef enum
@ -25,12 +29,10 @@ typedef enum
unused = 0,
} driven_mode;
void LKMotroInit(uint16_t motor_id,uint16_t rx_id,CAN_HandleTypeDef* hcan);
void LKMotroInit(driven_instance* instance,CAN_HandleTypeDef* _hcan,uint8_t tx_id,uint8_t rx_id);
void DrivenControl(int16_t motor1_current,int16_t motor2_current);
void SetDrivenMode(driven_mode cmd,uint16_t motor_id);
void DecodeDriven(can_instance* _instance);
#endif // LK9025_H

36
modules/motor/dji_motor.c
View File

@ -0,0 +1,36 @@
#include "dji_motor.h"
static dji_motor_instance* dji_motor_info[DJI_MOTOR_CNT];
static void DecodeDJIMotor(can_instance* _instance)
{
for (size_t i = 0; i < DJI_MOTOR_CNT; i++)
{
if(&dji_motor_info[i]->motor_can_instance==_instance)
{
dji_motor_info[i]->last_ecd = dji_motor_info[i]->ecd;
dji_motor_info[i]->ecd = (uint16_t)(_instance->rx_buff[0] << 8 | _instance->rx_buff[1]);
dji_motor_info[i]->speed_rpm = (uint16_t)(_instance->rx_buff[2] << 8 | _instance->rx_buff[3]);
dji_motor_info[i]->given_current = (uint16_t)(_instance->rx_buff[4] << 8 | _instance->rx_buff[5]);
dji_motor_info[i]->temperate = _instance->rx_buff[6];
break;
}
}
}
void DJIMotorInit(dji_motor_instance* motor_instance,CAN_HandleTypeDef* _hcan,uint16_t tx_id,uint16_t rx_id)
{
static uint8_t idx;
motor_instance->motor_can_instance.can_handle=_hcan;
motor_instance->motor_can_instance.tx_id=tx_id;
motor_instance->motor_can_instance.rx_id=rx_id;
motor_instance->motor_can_instance.can_module_callback=DecodeDJIMotor;
CANRegister(&motor_instance->motor_can_instance);
dji_motor_info[idx++]=motor_instance;
}
void DJIMotorControl()
{
}

29
modules/motor/dji_motor.h
View File

@ -0,0 +1,29 @@
#ifndef DJI_MOTOR_H
#define DJI_MOTOR_H
#define DJI_MOTOR_CNT 8
#include "bsp_can.h"
#include "controller.h"
typedef struct
{
uint16_t ecd;
int16_t speed_rpm;
int16_t given_current;
uint8_t temperate;
int16_t last_ecd;
PID_t motor_pid;
can_instance motor_can_instance;
} dji_motor_instance;
void DJIMotorInit(dji_motor_instance* motor_instace,CAN_HandleTypeDef* _hcan,uint16_t tx_id,uint16_t rx_id);
void DJIMotorControl();
#endif // !DJI_MOTOR_H

31
modules/motor/motor_task.c Normal file
View File

@ -0,0 +1,31 @@
#include "motor_task.h"
static dji_motor_instance* dji_motor_info[DJI_MOTOR_CNT];
static joint_instance* joint_motor_info[HT_MOTOR_CNT];
static driven_instance* driven_motor_info[LK_MOTOR_CNT];
void RegisterMotor(Motor_Type_e motor_type,void* motor_instance)
{
static uint8_t dji_idx,joint_idx,driven_idx;
switch (motor_type)
{
case GM6020:
case M3508:
case M2006:
dji_motor_info[dji_idx++]=(dji_motor_instance*)motor_instance;
break;
case LK9025:
driven_motor_info[driven_idx++]=(driven_instance*)motor_instance;
break;
case HT04:
joint_motor_info[joint_idx++]=(joint_instance*)motor_instance;
break;
}
}
void MotorControlTask()
{
}

22
modules/motor/motor_task.h Normal file
View File

@ -0,0 +1,22 @@
#ifndef MOTOR_TASK_H
#define MOTOR_TASK_H
#include "LK9025.h"
#include "HT04.h"
#include "dji_motor.h"
typedef enum
{
GM6020=0,
M3508=1,
M2006=2,
LK9025=3,
HT04=4
} Motor_Type_e;
void MotorControlTask();
void RegisterMotor(Motor_Type_e motor_type,void* motor_instance);
#endif // !MOTOR_TASK_H