Skip to content

Commit

Permalink
motor handle var rename
Browse files Browse the repository at this point in the history
  • Loading branch information
jia-xie committed Apr 3, 2024
1 parent bd048cc commit 7167f95
Show file tree
Hide file tree
Showing 2 changed files with 13 additions and 13 deletions.
12 changes: 6 additions & 6 deletions src/devices/inc/dm_motor.h
Original file line number Diff line number Diff line change
Expand Up @@ -65,12 +65,12 @@ typedef struct _DM_Motor {

/* Motor Sensor Feedback */
DM_Motor_Stats_t *stats;
} DM_Motor_t;
} DM_Motor_Handle_t;

void DM_Motor_Disable_Motor(DM_Motor_t *motor);
void DM_Motor_Enable_Motor(DM_Motor_t *motor);
DM_Motor_t* DM_Motor_Init(DM_Motor_Config_t *config);
void DM_Motor_Ctrl_MIT(DM_Motor_t *motor, float target_pos, float target_vel, float torq);
void DM_Motor_Set_MIT_PD(DM_Motor_t *motor, float kp, float kd);
void DM_Motor_Disable_Motor(DM_Motor_Handle_t *motor);
void DM_Motor_Enable_Motor(DM_Motor_Handle_t *motor);
DM_Motor_Handle_t* DM_Motor_Init(DM_Motor_Config_t *config);
void DM_Motor_Ctrl_MIT(DM_Motor_Handle_t *motor, float target_pos, float target_vel, float torq);
void DM_Motor_Set_MIT_PD(DM_Motor_Handle_t *motor, float kp, float kd);

#endif
14 changes: 7 additions & 7 deletions src/devices/src/dm_motor.c
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
#include <stdlib.h>

#define DM_MAX_DEVICE (10)
DM_Motor_t *g_dm_motors[DM_MAX_DEVICE] = {NULL};
DM_Motor_Handle_t *g_dm_motors[DM_MAX_DEVICE] = {NULL};

/**
* int float_to_uint(float x, float x_min, float x_max, int bits)
Expand Down Expand Up @@ -53,7 +53,7 @@ void DM_Motor_Decode(CAN_Instance_t *motor_can_instance)
data_frame->pos = data_frame->pos_raw - data_frame->pos_offset;
}

void DM_Motor_Enable_Motor(DM_Motor_t *motor)
void DM_Motor_Enable_Motor(DM_Motor_Handle_t *motor)
{
CAN_Instance_t *motor_can_instance = motor->can_instance;
uint8_t *data = motor_can_instance->tx_buffer;
Expand All @@ -68,7 +68,7 @@ void DM_Motor_Enable_Motor(DM_Motor_t *motor)
CAN_Transmit(motor_can_instance);
}

void DM_Motor_Disable_Motor(DM_Motor_t *motor)
void DM_Motor_Disable_Motor(DM_Motor_Handle_t *motor)
{
CAN_Instance_t *motor_can_instance = motor->can_instance;
uint8_t *data = motor_can_instance->tx_buffer;
Expand All @@ -83,7 +83,7 @@ void DM_Motor_Disable_Motor(DM_Motor_t *motor)
CAN_Transmit(motor_can_instance);
}

void DM_Motor_Ctrl_MIT(DM_Motor_t *motor, float target_pos, float target_vel, float torq)
void DM_Motor_Ctrl_MIT(DM_Motor_Handle_t *motor, float target_pos, float target_vel, float torq)
{
uint16_t pos_temp, vel_temp, kp_temp, kd_temp, torq_temp;
CAN_Instance_t *motor_can_instance = motor->can_instance;
Expand All @@ -109,15 +109,15 @@ void DM_Motor_Ctrl_MIT(DM_Motor_t *motor, float target_pos, float target_vel, fl
CAN_Transmit(motor_can_instance);
}

void DM_Motor_Set_MIT_PD(DM_Motor_t *motor, float kp, float kd)
void DM_Motor_Set_MIT_PD(DM_Motor_Handle_t *motor, float kp, float kd)
{
motor->kp = kp;
motor->kd = kd;
}

DM_Motor_t* DM_Motor_Init(DM_Motor_Config_t *config)
DM_Motor_Handle_t* DM_Motor_Init(DM_Motor_Config_t *config)
{
DM_Motor_t *motor = malloc(sizeof(DM_Motor_t));
DM_Motor_Handle_t *motor = malloc(sizeof(DM_Motor_Handle_t));
motor->can_bus = config->can_bus;
motor->control_mode = config->control_mode;
motor->tx_id = config->tx_id;
Expand Down

0 comments on commit 7167f95

Please sign in to comment.