Skip to content

Commit

Permalink
(#32) fix mf motor potential multi thread conflict
Browse files Browse the repository at this point in the history
  • Loading branch information
@jia-xie
jia-xie committed Apr 5, 2024
1 parent 9013c9a commit 30922ca
Show file tree
Hide file tree
Showing 2 changed files with 63 additions and 36 deletions.
22 changes: 10 additions & 12 deletions src/devices/inc/mf_motor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@ typedef struct _MF_Motor
/* CAN Information */
uint8_t can_bus;
uint8_t control_mode;
uint8_t send_pending_flag;
uint16_t tx_id;
uint16_t rx_id;
CAN_Instance_t *can_instance;
Expand All @@ -62,22 +63,22 @@ void MF_Motor_Decode(CAN_Instance_t *can_instance);
MF_Motor_Handle_t *MF_Motor_Init(MF_Motor_Config_t config);

// TODO: document functions
HAL_StatusTypeDef MF_Motor_GetPIDParam(MF_Motor_Handle_t *motor);
void MF_Motor_GetPIDParam(MF_Motor_Handle_t *motor);

// TODO: document functions
HAL_StatusTypeDef MF_Motor_PIDToRam(MF_Motor_Handle_t *motor,
void MF_Motor_PIDToRam(MF_Motor_Handle_t *motor,
uint8_t kp_ang, uint8_t ki_ang,
uint8_t kp_vel, uint8_t ki_vel,
uint8_t kp_torq, uint8_t ki_torq);

// TODO: document functions
HAL_StatusTypeDef MF_Motor_EnableMotor(MF_Motor_Handle_t *motor);
void MF_Motor_EnableMotor(MF_Motor_Handle_t *motor);

// TODO: document functions
HAL_StatusTypeDef MF_Motor_DisableMotor(MF_Motor_Handle_t *motor);
void MF_Motor_DisableMotor(MF_Motor_Handle_t *motor);

// TODO: document functions
HAL_StatusTypeDef MF_Motor_PIDToRam(MF_Motor_Handle_t *motor,
void MF_Motor_PIDToRam(MF_Motor_Handle_t *motor,
uint8_t kp_ang, uint8_t ki_ang,
uint8_t kp_vel, uint8_t ki_vel,
uint8_t kp_torq, uint8_t ki_torq);
Expand All @@ -90,39 +91,36 @@ HAL_StatusTypeDef MF_Motor_PIDToRam(MF_Motor_Handle_t *motor,
* MF Motors: 2048 represents 16.5A
* MG Motors: 2048 represents 33A
* - Check motor datasheet for torque constant
* @return HAL_StatusTypeDef: HAL_OK if successful, HAL_ERROR otherwise
*/
HAL_StatusTypeDef MF_Motor_TorqueCtrl(MF_Motor_Handle_t *motor, int16_t torq);
void MF_Motor_TorqueCtrl(MF_Motor_Handle_t *motor, int16_t torq);

/**
* @brief Closed loop velocity control for MF motor.
* @param motor Pointer to the MF motor handle structure.
* @param vel Desired velocity setpoint for the motor.
* - unit: 0.01dps/LSB
* @return HAL_StatusTypeDef Returns HAL_OK if the command is transmitted successfully, HAL_ERROR otherwise.
*
*
* @note The function packages the velocity command into a CAN bus frame.
* DATA[0] is set to command identifier 0xA2.
* DATA[4] to DATA[7] are assigned the velocity value, split into 4 bytes.
* This function uses a local buffer to avoid multiple dereferences of the motor handle.
*
* @warning motor respond velocity in 1dps/LSB
*/
HAL_StatusTypeDef MF_Motor_VelocityCtrl(MF_Motor_Handle_t *motor, int32_t vel);
void MF_Motor_VelocityCtrl(MF_Motor_Handle_t *motor, int32_t vel);

/**
* @brief Closed loop position control for MF motor.
* @param motor Pointer to the MF motor handle structure.
* @param pos Desired position setpoint for the motor.
* - Unit: 0.01 degree/LSB (36000 represents 360 degrees)
* @return HAL_StatusTypeDef Returns HAL_OK if the command is transmitted successfully, HAL_ERROR otherwise.
*
* @note The function packages the position command into a CAN bus frame.
* DATA[0] is set to the command identifier 0xA3.
* DATA[4] to DATA[7] are assigned the position value, split into 4 bytes.
* This function uses a local buffer to avoid multiple dereferences of the motor handle and initializes
* the buffer to zero before setting the command identifier and position value.
*/
HAL_StatusTypeDef MF_Motor_PositionCtrl(MF_Motor_Handle_t *motor, int32_t pos);
void MF_Motor_PositionCtrl(MF_Motor_Handle_t *motor, int32_t pos);

#endif
77 changes: 53 additions & 24 deletions src/devices/src/mf_motor.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,17 +3,23 @@
#include "user_math.h"
#include <stdlib.h>

#pragma message "Check Max Device Number"
#define MF_MAX_DEVICE (2)
MF_Motor_Handle_t *g_mf_motors[MF_MAX_DEVICE] = {NULL};
uint8_t g_mf_motor_num = 0;

void MF_Motor_Decode(CAN_Instance_t *can_instance);

MF_Motor_Handle_t* MF_Motor_Init(MF_Motor_Config_t config) {
MF_Motor_Handle_t *MF_Motor_Init(MF_Motor_Config_t config)
{
MF_Motor_Handle_t *motor = malloc(sizeof(MF_Motor_Handle_t));
motor->can_bus = config.can_bus;
motor->control_mode = config.control_mode;
motor->tx_id = config.tx_id;
motor->rx_id = config.rx_id;
motor->stats = malloc(sizeof(MF_Motor_Stats_t));
motor->can_instance = CAN_Device_Register(config.can_bus, config.tx_id, config.rx_id, MF_Motor_Decode);
motor->can_instance->binding_motor_stats = (void*) motor->stats;
motor->can_instance->binding_motor_stats = (void *)motor->stats;
motor->target_pos = 0;
motor->target_vel = 0;
motor->target_torq = 0;
Expand Down Expand Up @@ -64,37 +70,43 @@ void MF_Motor_Decode(CAN_Instance_t *can_instance)
}
}

HAL_StatusTypeDef MF_Motor_EnableMotor(MF_Motor_Handle_t *motor)
void MF_Motor_EnableMotor(MF_Motor_Handle_t *motor)
{
// store local pointer to avoid multiple dereference
uint8_t *tx_buffer = motor->can_instance->tx_buffer;
tx_buffer[0] = 0x88;
memset(&tx_buffer[1], 0, 7);
return CAN_Transmit(motor->can_instance);

// set the flag to send the data
motor->send_pending_flag = 1;
}

HAL_StatusTypeDef MF_Motor_DisableMotor(MF_Motor_Handle_t *motor)
void MF_Motor_DisableMotor(MF_Motor_Handle_t *motor)
{
// store local pointer to avoid multiple dereference
uint8_t *tx_buffer = motor->can_instance->tx_buffer;
tx_buffer[0] = 0x80;
memset(&tx_buffer[1], 0, 7);
return CAN_Transmit(motor->can_instance);

// set the flag to send the data
motor->send_pending_flag = 1;
}

HAL_StatusTypeDef MF_Motor_GetPIDParam(MF_Motor_Handle_t *motor)
void MF_Motor_GetPIDParam(MF_Motor_Handle_t *motor)
{
// store local pointer to avoid multiple dereference
uint8_t *tx_buffer = motor->can_instance->tx_buffer;
tx_buffer[0] = 0x30;
memset(&tx_buffer[1], 0, 7);
return CAN_Transmit(motor->can_instance);

// set the flag to send the data
motor->send_pending_flag = 1;
}

HAL_StatusTypeDef MF_Motor_PIDToRam(MF_Motor_Handle_t *motor,
uint8_t kp_ang, uint8_t ki_ang,
uint8_t kp_vel, uint8_t ki_vel,
uint8_t kp_torq, uint8_t ki_torq)
void MF_Motor_PIDToRam(MF_Motor_Handle_t *motor,
uint8_t kp_ang, uint8_t ki_ang,
uint8_t kp_vel, uint8_t ki_vel,
uint8_t kp_torq, uint8_t ki_torq)
{
// store local pointer to avoid multiple dereference
uint8_t *tx_buffer = motor->can_instance->tx_buffer;
Expand All @@ -106,7 +118,9 @@ HAL_StatusTypeDef MF_Motor_PIDToRam(MF_Motor_Handle_t *motor,
tx_buffer[5] = ki_vel;
tx_buffer[6] = kp_torq;
tx_buffer[7] = ki_torq;
return CAN_Transmit(motor->can_instance);

// set the flag to send the data
motor->send_pending_flag = 1;
}

/**
Expand All @@ -117,9 +131,8 @@ HAL_StatusTypeDef MF_Motor_PIDToRam(MF_Motor_Handle_t *motor,
* MF Motors: 2048 represents 16.5A
* MG Motors: 2048 represents 33A
* - Check motor datasheet for torque constant
* @return HAL_StatusTypeDef: HAL_OK if successful, HAL_ERROR otherwise
*/
HAL_StatusTypeDef MF_Motor_TorqueCtrl(MF_Motor_Handle_t *motor, int16_t torq)
*/
void MF_Motor_TorqueCtrl(MF_Motor_Handle_t *motor, int16_t torq)
{
__MAX_LIMIT(torq, -2048, 2048);
// store local pointer to avoid multiple dereference
Expand All @@ -136,24 +149,25 @@ HAL_StatusTypeDef MF_Motor_TorqueCtrl(MF_Motor_Handle_t *motor, int16_t torq)
tx_buffer[0] = 0xA1;
tx_buffer[4] = torq & 0xFF;
tx_buffer[5] = (torq >> 8) & 0xFF;
return CAN_Transmit(motor->can_instance);

// set the flag to send the data
motor->send_pending_flag = 1;
}

/**
* @brief Closed loop velocity control for MF motor.
* @param motor Pointer to the MF motor handle structure.
* @param vel Desired velocity setpoint for the motor.
* - unit: 0.01dps/LSB
* @return HAL_StatusTypeDef Returns HAL_OK if the command is transmitted successfully, HAL_ERROR otherwise.
*
* @note The function packages the velocity command into a CAN bus frame.
* DATA[0] is set to command identifier 0xA2.
* DATA[4] to DATA[7] are assigned the velocity value, split into 4 bytes.
* This function uses a local buffer to avoid multiple dereferences of the motor handle.
*
*
* @warning motor respond velocity in 1dps/LSB
*/
HAL_StatusTypeDef MF_Motor_VelocityCtrl(MF_Motor_Handle_t *motor, int32_t vel)
void MF_Motor_VelocityCtrl(MF_Motor_Handle_t *motor, int32_t vel)
{
// store local pointer to avoid multiple dereference
uint8_t *tx_buffer = motor->can_instance->tx_buffer;
Expand All @@ -171,23 +185,24 @@ HAL_StatusTypeDef MF_Motor_VelocityCtrl(MF_Motor_Handle_t *motor, int32_t vel)
tx_buffer[5] = (vel >> 8) & 0xFF;
tx_buffer[6] = (vel >> 16) & 0xFF;
tx_buffer[7] = (vel >> 24) & 0xFF;
return CAN_Transmit(motor->can_instance);

// set the flag to send the data
motor->send_pending_flag = 1;
}

/**
* @brief Closed loop position control for MF motor.
* @param motor Pointer to the MF motor handle structure.
* @param pos Desired position setpoint for the motor.
* - Unit: 0.01 degree/LSB (36000 represents 360 degrees)
* @return HAL_StatusTypeDef Returns HAL_OK if the command is transmitted successfully, HAL_ERROR otherwise.
*
* @note The function packages the position command into a CAN bus frame.
* DATA[0] is set to the command identifier 0xA3.
* DATA[4] to DATA[7] are assigned the position value, split into 4 bytes.
* This function uses a local buffer to avoid multiple dereferences of the motor handle and initializes
* the buffer to zero before setting the command identifier and position value.
*/
HAL_StatusTypeDef MF_Motor_PositionCtrl(MF_Motor_Handle_t *motor, int32_t pos)
void MF_Motor_PositionCtrl(MF_Motor_Handle_t *motor, int32_t pos)
{
// store local pointer to avoid multiple dereference
uint8_t *tx_buffer = motor->can_instance->tx_buffer;
Expand All @@ -207,5 +222,19 @@ HAL_StatusTypeDef MF_Motor_PositionCtrl(MF_Motor_Handle_t *motor, int32_t pos)
tx_buffer[5] = (pos >> 8) & 0xFF;
tx_buffer[6] = (pos >> 16) & 0xFF;
tx_buffer[7] = (pos >> 24) & 0xFF;
return CAN_Transmit(motor->can_instance);

// set the flag to send the data
motor->send_pending_flag = 1;
}

void MF_Motor_Send(void)
{
for (int i = 0; i < g_mf_motor_num; i++) // loop through all the motors
{
if (g_mf_motors[i]->send_pending_flag)
{ // check if the flag is set
CAN_Transmit(g_mf_motors[i]->can_instance); // send the data
g_mf_motors[i]->send_pending_flag = 0; // clear the flag
}
}
}

0 comments on commit 30922ca

Please sign in to comment.