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 <[email protected]>
Northeastern-Electric-Racing · Dec 29, 2023 · f112826 · f112826
1 parent a4056f8
commit f112826
Show file tree

Hide file tree

Showing 10 changed files with 212 additions and 97 deletions.
diff --git 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
+#define CANID_OUTBOUND_MSG 0xA55
+#define CANID_TORQUE_MSG 0xBA115
diff --git a/Core/Inc/dti.h b/Core/Inc/dti.h
@@ -16,8 +16,6 @@
 #include <stdbool.h>
 #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
@@ -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
@@ -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
@@ -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
@@ -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 <string.h>
 #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, &current, msg.len);
-    queue_can_msg(msg);
+	/* Send CAN message */
+	memcpy(msg.data, &current, 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, &current, msg.len);
-    queue_can_msg(msg);
+	/* Send CAN message */
+	memcpy(msg.data, &current, 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, &current, msg.len);
-    queue_can_msg(msg);
+	/* Send CAN message */
+	memcpy(msg.data, &current, 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, &current, msg.len);
-    queue_can_msg(msg);
+	/* Send CAN message */
+	memcpy(msg.data, &current, 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, &current, msg.len);
-    queue_can_msg(msg);
+	/* Send CAN message */
+	memcpy(msg.data, &current, 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);
 }
diff --git 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 */