From 93b6b2728229c5be3927d1a49a3c84027c976b2b Mon Sep 17 00:00:00 2001 From: Prithika Tharan Date: Tue, 10 Mar 2026 17:31:48 -0400 Subject: [PATCH 1/4] created intro project skeleton --- .../PrithikaTharan/intro_project.cpp | 373 ++++++++++++++++++ 1 file changed, 373 insertions(+) create mode 100644 intro_projects/PrithikaTharan/intro_project.cpp diff --git a/intro_projects/PrithikaTharan/intro_project.cpp b/intro_projects/PrithikaTharan/intro_project.cpp new file mode 100644 index 0000000..a5a7191 --- /dev/null +++ b/intro_projects/PrithikaTharan/intro_project.cpp @@ -0,0 +1,373 @@ +#include +#include +#include +#include + +/* +The car has three possible functional states: +1. Low Voltage (LV) +2. Tractive System (TS) +3. Ready to Drive (RTD) + +When in LV, the car cannot drive. When in TS, the car is energized, but still +cannot drive. When in RTD, the car is energized and able to drive. + +There are two Accumulator Isolation Relays (AIRs) that connect the high voltage +(HV) battery to the rest of the car. One is on the positive terminal, and the +other on the negative terminal (AIR+ and AIR-). That is to say, the battery is +disconnected unless we energize both relays, which is important for safety. +Whenever we have a critical error, we can open these relays / de-energize them +to make sure there is no HV outside the accumulator. + +When we go from LV -> TS, we cannot simply energize the relays (ask an +electrical lead why!). Instead we first close AIR-, then another relay called +the "Precharge Relay". After ~5 seconds we close AIR+ and open the precharge +relay. + +There is a TS On button and an RTD button on the dash that move the cars into +those states. ie. When you press TS On the car goes TS, and when you press RTD, +it goes to RTD. They are both active low, meaning when the button's are pressed, +the voltage goes low. + +We also have a few sensors that we need to be able to calculate how much torque +to command: + +1. Current sensor (How much current there is in the HV path) +2. Wheelspeeds +3. Steering Angle Sensor + +The current sensor is a Hall Effect sensor read by an ADC through GPIO pins. The +conversion is: 1 amp per 10mV + +Wheelspeeds are a bit tricky. The sensor is routed to a GPIO pin, and is +normally high. However, the wheels have a gear looking thing with 17 teeth -- +everytime one of the teeth passes the sensor, the voltage at the GPIO pin goes +low. You can derive the RPM of the wheel from how often the voltage goes low. + +The steering angle sensor comes to us via CAN. + +We also have to monitor our battery to make sure nothing explodes or catches on +fire... Thankfully we have a Battery Management System (BMS) that monitors cell +voltages and temperatures and can send them to us. It sends this info via SPI. +If any cell voltage goes below 2.8V or above 4.3V, de-energize the car. Also, if +any temperature exceeds 60 degrees, de-energize. + +Finally, we have an LCD on the dash which is useful to display information. You +should print all relevant info to the screen so we know what's going on with the +car. This also operates via SPI. + +Mock library functions have been provided where necessary (marked with extern). + +NOTE - EXTREMELY IMPORTANT: the CAN methods `can_send` and `can_receive` are NOT +thread safe. + +Also think about the implications of having two peripherals on one SPI bus. + +---------------------------- + +Requirements: + +1. Command each motor with an appropriate torque every 1ms +2. Print sensor values and torque values to the LCD every 100ms +3. Monitor the highest/lowest voltage and highest temperature from the BMS and +shutdown the car if there is any unsafe condition + +---------------------------- + +Pins: + +TS On: 12 +RTD: 13 +Current Sensor: 19 +CAN RX: 2 +CAN TX: 3 +MOSI: 5 +MISO: 6 +SCK: 7 +LCD_CS: 8 +BMS_CS: 9 +AIR+: 22 +Precharge: 23 +AIR-: 10 + +---------------------------- + +"Submission" instructions. I want you all to get familiar with git, so we will +do this project with git. The repo is +https://github.com/UTFR/firmware-tutorials. + +If you still haven't joined the github organization, let me know and i'll add +you. Clone the repository, and make a branch called `/intro_project`. +Copy this file into the directory `intro_projects/` and make all your +changes there. + +Whenever you add a feature, `git add` the files, and `git commit -m "..."` with +a useful/descriptive message. Whenver you want your changes to be public, do +`git push origin /intro_project`. + +Also, I won't enforce it for this project, but for the actual firmware repo we +have a code formatter (clang-format). It means that everyone's code will look +exactly the same, in terms of how much whitespace there is and other aesthetics +like that. It's not there for aesthetics, but moreso so that you when people +make changes you can see exactly what they changed, whereas without a formatter, +you end up with lots of useless formatting changes. You should install +clang-format as soon as possible and set it up :) +*/ + +constexpr int TS_ON_PIN = 12; +constexpr int RTD_PIN = 13; +constexpr int CURRENT_SENSOR_PIN = 19; +constexpr int CAN_RX_PIN = 2; +constexpr int CAN_TX_PIN = 3; +constexpr int MOSI_PIN = 5; +constexpr int MISO_PIN = 6; +constexpr int SCK_PIN = 7; +constexpr int LCD_CS_PIN = 8; +constexpr int BMS_CS_PIN = 9; +constexpr int AIR_PLUS_PIN = 22; +constexpr int PRECHARGE_PIN = 23; +constexpr int AIR_MINUS_PIN = 10; +constexpr int WHEELSPEED_PIN = 11; // 11 ?? + +constexpr int BATTERIES_COUNT = 138; + +QueueHandle_t current_queue; + +volatile unsigned long wheel_speed_interval = 0; +volatile unsigned long wheel_speed_last_pulse = 0; + +QueueHandle_t steering_angle_queue; + +volatile float bms_voltage_min = 0.0f; +volatile float bms_voltage_max = 0.0f; +volatile float bms_temperature_max = 0.0f; +SemaphoreHandle_t bms_mutex; + +QueueHandle_t torques_queue; + +SemaphoreHandle_t can_mutex; +SemaphoreHandle_t spi_mutex; + +constexpr int TaskPriority_LCD = 0; +constexpr int TaskPriority_SENSOR = 1; +constexpr int TaskPriority_BMS = 2; + +/* + Initialize the CAN peripheral with given RX and TX pins at a given baudrate. +*/ +extern void can_init(uint8_t rx, uint8_t tx, uint32_t baudrate); + +/* + Send a CAN message with a given id. + The 8 byte payload is encoded as a uint64_t +*/ +extern void can_send(uint8_t id, uint64_t payload); +/* + Receive a CAN message with a given id into a uint64_t +*/ +extern void can_receive(uint64_t *payload, uint8_t id); + +/* + Calculates four torques, in order, for the Front Left, Front Right, Rear Left, + and Rear Right motors given current, wheelspeeds (in the same order), and a + steering angle. +*/ +extern void calculate_torque_cmd( + float *torques, float current, float *wheelspeeds, float steering_angle +); + +/* + Initialize the LCD peripheral +*/ +extern void lcd_init(uint8_t mosi, uint8_t miso, uint8_t sck, uint8_t lcs_cs); + +/* + Print something to the LCD +*/ +extern void lcd_printf(const char *fmt, ...); + +/* + Initialize the BMS +*/ +extern void bms_init(uint8_t mosi, uint8_t miso, uint8_t sck, uint8_t lcs_cs); + +/* + Get voltage of the nth cell in the battery +*/ +extern float bms_get_voltage(uint8_t n); + +/* + Get temperature of the nth cell in the battery +*/ +extern float bms_get_temperature(uint8_t n); + +float sensor_current_read() { + static const float ADC_VREF_V = 3.3f; + static const float ADC_RESOLUTION = 1024.0f; + static const float ADC_CONVERSION = 0.01f; + + int raw = analogRead(CURRENT_SENSOR_PIN); + + float voltage_V = (raw * ADC_VREF_V) / ADC_RESOLUTION; + float current_A = voltage_V / ADC_CONVERSION; + + return current_A; +} + +void sensor_current_update(void *parameters) { + TickType_t last_wake_time = xTaskGetTickCount(); + const TickType_t frequency = pdMS_TO_TICKS(1); + + while (1) { + float new_current = sensor_current_read(); + xQueueOverwrite(current_queue, &new_current); + + vTaskDelayUntil(&last_wake_time, frequency); + } +} + +float sensor_current_get(void) { + float current = 0.0f; + xQueueReceive(current_queue, ¤t, 0); + + return current; +} + +void sensor_wheel_speed_update(void) { + unsigned long current = micros(); + wheel_speed_interval = current - wheel_speed_last_pulse; + wheel_speed_last_pulse = current; +} + +float sensor_wheel_speed_get(void) { + return wheel_speed_interval * 17; +} + +void sensor_steering_angle_update(void *parameters) { + TickType_t last_wake_time = xTaskGetTickCount(); + const TickType_t frequency = pdMS_TO_TICKS(10); + + while (1) { + uint64_t steering_angle_raw; + + if (xSemaphoreTake(can_mutex, pdMS_TO_TICKS(1)) == pdTRUE) { + can_receive(&steering_angle_raw, 0x1); + xSemaphoreGive(can_mutex); + + float steering_angle = (float)steering_angle_raw; + xQueueOverwrite(steering_angle_queue, &steering_angle); + } + + vTaskDelayUntil(&last_wake_time, frequency); + } +} + +float sensor_steering_angle_get(void) { + float steering_angle = 0.0f; + xQueueReceive(steering_angle_queue, &steering_angle, 0); + + return steering_angle; +} + +void torque_update(void *parameters) { + TickType_t last_wake_time = xTaskGetTickCount(); + const TickType_t frequency = pdMS_TO_TICKS(1); + + while (1) { + float current = sensor_current_get(); + float wheel_speed = sensor_wheel_speed_get(); + float steering_angle = sensor_steering_angle_get(); + + float torques[4]; + calculate_torque_cmd(torques, current, &wheel_speed, steering_angle); + + if (xSemaphoreTake(can_mutex, pdMS_TO_TICKS(1)) == pdTRUE) { + can_send(0x2, torques[0]); + can_send(0x3, torques[1]); + can_send(0x4, torques[2]); + can_send(0x5, torques[3]); + xSemaphoreGive(can_mutex); + } + + xQueueOverwrite(torques_queue, torques); + + vTaskDelayUntil(&last_wake_time, frequency); + } +} + +void lcd_update(void *parameters) { + TickType_t last_wake_time = xTaskGetTickCount(); + const TickType_t frequency = pdMS_TO_TICKS(100); + + while (1) { + if (xSemaphoreTake(spi_mutex, pdMS_TO_TICKS(100))) { + float current = sensor_current_get(); + float wheel_speed = sensor_wheel_speed_get(); + float steering_angle = sensor_steering_angle_get(); + + float torques[4]; + xQueueReceive(torques_queue, torques, 0); + + lcd_printf("current: %.3f A\n", current); + lcd_printf("wheel speed: %.3f RPM\n", wheel_speed); + lcd_printf("steering angle: %.3f degrees", steering_angle); + lcd_printf("torque\n\tFL: %.3f \tFR: %.3f\n\tRL: %.3f RR %.3f", torques[0], torques[1], torques[2], torques[3]); + + xSemaphoreGive(spi_mutex); + } + + vTaskDelayUntil(&last_wake_time, frequency); + } +} + +void deenergize(void) { + digitalWrite(AIR_PLUS_PIN, arduino::LOW); + digitalWrite(AIR_MINUS_PIN, arduino::LOW); + digitalWrite(PRECHARGE_PIN, arduino::LOW); +} + +void bms_monitoring_task(void *parameters) { + TickType_t last_wake_time = xTaskGetTickCount(); + const TickType_t frequency = pdMS_TO_TICKS(50); + + while (1) { + if (xSemaphoreTake(spi_mutex, pdMS_TO_TICKS(100)) == pdTRUE) { + for (int i = 0; i < BATTERIES_COUNT; i++) { + float voltage = bms_get_voltage(i); + float temperature = bms_get_temperature(i); + + if (voltage < 2.8f || voltage > 4.3f || temperature > 60.0f) { + xSemaphoreGive(spi_mutex); + deenergize(); + } + } + + xSemaphoreGive(spi_mutex); + } + + vTaskDelayUntil(&last_wake_time, frequency); + } +} + +void setup(void) { + current_queue = xQueueCreate(1, sizeof(float)); + steering_angle_queue = xQueueCreate(1, sizeof(float)); + torques_queue = xQueueCreate(1, sizeof(float) * 4); + can_mutex = xSemaphoreCreateMutex(); + + can_init(CAN_RX_PIN, CAN_TX_PIN, 250000); // 250000 ?? + lcd_init(MOSI_PIN, MISO_PIN, SCK_PIN, LCD_CS_PIN); + bms_init(MOSI_PIN, MISO_PIN, SCK_PIN, BMS_CS_PIN); + + pinMode(WHEELSPEED_PIN, arduino::INPUT_PULLUP); + attachInterrupt(digitalPinToInterrupt(WHEELSPEED_PIN), sensor_wheel_speed_update, arduino::FALLING); + + xTaskCreate(sensor_current_update, "sensor_current_update", 1024, NULL, TaskPriority_SENSOR, NULL); + xTaskCreate(sensor_steering_angle_update, "sensor_steering_angle_update", 1024, NULL, TaskPriority_SENSOR, NULL); + xTaskCreate(bms_monitoring_task, "bms_monitoring_task", 1024, NULL, TaskPriority_BMS, NULL); + xTaskCreate(lcd_update, "lcd_update", 1024, NULL, TaskPriority_LCD, NULL); + + vTaskStartScheduler(); +} + +void loop(void) {} \ No newline at end of file From 0b0f4adfddf3f352537ae8e32002fb9f09da8f94 Mon Sep 17 00:00:00 2001 From: Prithika Tharan Date: Tue, 17 Mar 2026 18:55:04 -0400 Subject: [PATCH 2/4] made all changes required, however, still need to check for errors. Added a state machine to transition between states, updated calculation for wheelspin RPM, and initalized missing things from setup(). Also added a safe gaurd for torque_update() and fixed some errors. --- .../PrithikaTharan/intro_project.cpp | 55 +++++++++++++++++-- 1 file changed, 50 insertions(+), 5 deletions(-) diff --git a/intro_projects/PrithikaTharan/intro_project.cpp b/intro_projects/PrithikaTharan/intro_project.cpp index a5a7191..8f96737 100644 --- a/intro_projects/PrithikaTharan/intro_project.cpp +++ b/intro_projects/PrithikaTharan/intro_project.cpp @@ -152,6 +152,10 @@ constexpr int TaskPriority_LCD = 0; constexpr int TaskPriority_SENSOR = 1; constexpr int TaskPriority_BMS = 2; +// handling states for transitions (state machine) +enum class CarState { LV, TS, RTD }; +volatile CarState car_state = CarState::LV; + /* Initialize the CAN peripheral with given RX and TX pins at a given baudrate. */ @@ -233,14 +237,15 @@ float sensor_current_get(void) { return current; } -void sensor_wheel_speed_update(void) { +void sensor_wheel_speed_update(void) { // the ISR to sensor_steering_angle_update unsigned long current = micros(); wheel_speed_interval = current - wheel_speed_last_pulse; wheel_speed_last_pulse = current; } -float sensor_wheel_speed_get(void) { - return wheel_speed_interval * 17; +float sensor_wheel_speed_get(void) { ///////////////////// FIX THIS + if (wheel_speed_interval == 0) return 0.0f; // avoid divide by zero + return (60.0f * 1000000.0f) / (17.0f * (float)wheel_speed_interval); } void sensor_steering_angle_update(void *parameters) { @@ -274,12 +279,17 @@ void torque_update(void *parameters) { const TickType_t frequency = pdMS_TO_TICKS(1); while (1) { + if (car_state != CarState::RTD) { // don't drive unless right state. Just extra safe gaurding + vTaskDelayUntil(&last_wake_time, frequency); + continue; + } float current = sensor_current_get(); float wheel_speed = sensor_wheel_speed_get(); float steering_angle = sensor_steering_angle_get(); float torques[4]; - calculate_torque_cmd(torques, current, &wheel_speed, steering_angle); + float wheelspeeds[4] = { wheel_speed, wheel_speed, wheel_speed, wheel_speed }; // adding cause calculate_torque_cmd expects an array of 4 items + calculate_torque_cmd(torques, current, wheelspeeds, steering_angle); if (xSemaphoreTake(can_mutex, pdMS_TO_TICKS(1)) == pdTRUE) { can_send(0x2, torques[0]); @@ -349,23 +359,58 @@ void bms_monitoring_task(void *parameters) { } } +void state_machine_task(void *parameters) { + // set all relay pins as outputs + pinMode(AIR_MINUS_PIN, arduino::OUTPUT); + pinMode(AIR_PLUS_PIN, arduino::OUTPUT); + pinMode(PRECHARGE_PIN, arduino::OUTPUT); + pinMode(TS_ON_PIN, arduino::INPUT_PULLUP); + pinMode(RTD_PIN, arduino::INPUT_PULLUP); + + while (1) { + if (car_state == CarState::LV) { + if (digitalRead(TS_ON_PIN) == LOW) { // active low = pressed + // precharge sequence + digitalWrite(AIR_MINUS_PIN, arduino::HIGH); + digitalWrite(PRECHARGE_PIN, arduino::HIGH); + vTaskDelay(pdMS_TO_TICKS(5000)); // wait 5 seconds + digitalWrite(AIR_PLUS_PIN, arduino::HIGH); + digitalWrite(PRECHARGE_PIN, arduino::LOW); + car_state = CarState::TS; + } + } + + if (car_state == CarState::TS) { + if (digitalRead(RTD_PIN) == LOW) { // active low = pressed + car_state = CarState::RTD; + } + } + + vTaskDelay(pdMS_TO_TICKS(10)); // check buttons every 10ms + } +} + void setup(void) { current_queue = xQueueCreate(1, sizeof(float)); steering_angle_queue = xQueueCreate(1, sizeof(float)); torques_queue = xQueueCreate(1, sizeof(float) * 4); can_mutex = xSemaphoreCreateMutex(); + spi_mutex = xSemaphoreCreateMutex(); // set up the mutex for spi can_init(CAN_RX_PIN, CAN_TX_PIN, 250000); // 250000 ?? lcd_init(MOSI_PIN, MISO_PIN, SCK_PIN, LCD_CS_PIN); bms_init(MOSI_PIN, MISO_PIN, SCK_PIN, BMS_CS_PIN); pinMode(WHEELSPEED_PIN, arduino::INPUT_PULLUP); - attachInterrupt(digitalPinToInterrupt(WHEELSPEED_PIN), sensor_wheel_speed_update, arduino::FALLING); + attachInterrupt(digitalPinToInterrupt(WHEELSPEED_PIN), sensor_wheel_speed_update, arduino::FALLING); // inturupt to check for if the wheel turned xTaskCreate(sensor_current_update, "sensor_current_update", 1024, NULL, TaskPriority_SENSOR, NULL); xTaskCreate(sensor_steering_angle_update, "sensor_steering_angle_update", 1024, NULL, TaskPriority_SENSOR, NULL); xTaskCreate(bms_monitoring_task, "bms_monitoring_task", 1024, NULL, TaskPriority_BMS, NULL); xTaskCreate(lcd_update, "lcd_update", 1024, NULL, TaskPriority_LCD, NULL); + xTaskCreate(torque_update, "torque_update", 1024, NULL, TaskPriority_SENSOR, NULL); // wasn't started before + // state machine stuff + xTaskCreate(state_machine_task, "state_machine_task", 1024, NULL, TaskPriority_SENSOR, NULL); vTaskStartScheduler(); } From a4f3c0adb1a539be6bb5614a738db9c6bd8b0983 Mon Sep 17 00:00:00 2001 From: Prithika Tharan Date: Tue, 17 Mar 2026 21:33:55 -0400 Subject: [PATCH 3/4] updated BMS task to have more logic to allow information to be written on LCD. Wrote more relevant info to lcd. Fixed a few bugs --- .../PrithikaTharan/intro_project.cpp | 398 +++++++++++------- 1 file changed, 237 insertions(+), 161 deletions(-) diff --git a/intro_projects/PrithikaTharan/intro_project.cpp b/intro_projects/PrithikaTharan/intro_project.cpp index 8f96737..dcc9041 100644 --- a/intro_projects/PrithikaTharan/intro_project.cpp +++ b/intro_projects/PrithikaTharan/intro_project.cpp @@ -1,6 +1,6 @@ #include -#include #include +#include #include /* @@ -114,20 +114,20 @@ you end up with lots of useless formatting changes. You should install clang-format as soon as possible and set it up :) */ -constexpr int TS_ON_PIN = 12; -constexpr int RTD_PIN = 13; -constexpr int CURRENT_SENSOR_PIN = 19; -constexpr int CAN_RX_PIN = 2; -constexpr int CAN_TX_PIN = 3; -constexpr int MOSI_PIN = 5; -constexpr int MISO_PIN = 6; -constexpr int SCK_PIN = 7; -constexpr int LCD_CS_PIN = 8; -constexpr int BMS_CS_PIN = 9; -constexpr int AIR_PLUS_PIN = 22; -constexpr int PRECHARGE_PIN = 23; -constexpr int AIR_MINUS_PIN = 10; -constexpr int WHEELSPEED_PIN = 11; // 11 ?? +constexpr int TS_ON_PIN = 12; +constexpr int RTD_PIN = 13; +constexpr int CURRENT_SENSOR_PIN = 19; +constexpr int CAN_RX_PIN = 2; +constexpr int CAN_TX_PIN = 3; +constexpr int MOSI_PIN = 5; +constexpr int MISO_PIN = 6; +constexpr int SCK_PIN = 7; +constexpr int LCD_CS_PIN = 8; +constexpr int BMS_CS_PIN = 9; +constexpr int AIR_PLUS_PIN = 22; +constexpr int PRECHARGE_PIN = 23; +constexpr int AIR_MINUS_PIN = 10; +constexpr int WHEELSPEED_PIN = 11; // 11 ?? constexpr int BATTERIES_COUNT = 138; @@ -141,6 +141,7 @@ QueueHandle_t steering_angle_queue; volatile float bms_voltage_min = 0.0f; volatile float bms_voltage_max = 0.0f; volatile float bms_temperature_max = 0.0f; +volatile bool bms_fault_active = false; SemaphoreHandle_t bms_mutex; QueueHandle_t torques_queue; @@ -153,7 +154,10 @@ constexpr int TaskPriority_SENSOR = 1; constexpr int TaskPriority_BMS = 2; // handling states for transitions (state machine) -enum class CarState { LV, TS, RTD }; +enum class CarState { LV, + PRECHARGE, + TS, + RTD }; volatile CarState car_state = CarState::LV; /* @@ -169,16 +173,14 @@ extern void can_send(uint8_t id, uint64_t payload); /* Receive a CAN message with a given id into a uint64_t */ -extern void can_receive(uint64_t *payload, uint8_t id); +extern void can_receive(uint64_t* payload, uint8_t id); /* Calculates four torques, in order, for the Front Left, Front Right, Rear Left, and Rear Right motors given current, wheelspeeds (in the same order), and a steering angle. */ -extern void calculate_torque_cmd( - float *torques, float current, float *wheelspeeds, float steering_angle -); +extern void calculate_torque_cmd(float* torques, float current, float* wheelspeeds, float steering_angle); /* Initialize the LCD peripheral @@ -188,7 +190,7 @@ extern void lcd_init(uint8_t mosi, uint8_t miso, uint8_t sck, uint8_t lcs_cs); /* Print something to the LCD */ -extern void lcd_printf(const char *fmt, ...); +extern void lcd_printf(const char* fmt, ...); /* Initialize the BMS @@ -206,160 +208,227 @@ extern float bms_get_voltage(uint8_t n); extern float bms_get_temperature(uint8_t n); float sensor_current_read() { - static const float ADC_VREF_V = 3.3f; - static const float ADC_RESOLUTION = 1024.0f; - static const float ADC_CONVERSION = 0.01f; + static const float ADC_VREF_V = 3.3f; + static const float ADC_RESOLUTION = 1024.0f; + static const float ADC_CONVERSION = 0.01f; - int raw = analogRead(CURRENT_SENSOR_PIN); + int raw = analogRead(CURRENT_SENSOR_PIN); - float voltage_V = (raw * ADC_VREF_V) / ADC_RESOLUTION; - float current_A = voltage_V / ADC_CONVERSION; + float voltage_V = (raw * ADC_VREF_V) / ADC_RESOLUTION; + float current_A = voltage_V / ADC_CONVERSION; - return current_A; + return current_A; } -void sensor_current_update(void *parameters) { - TickType_t last_wake_time = xTaskGetTickCount(); - const TickType_t frequency = pdMS_TO_TICKS(1); +void sensor_current_update(void* parameters) { + TickType_t last_wake_time = xTaskGetTickCount(); + const TickType_t frequency = pdMS_TO_TICKS(1); - while (1) { - float new_current = sensor_current_read(); - xQueueOverwrite(current_queue, &new_current); + while (1) { + float new_current = sensor_current_read(); + xQueueOverwrite(current_queue, &new_current); - vTaskDelayUntil(&last_wake_time, frequency); - } + vTaskDelayUntil(&last_wake_time, frequency); + } } float sensor_current_get(void) { - float current = 0.0f; - xQueueReceive(current_queue, ¤t, 0); + float current = 0.0f; + xQueueReceive(current_queue, ¤t, 0); - return current; + return current; } -void sensor_wheel_speed_update(void) { // the ISR to sensor_steering_angle_update - unsigned long current = micros(); - wheel_speed_interval = current - wheel_speed_last_pulse; - wheel_speed_last_pulse = current; +void sensor_wheel_speed_update(void) { // ISR called on each falling edge from the wheelspeed sensor + unsigned long current = micros(); + wheel_speed_interval = current - wheel_speed_last_pulse; + wheel_speed_last_pulse = current; } -float sensor_wheel_speed_get(void) { ///////////////////// FIX THIS - if (wheel_speed_interval == 0) return 0.0f; // avoid divide by zero - return (60.0f * 1000000.0f) / (17.0f * (float)wheel_speed_interval); +float sensor_wheel_speed_get(void) { + // check for a timeout in case car stops moving. Use about 0.5s as the check + unsigned long current_time = micros(); + + if ((current_time - wheel_speed_last_pulse) > 500000) + return 0.0f; + + // RPM is calculated as: (60 * wheel_pulse_freq) / (total pulses per revolution) + if (wheel_speed_interval == 0) return 0.0f; // avoid divide by zero + return (60.0f * 1000000.0f) / (17.0f * (float)wheel_speed_interval); // mult to get right units } -void sensor_steering_angle_update(void *parameters) { - TickType_t last_wake_time = xTaskGetTickCount(); - const TickType_t frequency = pdMS_TO_TICKS(10); +void sensor_steering_angle_update(void* parameters) { + TickType_t last_wake_time = xTaskGetTickCount(); + const TickType_t frequency = pdMS_TO_TICKS(10); - while (1) { - uint64_t steering_angle_raw; - - if (xSemaphoreTake(can_mutex, pdMS_TO_TICKS(1)) == pdTRUE) { - can_receive(&steering_angle_raw, 0x1); - xSemaphoreGive(can_mutex); + while (1) { + uint64_t steering_angle_raw; - float steering_angle = (float)steering_angle_raw; - xQueueOverwrite(steering_angle_queue, &steering_angle); - } + if (xSemaphoreTake(can_mutex, pdMS_TO_TICKS(1)) == pdTRUE) { + can_receive(&steering_angle_raw, 0x1); + xSemaphoreGive(can_mutex); - vTaskDelayUntil(&last_wake_time, frequency); - } + float steering_angle = (float)steering_angle_raw; + xQueueOverwrite(steering_angle_queue, &steering_angle); + } + + vTaskDelayUntil(&last_wake_time, frequency); + } } float sensor_steering_angle_get(void) { - float steering_angle = 0.0f; - xQueueReceive(steering_angle_queue, &steering_angle, 0); + float steering_angle = 0.0f; + xQueueReceive(steering_angle_queue, &steering_angle, 0); - return steering_angle; + return steering_angle; } -void torque_update(void *parameters) { - TickType_t last_wake_time = xTaskGetTickCount(); - const TickType_t frequency = pdMS_TO_TICKS(1); +void torque_update(void* parameters) { + TickType_t last_wake_time = xTaskGetTickCount(); + const TickType_t frequency = pdMS_TO_TICKS(1); + + while (1) { + if (car_state != CarState::RTD) { // don't drive unless right state. Just extra safe gaurding + vTaskDelayUntil(&last_wake_time, frequency); + continue; + } + float current = sensor_current_get(); + float wheel_speed = sensor_wheel_speed_get(); + float steering_angle = sensor_steering_angle_get(); + + float torques[4]; + float wheelspeeds[4] = {wheel_speed, wheel_speed, wheel_speed, wheel_speed}; // adding cause calculate_torque_cmd expects an array of 4 items + calculate_torque_cmd(torques, current, wheelspeeds, steering_angle); + + if (xSemaphoreTake(can_mutex, pdMS_TO_TICKS(1)) == pdTRUE) { + can_send(0x2, torques[0]); + can_send(0x3, torques[1]); + can_send(0x4, torques[2]); + can_send(0x5, torques[3]); + xSemaphoreGive(can_mutex); + } + + xQueueOverwrite(torques_queue, torques); - while (1) { - if (car_state != CarState::RTD) { // don't drive unless right state. Just extra safe gaurding - vTaskDelayUntil(&last_wake_time, frequency); - continue; + vTaskDelayUntil(&last_wake_time, frequency); } - float current = sensor_current_get(); - float wheel_speed = sensor_wheel_speed_get(); - float steering_angle = sensor_steering_angle_get(); - - float torques[4]; - float wheelspeeds[4] = { wheel_speed, wheel_speed, wheel_speed, wheel_speed }; // adding cause calculate_torque_cmd expects an array of 4 items - calculate_torque_cmd(torques, current, wheelspeeds, steering_angle); - - if (xSemaphoreTake(can_mutex, pdMS_TO_TICKS(1)) == pdTRUE) { - can_send(0x2, torques[0]); - can_send(0x3, torques[1]); - can_send(0x4, torques[2]); - can_send(0x5, torques[3]); - xSemaphoreGive(can_mutex); +} + +// helper to print car state on the lcd +const char* car_state_to_string(CarState state) { + switch (state) { + case CarState::LV: + return "LV"; + case CarState::PRECHARGE: + return "PRECHARGE"; + case CarState::TS: + return "TS"; + case CarState::RTD: + return "RTD"; + default: + return "UNKNOWN"; } +} - xQueueOverwrite(torques_queue, torques); +void lcd_update(void* parameters) { + TickType_t last_wake_time = xTaskGetTickCount(); + const TickType_t frequency = pdMS_TO_TICKS(100); - vTaskDelayUntil(&last_wake_time, frequency); - } -} + while (1) { + if (xSemaphoreTake(spi_mutex, pdMS_TO_TICKS(100))) { + float min_voltage = 0.0f; + float max_voltage = 0.0f; + float max_temperature = 0.0f; + bool fault_active = false; + + // read bms values with mutex to prevent race conditions + if (xSemaphoreTake(bms_mutex, pdMS_TO_TICKS(10)) == pdTRUE) { + min_voltage = bms_voltage_min; + max_voltage = bms_voltage_max; + max_temperature = bms_temperature_max; + fault_active = bms_fault_active; + xSemaphoreGive(bms_mutex); + } -void lcd_update(void *parameters) { - TickType_t last_wake_time = xTaskGetTickCount(); - const TickType_t frequency = pdMS_TO_TICKS(100); + float current = sensor_current_get(); + float wheel_speed = sensor_wheel_speed_get(); + float steering_angle = sensor_steering_angle_get(); - while (1) { - if (xSemaphoreTake(spi_mutex, pdMS_TO_TICKS(100))) { - float current = sensor_current_get(); - float wheel_speed = sensor_wheel_speed_get(); - float steering_angle = sensor_steering_angle_get(); + float torques[4] = {0.0f, 0.0f, 0.0f, 0.0f}; + xQueueReceive(torques_queue, torques, 0); - float torques[4]; - xQueueReceive(torques_queue, torques, 0); + lcd_printf("state: %s\n", car_state_to_string(car_state)); // car state + lcd_printf("fault: %s\n", fault_active ? "YES" : "NO"); // bms fault + lcd_printf("current: %.3f A\n", current); // current + lcd_printf("wheel speed: %.3f RPM\n", wheel_speed); // rpm + lcd_printf("steering angle: %.3f degrees", steering_angle); // steering angle + lcd_printf("bms minV: %.3f V\n", min_voltage); // bms vals as computed in the bms task (min and max voltage and max temp) + lcd_printf("bms maxV: %.3f V\n", max_voltage); + lcd_printf("bms maxT: %.3f C\n", max_temperature); + lcd_printf("torque\n\tFL: %.3f \tFR: %.3f\n\tRL: %.3f RR %.3f", torques[0], torques[1], torques[2], torques[3]); - lcd_printf("current: %.3f A\n", current); - lcd_printf("wheel speed: %.3f RPM\n", wheel_speed); - lcd_printf("steering angle: %.3f degrees", steering_angle); - lcd_printf("torque\n\tFL: %.3f \tFR: %.3f\n\tRL: %.3f RR %.3f", torques[0], torques[1], torques[2], torques[3]); + xSemaphoreGive(spi_mutex); + } - xSemaphoreGive(spi_mutex); + vTaskDelayUntil(&last_wake_time, frequency); } - - vTaskDelayUntil(&last_wake_time, frequency); - } } void deenergize(void) { - digitalWrite(AIR_PLUS_PIN, arduino::LOW); - digitalWrite(AIR_MINUS_PIN, arduino::LOW); - digitalWrite(PRECHARGE_PIN, arduino::LOW); + digitalWrite(AIR_PLUS_PIN, arduino::LOW); + digitalWrite(AIR_MINUS_PIN, arduino::LOW); + digitalWrite(PRECHARGE_PIN, arduino::LOW); + car_state = CarState::LV; // go back to LV state } -void bms_monitoring_task(void *parameters) { - TickType_t last_wake_time = xTaskGetTickCount(); - const TickType_t frequency = pdMS_TO_TICKS(50); +void bms_monitoring_task(void* parameters) { + TickType_t last_wake_time = xTaskGetTickCount(); + const TickType_t frequency = pdMS_TO_TICKS(50); - while (1) { - if (xSemaphoreTake(spi_mutex, pdMS_TO_TICKS(100)) == pdTRUE) { - for (int i = 0; i < BATTERIES_COUNT; i++) { - float voltage = bms_get_voltage(i); - float temperature = bms_get_temperature(i); - - if (voltage < 2.8f || voltage > 4.3f || temperature > 60.0f) { - xSemaphoreGive(spi_mutex); - deenergize(); + while (1) { + if (xSemaphoreTake(spi_mutex, pdMS_TO_TICKS(100)) == pdTRUE) { + bool fault_active = false; // to prevent giving spi mutex twice + float lowest_voltage = 10000.0f; + float highest_voltage = 0.0f; + float highest_temp = -10000.0f; + + for (int i = 0; i < BATTERIES_COUNT; i++) { + float voltage = bms_get_voltage(i); + float temperature = bms_get_temperature(i); + + // get the max/min values to print on lcd + if (voltage < lowest_voltage) + lowest_voltage = voltage; + if (voltage > highest_voltage) + highest_voltage = voltage; + if (temperature > highest_temp) + highest_temp = temperature; + + if (voltage < 2.8f || voltage > 4.3f || temperature > 60.0f) { + fault_active = true; + break; + } + } + xSemaphoreGive(spi_mutex); // give the mutex first + + if (fault_active) + deenergize(); + + // modfiy the global variables using mutex cause LCD reads from them, this task writes to them, avoid race conditions + if (xSemaphoreTake(bms_mutex, pdMS_TO_TICKS(10)) == pdTRUE) { + bms_voltage_min = lowest_voltage; + bms_voltage_max = highest_voltage; + bms_temperature_max = highest_temp; + bms_fault_active = fault_active; + xSemaphoreGive(bms_mutex); + } } - } - xSemaphoreGive(spi_mutex); + vTaskDelayUntil(&last_wake_time, frequency); } - - vTaskDelayUntil(&last_wake_time, frequency); - } } -void state_machine_task(void *parameters) { +void state_machine_task(void* parameters) { // set all relay pins as outputs pinMode(AIR_MINUS_PIN, arduino::OUTPUT); pinMode(AIR_PLUS_PIN, arduino::OUTPUT); @@ -369,50 +438,57 @@ void state_machine_task(void *parameters) { while (1) { if (car_state == CarState::LV) { - if (digitalRead(TS_ON_PIN) == LOW) { // active low = pressed + if (digitalRead(TS_ON_PIN) == LOW) { // active low = pressed + car_state = CarState::PRECHARGE; // PRECHARGE state just to track if smth inturrupts this sequence // precharge sequence - digitalWrite(AIR_MINUS_PIN, arduino::HIGH); - digitalWrite(PRECHARGE_PIN, arduino::HIGH); - vTaskDelay(pdMS_TO_TICKS(5000)); // wait 5 seconds - digitalWrite(AIR_PLUS_PIN, arduino::HIGH); - digitalWrite(PRECHARGE_PIN, arduino::LOW); - car_state = CarState::TS; + digitalWrite(AIR_MINUS_PIN, arduino::HIGH); // close AIR- + digitalWrite(PRECHARGE_PIN, arduino::HIGH); // close precharge relay + vTaskDelay(pdMS_TO_TICKS(5000)); // wait 5 seconds + if (car_state == CarState::PRECHARGE) { + digitalWrite(AIR_PLUS_PIN, arduino::HIGH); // close AIR+ + digitalWrite(PRECHARGE_PIN, arduino::LOW); // open precharge relay + car_state = CarState::TS; // set state + } else { + // sequence aborted due to deenergize(), undo changes preivously made + digitalWrite(AIR_MINUS_PIN, arduino::LOW); + digitalWrite(PRECHARGE_PIN, arduino::LOW); + } } - } - - if (car_state == CarState::TS) { - if (digitalRead(RTD_PIN) == LOW) { // active low = pressed + } else if (car_state == CarState::TS) { + if (digitalRead(RTD_PIN) == LOW) { // active low = pressed car_state = CarState::RTD; } } - vTaskDelay(pdMS_TO_TICKS(10)); // check buttons every 10ms + vTaskDelay(pdMS_TO_TICKS(10)); // check buttons every 10ms } } void setup(void) { - current_queue = xQueueCreate(1, sizeof(float)); - steering_angle_queue = xQueueCreate(1, sizeof(float)); - torques_queue = xQueueCreate(1, sizeof(float) * 4); - can_mutex = xSemaphoreCreateMutex(); - spi_mutex = xSemaphoreCreateMutex(); // set up the mutex for spi - - can_init(CAN_RX_PIN, CAN_TX_PIN, 250000); // 250000 ?? - lcd_init(MOSI_PIN, MISO_PIN, SCK_PIN, LCD_CS_PIN); - bms_init(MOSI_PIN, MISO_PIN, SCK_PIN, BMS_CS_PIN); - - pinMode(WHEELSPEED_PIN, arduino::INPUT_PULLUP); - attachInterrupt(digitalPinToInterrupt(WHEELSPEED_PIN), sensor_wheel_speed_update, arduino::FALLING); // inturupt to check for if the wheel turned - - xTaskCreate(sensor_current_update, "sensor_current_update", 1024, NULL, TaskPriority_SENSOR, NULL); - xTaskCreate(sensor_steering_angle_update, "sensor_steering_angle_update", 1024, NULL, TaskPriority_SENSOR, NULL); - xTaskCreate(bms_monitoring_task, "bms_monitoring_task", 1024, NULL, TaskPriority_BMS, NULL); - xTaskCreate(lcd_update, "lcd_update", 1024, NULL, TaskPriority_LCD, NULL); - xTaskCreate(torque_update, "torque_update", 1024, NULL, TaskPriority_SENSOR, NULL); // wasn't started before - // state machine stuff - xTaskCreate(state_machine_task, "state_machine_task", 1024, NULL, TaskPriority_SENSOR, NULL); - - vTaskStartScheduler(); + current_queue = xQueueCreate(1, sizeof(float)); + steering_angle_queue = xQueueCreate(1, sizeof(float)); + torques_queue = xQueueCreate(1, sizeof(float) * 4); + + can_mutex = xSemaphoreCreateMutex(); + spi_mutex = xSemaphoreCreateMutex(); // create mutex for spi + bms_mutex = xSemaphoreCreateMutex(); // create mutex for bms + + can_init(CAN_RX_PIN, CAN_TX_PIN, 250000); // 250000 ?? + lcd_init(MOSI_PIN, MISO_PIN, SCK_PIN, LCD_CS_PIN); + bms_init(MOSI_PIN, MISO_PIN, SCK_PIN, BMS_CS_PIN); + + pinMode(WHEELSPEED_PIN, arduino::INPUT_PULLUP); + attachInterrupt(digitalPinToInterrupt(WHEELSPEED_PIN), sensor_wheel_speed_update, arduino::FALLING); // inturupt to check for if the wheel turned + + xTaskCreate(sensor_current_update, "sensor_current_update", 1024, NULL, TaskPriority_SENSOR, NULL); + xTaskCreate(sensor_steering_angle_update, "sensor_steering_angle_update", 1024, NULL, TaskPriority_SENSOR, NULL); + xTaskCreate(bms_monitoring_task, "bms_monitoring_task", 1024, NULL, TaskPriority_BMS, NULL); + xTaskCreate(lcd_update, "lcd_update", 1024, NULL, TaskPriority_LCD, NULL); + xTaskCreate(torque_update, "torque_update", 1024, NULL, TaskPriority_SENSOR, NULL); // wasn't started before + // state machine stuff + xTaskCreate(state_machine_task, "state_machine_task", 1024, NULL, TaskPriority_SENSOR, NULL); + + vTaskStartScheduler(); } void loop(void) {} \ No newline at end of file From f750f202703bde38624ea53e462d0d5e4c896390 Mon Sep 17 00:00:00 2001 From: Prithika Tharan Date: Tue, 17 Mar 2026 21:36:50 -0400 Subject: [PATCH 4/4] finished onboarding project --- intro_projects/PrithikaTharan/intro_project.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/intro_projects/PrithikaTharan/intro_project.cpp b/intro_projects/PrithikaTharan/intro_project.cpp index dcc9041..e0b5b76 100644 --- a/intro_projects/PrithikaTharan/intro_project.cpp +++ b/intro_projects/PrithikaTharan/intro_project.cpp @@ -363,8 +363,8 @@ void lcd_update(void* parameters) { lcd_printf("wheel speed: %.3f RPM\n", wheel_speed); // rpm lcd_printf("steering angle: %.3f degrees", steering_angle); // steering angle lcd_printf("bms minV: %.3f V\n", min_voltage); // bms vals as computed in the bms task (min and max voltage and max temp) - lcd_printf("bms maxV: %.3f V\n", max_voltage); - lcd_printf("bms maxT: %.3f C\n", max_temperature); + lcd_printf("bms maxV: %.3f V\n", max_voltage); // max voltage + lcd_printf("bms maxT: %.3f C\n", max_temperature); // max temp lcd_printf("torque\n\tFL: %.3f \tFR: %.3f\n\tRL: %.3f RR %.3f", torques[0], torques[1], torques[2], torques[3]); xSemaphoreGive(spi_mutex);