From f112826593020c8aba39e16f8f1ff682b1d1b227 Mon Sep 17 00:00:00 2001 From: Nicholas DePatie <80368116+nwdepatie@users.noreply.github.com> Date: Thu, 28 Dec 2023 21:13:18 -0500 Subject: [PATCH] Create task to send motor torque to motor controller (#110) * convert accel and queue torque msg * adding in emrax motor parameters for torque calculation based on AC current --------- Co-authored-by: Scott A <89099102+Scotab@users.noreply.github.com> --- Core/Inc/cerberus_conf.h | 3 +- Core/Inc/dti.h | 34 +++--------- Core/Inc/emrax.h | 33 ++++++++++++ Core/Inc/queues.h | 4 +- Core/Inc/torque.h | 21 ++++++++ Core/Src/dti.c | 113 +++++++++++++++++++++------------------ Core/Src/main.c | 10 ++-- Core/Src/monitor.c | 22 ++++---- Core/Src/torque.c | 68 +++++++++++++++++++++++ Makefile | 1 + 10 files changed, 212 insertions(+), 97 deletions(-) create mode 100644 Core/Inc/emrax.h create mode 100644 Core/Inc/torque.h create mode 100644 Core/Src/torque.c diff --git a/Core/Inc/cerberus_conf.h b/Core/Inc/cerberus_conf.h index a18ce02f..efed97e1 100644 --- a/Core/Inc/cerberus_conf.h +++ b/Core/Inc/cerberus_conf.h @@ -30,4 +30,5 @@ //TODO: GET CORRECT CAN ID #define CANID_IMU 0xDEAD #define CANID_TEMP_SENSOR 0xBEEF -#define CANID_OUTBOUND_MSG 0xA55 \ No newline at end of file +#define CANID_OUTBOUND_MSG 0xA55 +#define CANID_TORQUE_MSG 0xBA115 \ No newline at end of file diff --git a/Core/Inc/dti.h b/Core/Inc/dti.h index 1acd3602..420f8777 100644 --- a/Core/Inc/dti.h +++ b/Core/Inc/dti.h @@ -16,8 +16,6 @@ #include #include "can_handler.h" -#define NUM_POLES 4 // TODO: Find num poles? - typedef struct { int32_t rpm; /* SCALE: 1 UNITS: Rotations per Minute */ int16_t duty_cycle; /* SCALE: 10 UNITS: Percentage */ @@ -32,8 +30,16 @@ typedef struct { int8_t drive_enable; /* SCALE: 1 UNITS: No units just a number */ } dti_t; +//TODO: Expand GET interface + void dti_init(dti_t* mc); +/* + * SCALE: 10 + * UNITS: Nm + */ +void dti_set_torque(int16_t torque); + /* * SCALE: bool * UNITS: No units @@ -82,30 +88,6 @@ void dti_set_relative_brake_current(int16_t relative_brake_current); */ void dti_set_digital_output(uint8_t output, bool value); -/* - * SCALE: 10 - * UNITS: Amps - */ -void dti_set_max_ac_current(int16_t current); - -/* - * SCALE: 10 - * UNITS: Amps - */ -void dti_set_max_ac_brake_current(int16_t current); - -/* - * SCALE: 10 - * UNITS: Amps - */ -void dti_set_max_dc_current(int16_t current); - -/* - * SCALE: 10 - * UNITS: Amps - */ -void dti_set_max_dc_brake_current(int16_t current); - /* * SCALE: bool * UNITS: No units diff --git a/Core/Inc/emrax.h b/Core/Inc/emrax.h new file mode 100644 index 00000000..94577299 --- /dev/null +++ b/Core/Inc/emrax.h @@ -0,0 +1,33 @@ +/** + * @file emrax.h + * @author Nick DePatie + * @brief Config file to hold some important values for Emrax 228-MV motor + * @version 0.1 + * @date 2023-12-28 + * + * @copyright Copyright (c) 2023 + * + */ + +/* + * Values retrieved from datasheet + * Note: Emrax is Liquid Cooled + * Note: We are using an encoder for position data + */ +#define EMRAX_MAX_MOTOR_TEMP 120 /* degC */ +#define EMRAX_NOMINAL_VOLTAGE 520 /* V */ +#define EMRAX_PEAK_EFFICIENCY 90 /* % */ +#define EMRAX_PEAK_POWER 124 /* kW, at 5500 RPM */ +#define EMRAX_PEAK_TORQUE 230 /* Nm */ +#define EMRAX_CONT_TORQUE 112 /* Nm */ +#define EMRAX_LIMITING_SPEED 6500 /* RPM */ +#define EMRAX_KV 15.53 +#define EMRAX_KT 0.61 +#define EMRAX_PEAK_CURRENT 380 /* A_RMS */ +#define EMRAX_CONT_CURRENT 180 /* A_RMS */ +#define EMRAX_INTERN_PHASE_RES 7.06 /* mOhm, at 25 degC */ +#define EMRAX_INDUCTION_2PHASE 96.5 /* mHenry */ +#define EMRAX_INDUCED_VOLTAGE 0.04793 /* V_RMS / RPM */ +#define EMRAX_NUM_POLE_PAIRS 10 +#define EMRAX_MOTOR_INTERTIA 0.02521 /* kg * m^2 */ +#define EMRAX_WEIGHT 13.5 /* kg */ diff --git a/Core/Inc/queues.h b/Core/Inc/queues.h index 8e26e196..8436c5ad 100644 --- a/Core/Inc/queues.h +++ b/Core/Inc/queues.h @@ -27,8 +27,8 @@ typedef struct { extern osMessageQueueId_t imu_queue; typedef struct { - uint16_t brakeValue; - uint16_t acceleratorValue; + uint16_t brake_value; + uint16_t accelerator_value; } pedals_t; extern osMessageQueueId_t pedal_data_queue; diff --git a/Core/Inc/torque.h b/Core/Inc/torque.h new file mode 100644 index 00000000..7109d7a5 --- /dev/null +++ b/Core/Inc/torque.h @@ -0,0 +1,21 @@ +/** + * @file torque.h + * @author Scott Abramson + * @brief Convert pedal acceleration to torque and queue message with calcualted torque + * @version 1.69 + * @date 2023-11-09 + * + * @copyright Copyright (c) 2023 + * + */ + +#ifndef TORQUE_H +#define TORQUE_H + +#include "cmsis_os.h" + +void vCalcTorque(void* pv_params); +extern osThreadId_t torque_calc_handle; +extern const osThreadAttr_t torque_calc_attributes; + +#endif diff --git a/Core/Src/dti.c b/Core/Src/dti.c index d09d9bf7..420eb4f8 100644 --- a/Core/Src/dti.c +++ b/Core/Src/dti.c @@ -1,6 +1,6 @@ /** * @file dti.c - * @author Hamza Iqbal + * @author Hamza Iqbal and Nick DePatie * @brief Source file for Motor Controller Driver * @version 0.1 * @date 2023-08-22 @@ -12,132 +12,143 @@ #include #include "dti.h" #include "can.h" +#include "emrax.h" void dti_init(dti_t* mc) { + //TODO: Set safety parameters for operation (maxes, mins, etc) + //dti_set_max_ac_brake_current(); + //dti_set_max_ac_current(); + //dti_set_max_dc_brake_current(); + //dti_set_max_dc_current(); +} +void dti_set_torque(int16_t torque) +{ + int16_t ac_current = (torque / EMRAX_KT); /* times 10 */ + dti_set_current(ac_current); } void dti_set_current(int16_t current) { - can_msg_t msg + can_msg_t msg = { .id = 0x00000110, .len = 2, .data = { 0 } }; - /* Send CAN message */ - memcpy(msg.data, ¤t, msg.len); - queue_can_msg(msg); + /* Send CAN message */ + memcpy(msg.data, ¤t, msg.len); + queue_can_msg(msg); } void dti_set_brake_current(int16_t brake_current) { - can_msg_t msg + can_msg_t msg = { .id = 0x00000210, .len = 2, .data = { 0 } }; - /* Send CAN message */ - memcpy(msg.data, &brake_current, msg.len); - queue_can_msg(msg); + /* Send CAN message */ + memcpy(msg.data, &brake_current, msg.len); + queue_can_msg(msg); } void dti_set_speed(int32_t rpm) { - can_msg_t msg + can_msg_t msg = { .id = 0x00000310, .len = 4, .data = { 0 } }; - rpm = rpm * NUM_POLES; + rpm = rpm * EMRAX_NUM_POLE_PAIRS; - /* Send CAN message */ - memcpy(msg.data, &rpm, msg.len); - queue_can_msg(msg); + /* Send CAN message */ + memcpy(msg.data, &rpm, msg.len); + queue_can_msg(msg); } void dti_set_position(int16_t angle) { - can_msg_t msg + can_msg_t msg = { .id = 0x00000410, .len = 2, .data = { 0 } }; - /* Send CAN message */ - memcpy(msg.data, &angle, msg.len); - queue_can_msg(msg); + /* Send CAN message */ + memcpy(msg.data, &angle, msg.len); + queue_can_msg(msg); } void dti_set_relative_current(int16_t relative_current) { - can_msg_t msg + can_msg_t msg = { .id = 0x00000510, .len = 2, .data = { 0 } }; - /* Send CAN message */ - memcpy(msg.data, &relative_current, msg.len); - queue_can_msg(msg); + /* Send CAN message */ + memcpy(msg.data, &relative_current, msg.len); + queue_can_msg(msg); } void dti_set_relative_brake_current(int16_t relative_brake_current) { - can_msg_t msg + can_msg_t msg = { .id = 0x00000610, .len = 2, .data = { 0 } }; - /* Send CAN message */ - memcpy(msg.data, &relative_brake_current, msg.len); - queue_can_msg(msg); + /* Send CAN message */ + memcpy(msg.data, &relative_brake_current, msg.len); + queue_can_msg(msg); } void dti_set_digital_output(uint8_t output, bool value) { - can_msg_t msg + can_msg_t msg = { .id = 0x00000710, .len = 1, .data = { 0 } }; - uint8_t ctrl = value >> output; + uint8_t ctrl = value >> output; - /* Send CAN message */ - memcpy(msg.data, &ctrl, msg.len); - queue_can_msg(msg); + /* Send CAN message */ + memcpy(msg.data, &ctrl, msg.len); + queue_can_msg(msg); } void dti_set_max_ac_current(int16_t current) { - can_msg_t msg + can_msg_t msg = { .id = 0x00000810, .len = 2, .data = { 0 } }; - /* Send CAN message */ - memcpy(msg.data, ¤t, msg.len); - queue_can_msg(msg); + /* Send CAN message */ + memcpy(msg.data, ¤t, msg.len); + queue_can_msg(msg); } void dti_set_max_ac_brake_current(int16_t current) { - can_msg_t msg + can_msg_t msg = { .id = 0x00000910, .len = 2, .data = { 0 } }; - /* Send CAN message */ - memcpy(msg.data, ¤t, msg.len); - queue_can_msg(msg); + /* Send CAN message */ + memcpy(msg.data, ¤t, msg.len); + queue_can_msg(msg); } void dti_set_max_dc_current(int16_t current) { - can_msg_t msg + can_msg_t msg = { .id = 0x00000A10, .len = 2, .data = { 0 } }; - /* Send CAN message */ - memcpy(msg.data, ¤t, msg.len); - queue_can_msg(msg); + /* Send CAN message */ + memcpy(msg.data, ¤t, msg.len); + queue_can_msg(msg); } void dti_set_max_dc_brake_current(int16_t current) { - can_msg_t msg + can_msg_t msg = { .id = 0x00000B10, .len = 2, .data = { 0 } }; - /* Send CAN message */ - memcpy(msg.data, ¤t, msg.len); - queue_can_msg(msg); + /* Send CAN message */ + memcpy(msg.data, ¤t, msg.len); + queue_can_msg(msg); } void dti_set_drive_enable(bool drive_enable) { - can_msg_t msg + can_msg_t msg = { .id = 0x00000C10, .len = 1, .data = { 0 } }; - /* Send CAN message */ - memcpy(msg.data, &drive_enable, msg.len); - queue_can_msg(msg); + /* Send CAN message */ + memcpy(msg.data, &drive_enable, msg.len); + queue_can_msg(msg); } \ No newline at end of file diff --git a/Core/Src/main.c b/Core/Src/main.c index a97b6a23..b641214d 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -31,6 +31,7 @@ #include "can_handler.h" #include "serial_monitor.h" #include "state_machine.h" +#include "torque.h" /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ @@ -201,15 +202,16 @@ int main(void) defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes); /* USER CODE BEGIN RTOS_THREADS */ - //temp_monitor_handle = osThreadNew(vTempMonitor, &hi2c1, &temp_monitor_attributes); + temp_monitor_handle = osThreadNew(vTempMonitor, &hi2c1, &temp_monitor_attributes); watchdog_monitor_handle = osThreadNew(vWatchdogMonitor, GPIOB, &watchdog_monitor_attributes); - //imu_monitor_handle = osThreadNew(vIMUMonitor, &hi2c1, &imu_monitor_attributes); + imu_monitor_handle = osThreadNew(vIMUMonitor, &hi2c1, &imu_monitor_attributes); serial_monitor_handle = osThreadNew(vSerialMonitor, NULL, &serial_monitor_attributes); - //fault_handle = osThreadNew(vFaultHandler, NULL, &fault_handle_attributes); + fault_handle = osThreadNew(vFaultHandler, NULL, &fault_handle_attributes); pedals_monitor_handle = osThreadNew(vPedalsMonitor, pedal_params, &pedals_monitor_attributes); //route_can_incoming_handle = osThreadNew(vRouteCanIncoming, &hcan1, &route_can_incoming_attributes); can_dispatch_handle = osThreadNew(vCanDispatch, &hcan1, &can_dispatch_attributes); - //sm_director_handle = osThreadNew(vStateMachineDirector, NULL, &sm_director_attributes); + sm_director_handle = osThreadNew(vStateMachineDirector, NULL, &sm_director_attributes); + torque_calc_handle = osThreadNew(vCalcTorque, NULL, &torque_calc_attributes); /* USER CODE END RTOS_THREADS */ diff --git a/Core/Src/monitor.c b/Core/Src/monitor.c index 812e9176..7da451a5 100644 --- a/Core/Src/monitor.c +++ b/Core/Src/monitor.c @@ -40,7 +40,7 @@ void vTempMonitor(void* pv_params) for (;;) { /* Take measurement */ - //serial_print("Temp Sensor Task\r\n"); + serial_print("Temp Sensor Task\r\n"); //if (sht30_get_temp_humid(&temp_sensor)) { // fault_data.diag = "Failed to get temp"; // queue_fault(&fault_data); @@ -98,7 +98,7 @@ void vPedalsMonitor(void* pv_params) { const uint8_t num_samples = 10; enum { ACCELPIN_1, ACCELPIN_2, BRAKEPIN_1, BRAKEPIN_2 }; - const uint16_t delay_time = 15; /* ms */ + const uint16_t delay_time = 5; /* ms */ const uint16_t adc_sample_time = 2; /* ms */ const uint8_t can_msg_len = 4; /* bytes */ @@ -117,9 +117,10 @@ void vPedalsMonitor(void* pv_params) uint16_t adc_data[4]; - for (;;) { - serial_print("Pedals Task\r\n"); + uint32_t curr_tick = HAL_GetTick(); + for (;;) { + //serial_print("Pedals Task: %d\r\n", HAL_GetTick() - curr_tick); /* * Get the value from the adc at the brake and accelerator * pin addresses and average them to the sensor data value @@ -131,22 +132,17 @@ void vPedalsMonitor(void* pv_params) /* Yield to other tasks */ osDelay(delay_time); - //uint32_t adc_err; - - //adc_errs = HAL_ADC_PollForConversion(params->accel_adc1, HAL_MAX_DELAY); - //adc_err <<= 2; - HAL_StatusTypeDef err = HAL_ADC_PollForConversion(params->accel_adc1, HAL_MAX_DELAY); if (!err) - serial_print("Accel 1: %d\r\n", HAL_ADC_GetValue(params->accel_adc1)); + HAL_ADC_GetValue(params->accel_adc1); err = HAL_ADC_PollForConversion(params->accel_adc2, HAL_MAX_DELAY); if (!err) - serial_print("Accel 2: %d\r\n", HAL_ADC_GetValue(params->accel_adc2)); + HAL_ADC_GetValue(params->accel_adc2); err = HAL_ADC_PollForConversion(params->brake_adc, HAL_MAX_DELAY); if (!err) - serial_print("Brake 1: %d\r\n", HAL_ADC_GetValue(params->brake_adc)); + HAL_ADC_GetValue(params->brake_adc); //err = HAL_ADC_PollForConversion(params->brake_adc, HAL_MAX_DELAY); //if (!err) @@ -239,7 +235,7 @@ void vIMUMonitor(void *pv_params) //} for(;;) { - //serial_print("IMU Task\r\n"); + serial_print("IMU Task\r\n"); /* Take measurement */ //if (lsm6dso_read_accel(&imu)) { // fault_data.diag = "Failed to get IMU acceleration"; diff --git a/Core/Src/torque.c b/Core/Src/torque.c new file mode 100644 index 00000000..61b51607 --- /dev/null +++ b/Core/Src/torque.c @@ -0,0 +1,68 @@ +/** + * @file torque.c + * @author Scott Abramson + * @brief Convert pedal acceleration to torque and queue message with calcualted torque + * @version 1.69 + * @date 2023-11-09 + * + * @copyright Copyright (c) 2023 + * + */ + +#include +#include "can_handler.h" +#include "queues.h" +#include "cerberus_conf.h" +#include "dti.h" + +#define MAX_TORQUE 10 /* Nm */ + +//TODO: Might want to make these more dynamic to account for MechE tuning +#define MIN_PEDAL_VAL 2 /* Raw ADC */ +#define MAX_PEDAL_VAL 2 /* Raw ADC */ + +/* DO NOT ATTEMPT TO SEND TORQUE COMMANDS LOWER THAN THIS VALUE */ +#define MIN_COMMAND_FREQ 60 /* Hz */ +#define MAX_COMMAND_DELAY 1000 / MIN_COMMAND_FREQ /* ms */ + +osThreadId_t torque_calc_handle; +const osThreadAttr_t torque_calc_attributes = { + .name = "SendTorque", + .stack_size = 128 * 8, + .priority = (osPriority_t)osPriorityAboveNormal4 +}; + +void vCalcTorque(void* pv_params) { + + const uint16_t delay_time = 5; /* ms */ + + /* End application if we try to update motor at freq below this value */ + assert(delay_time < MAX_COMMAND_DELAY); + + pedals_t pedal_data; + uint16_t torque; + osStatus_t stat; + dti_t mc; + + dti_init(&mc); + + for (;;) { + stat = osMessageQueueGet(pedal_data_queue, &pedal_data, 0U, delay_time); + + /* If we receive a new message within the time frame, calc new torque */ + if (stat == osOK) { + // TODO: Add state based torque calculation + + uint16_t accel = pedal_data.accelerator_value; + + if (accel < MIN_PEDAL_VAL) + torque = 0; + else + /* Linear scale of torque */ + torque = (MAX_TORQUE / MAX_PEDAL_VAL) * accel - MIN_PEDAL_VAL; + } + + /* Send whatever torque command we have on record */ + //dti_set_torque(torque); + } +} \ No newline at end of file diff --git a/Makefile b/Makefile index 22fc431f..5faf9f6b 100644 --- a/Makefile +++ b/Makefile @@ -52,6 +52,7 @@ Core/Src/can_handler.c \ Core/Src/serial_monitor.c \ Core/Src/dti.c \ Core/Src/state_machine.c \ +Core/Src/torque.c \ Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c \ Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c \ Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc_ex.c \