From 24a675ccb123ce942fab65dd041ca78b2f4d68a1 Mon Sep 17 00:00:00 2001 From: Scott A <89099102+Sabramz@users.noreply.github.com> Date: Wed, 31 Jul 2024 17:39:01 -0400 Subject: [PATCH] Regen Braking (#188) * switch to pca9539 (untested) (#175) * switch to pca9539 (untested) * update pdu to improved api * fix addr * new reg name * disable shutdown * removed debugging stuff * bump --------- Co-authored-by: Scott A <89099102+Sabramz@users.noreply.github.com> * Feature/pedal tune (#180) * prepped for pedal tuning + added break check when going to drive mode * added Scott's implementation for bspd pre check * added defcon 5 fault * fuck * remoced torque send * pedals calibrated * fix bit * im stupid * new pedals * added debug * poop farty * Auto stash before merge of "feature/pedal-tune" and "origin/feature/pedal-tune" * move tsms status * reset queuue * fixed stuff --------- Co-authored-by: Jack Rubacha * Finally get mph working! (#183) * dti back and mph calcs * queue fixes and got a backwards erpm reading * use correct units * fix up mph, tested recieve and send, not validated connection --------- Co-authored-by: Scott A <89099102+Sabramz@users.noreply.github.com> * Monitor fixes (#181) * add lv monitor again * remove bad line * fix missed line in rebase * fix up math * regen fix issue * add dti router, disable preemption * regen breaking ported from 17A, get voltage from DTI * it builds * new regen function, get ccl from bms, brake consts, adj_pedal_val func * fixing some calcs, went back to torque pass by reference * regen only when car is moving, adjust range of max regen, documetation * ccl to little endian * rewriting entire codebase in rust * misc fixes * comment out torque accel limit * fix send torque * fix send torque * add accel, make vital improvements * Bms fault fixes (#182) * fix bms fault, get rid of write fan battbox * add note * add torque.py * add max brake ac current, new accel pedal algo, makes forward accel more sensitive * fix bspd prefault * adjust value for proper scale * fix mph, bspd prefault * fix calcs * fix nero * more torque work * Accel pedal regen cleanup, dti_set_brake_current adjusted to use current instead of torque, cleanup and docs, bumb embedded base * override tsms, ccl fixed, adjusting torque and regen calcs * messages successfully sent * acceleration is steps of 1% travel * fixed commands to not overwrite eachother * regen working, comment out bspd prefault * formatting * merge failures fixed * remove garbage and merge garbage, change send_regen_braking function signature, add docs * remove bad comments, change func signature to uint16, edit documentation * debugging, changing more func signatures * change DTI functions to not use floats * formatting --------- Co-authored-by: Jack Rubacha Co-authored-by: dyldonahue <98500199+dyldonahue@users.noreply.github.com> --- Core/Inc/bms.h | 4 +- Core/Inc/cerberus_conf.h | 11 +- Core/Inc/dti.h | 29 ++++- Core/Src/bms.c | 4 - Core/Src/dti.c | 38 +++++-- Core/Src/monitor.c | 88 ++++++--------- Core/Src/state_machine.c | 5 +- Core/Src/steeringio.c | 2 - Core/Src/torque.c | 230 +++++++++++++++++++++++---------------- Drivers/Embedded-Base | 2 +- torque_calc_sim.py | 100 +++++++++++++++++ 11 files changed, 340 insertions(+), 173 deletions(-) create mode 100644 torque_calc_sim.py diff --git a/Core/Inc/bms.h b/Core/Inc/bms.h index 378673e..83ec11e 100644 --- a/Core/Inc/bms.h +++ b/Core/Inc/bms.h @@ -4,11 +4,13 @@ #include "cmsis_os.h" -#define BMS_DCL_MSG 0x156 /*BMS MONITOR WATCHDOG*/ +#define BMS_DCL_MSG 0x156 /*BMS MONITOR WATCHDOG*/ +#define BMS_CURR_LIMIT_MSG 0x176 typedef struct { osTimerId bms_monitor_timer; uint16_t dcl; + uint16_t ccl; } bms_t; extern bms_t *bms; diff --git a/Core/Inc/cerberus_conf.h b/Core/Inc/cerberus_conf.h index a580463..5700d84 100644 --- a/Core/Inc/cerberus_conf.h +++ b/Core/Inc/cerberus_conf.h @@ -6,10 +6,19 @@ #define SHUTDOWN_MONITOR_DELAY 500 /* ms */ #define NERO_DELAY_TIME 100 /* ms*/ #define LV_READ_DELAY 1000 +#define YELLOW_LED_BLINK_DELAY 500 /* ms */ +#define TEMP_SENS_SAMPLE_DELAY 200 /* ms */ +#define IMU_SAMPLE_DELAY 500 /* ms */ +#define FUSES_SAMPLE_DELAY 1000 /* ms */ +#define SHUTDOWN_MONITOR_DELAY 500 /* ms */ +#define NERO_DELAY_TIME 100 /* ms*/ +#define LV_READ_DELAY 1000 #define SERIAL_MONITOR_DELAY #define CAN_ROUTER_DELAY #define CAN_DISPATCH_DELAY 5 #define BMS_CAN_MONITOR_DELAY 4000 +#define CAN_DISPATCH_DELAY 5 +#define BMS_CAN_MONITOR_DELAY 4000 #define STATE_MACHINE_DELAY #define TORQUE_CALC_DELAY #define FAULT_HANDLE_DELAY @@ -23,7 +32,7 @@ #define PEDAL_BRAKE_THRESH 650 /* Torque Tuning */ -#define MAX_TORQUE 220 /* Nm * 10 */ +#define MAX_TORQUE 220 /* Nm */ // DEBUGGING: Start with a high debounce period, lower it if it is too slow // NOTE: SteeringIOMonitor updates every 25 ms. Previous value of 25 ms doesn't diff --git a/Core/Inc/dti.h b/Core/Inc/dti.h index 7b6a083..2c4e2e9 100644 --- a/Core/Inc/dti.h +++ b/Core/Inc/dti.h @@ -19,12 +19,22 @@ /* Message IDs from DTI CAN Datasheet */ #define DTI_CANID_ERPM 0x416 /* ERPM, Duty, Input Voltage */ #define DTI_CANID_CURRENTS 0x436 /* AC Current, DC Current */ +#define DTI_CANID_ERPM 0x416 /* ERPM, Duty, Input Voltage */ +#define DTI_CANID_CURRENTS 0x436 /* AC Current, DC Current */ #define DTI_CANID_TEMPS_FAULT 0x456 /* Controller Temp, Motor Temp, Faults */ #define DTI_CANID_ID_IQ 0x476 /* Id, Iq values */ #define DTI_CANID_SIGNALS \ 0x496 /* Throttle signal, Brake signal, IO, Drive enable */ #define DTI_QUEUE_SIZE 5 +#define TIRE_DIAMETER 16 /* inches */ +#define GEAR_RATIO 47 / 13.0 /* unitless */ +#define POLE_PAIRS 10 /* unitless */ +#define DTI_CANID_ID_IQ 0x476 /* Id, Iq values */ +#define DTI_CANID_SIGNALS \ + 0x496 /* Throttle signal, Brake signal, IO, Drive enable */ +#define DTI_QUEUE_SIZE 5 + #define TIRE_DIAMETER 16 /* inches */ #define GEAR_RATIO 47 / 13.0 /* unitless */ #define POLE_PAIRS 10 /* unitless */ @@ -47,6 +57,8 @@ typedef struct { // TODO: Expand GET interface int32_t dti_get_rpm(dti_t *dti); +uint16_t dti_get_input_voltage(dti_t *dti); + /* Utilities for Decoding CAN message */ extern osThreadId_t dti_router_handle; extern const osThreadAttr_t dti_router_attributes; @@ -61,14 +73,19 @@ dti_t *dti_init(); */ void dti_set_torque(int16_t torque); -// TODO: Regen interface -// void dti_set_regen(int16_t regen); +/** + * @brief Set the brake AC current target for regenerative braking. Only positive values are accepted by the DTI. + * + * @param current_target The desired AC current to do regenerative braking at. Must be positive. This argument must be the actual value to set multiplied by 10. + */ +void dti_set_regen(uint16_t current_target); -/* - * SCALE: 10 - * UNITS: Amps +/** + * @brief Send a CAN message containing the AC current target for regenerative braking. + * + * @param brake_current AC current target for regenerative braking. The actual value sent to the motor controller must be multiplied by 10. */ -void dti_set_brake_current(int16_t brake_current); +void dti_send_brake_current(uint16_t brake_current); /* * SCALE: 10 diff --git a/Core/Src/bms.c b/Core/Src/bms.c index 1c87c6e..36289a5 100644 --- a/Core/Src/bms.c +++ b/Core/Src/bms.c @@ -56,12 +56,8 @@ void vBMSCANMonitor(void *pv_params) if (osOK == osMessageQueueGet(bms_monitor_queue, &msg_from_queue, NULL, osWaitForever)) { - /*TO-DO: fix duration (ticks)*/ osTimerStart(bms->bms_monitor_timer, BMS_CAN_MONITOR_DELAY); - bms->dcl = (uint16_t)((msg_from_queue.data[1] << 8) & - msg_from_queue.data[0]); - //serial_print("BMS DCL %d", bms->dcl); } } } \ No newline at end of file diff --git a/Core/Src/dti.c b/Core/Src/dti.c index 84a759c..5edf030 100644 --- a/Core/Src/dti.c +++ b/Core/Src/dti.c @@ -24,7 +24,6 @@ #define CAN_QUEUE_SIZE 5 /* messages */ #define SAMPLES 20 static osMutexAttr_t dti_mutex_attributes; -// osMessageQueueId_t dti_router_queue; dti_t *dti_init() { @@ -64,11 +63,35 @@ void dti_set_torque(int16_t torque) int16_t ac_current = (((float)average / EMRAX_KT) * 10); /* times 10 */ - //serial_print("Commanded Current: %d \r\n", ac_current); + // serial_print("Commanded Current: %d \r\n", ac_current); dti_set_current(ac_current); } +void dti_set_regen(uint16_t current_target) +{ + /* Simple moving average to smooth change in braking target */ + + // Static variables for the buffer and index + static uint16_t buffer[SAMPLES] = { 0 }; + static int index = 0; + + // Add the new value to the buffer + buffer[index] = current_target; + + // Increment the index, wrapping around if necessary + index = (index + 1) % SAMPLES; + + // Calculate the average of the buffer + uint16_t sum = 0; + for (int i = 0; i < SAMPLES; ++i) { + sum += buffer[i]; + } + uint16_t average = sum / SAMPLES; + + dti_send_brake_current(average); +} + void dti_set_current(int16_t current) { can_msg_t msg = { .id = 0x036, .len = 2, .data = { 0 } }; @@ -77,8 +100,6 @@ void dti_set_current(int16_t current) //endian_swap(¤t, sizeof(current)); //memcpy(msg.data, ¤t, msg.len); - serial_print("current: %d\r\n", current); - int8_t msb = (int8_t)((current >> 8) & 0xFF); int8_t lsb = (uint8_t)(current & 0xFF); @@ -88,14 +109,17 @@ void dti_set_current(int16_t current) queue_can_msg(msg); } -void dti_set_brake_current(int16_t brake_current) +void dti_send_brake_current(uint16_t brake_current) { - can_msg_t msg = { .id = 0x056, .len = 2, .data = { 0 } }; + can_msg_t msg = { .id = 0x056, + .len = 8, + .data = { 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0 } }; /* convert to big endian */ endian_swap(&brake_current, sizeof(brake_current)); + /* Send CAN message */ - memcpy(&msg.data, &brake_current, msg.len); + memcpy(&msg.data, &brake_current, 2); queue_can_msg(msg); } diff --git a/Core/Src/monitor.c b/Core/Src/monitor.c index 193a6df..5f13181 100644 --- a/Core/Src/monitor.c +++ b/Core/Src/monitor.c @@ -56,7 +56,6 @@ void vLVMonitor(void *pv_params) // get final voltage v_int = (uint32_t)(v_dec * 10.0); - // endian_swap(&v_int, sizeof(v_int)); memcpy(msg.data, &v_int, msg.len); if (queue_can_msg(msg)) { fault_data.diag = @@ -196,10 +195,23 @@ void eval_pedal_fault(uint16_t accel_1, uint16_t accel_2, } } +/** + * @brief Return the adjusted pedal value based on its offset and maximum value. Clamps negative values to 0. + * + * @param raw the raw pedal value + * @param offset the offset for the pedal + * @param max the maximum value of the pedal + */ +uint16_t adjust_pedal_val(uint32_t raw, int32_t offset, int32_t max) +{ + return (int16_t)raw - offset <= 0 ? + 0 : + (uint16_t)(raw - offset) * 100 / (max - offset); +} + void vPedalsMonitor(void *pv_params) { const uint8_t num_samples = 10; - static const uint8_t buffer_size = 10; enum { ACCELPIN_2, ACCELPIN_1, BRAKEPIN_1, BRAKEPIN_2 }; static pedals_t sensor_data; @@ -226,7 +238,6 @@ void vPedalsMonitor(void *pv_params) mpu_t *mpu = (mpu_t *)pv_params; uint8_t counter = 0; - static int index = 0; for (;;) { read_pedals(mpu, adc_data); @@ -235,69 +246,30 @@ void vPedalsMonitor(void *pv_params) eval_pedal_fault(adc_data[ACCELPIN_1], adc_data[ACCELPIN_2], &diff_timer_accelerator, &sc_timer_accelerator, &oc_timer_accelerator, &fault_data); - // eval_pedal_fault(adc_data[BRAKEPIN_1], adc_data[BRAKEPIN_1], &diff_timer_brake, &sc_timer_brake, &oc_timer_brake, &fault_data); /* Offset adjusted per pedal sensor, clamp to be above 0 */ - uint16_t accel_val1 = - (int16_t)adc_data[ACCELPIN_1] - ACCEL1_OFFSET <= 0 ? - 0 : - (uint16_t)(adc_data[ACCELPIN_1] - - ACCEL1_OFFSET) * - 100 / (ACCEL1_MAX_VAL - ACCEL1_OFFSET); - // printf("Accel 1: %d\r\n", max_pedal1); - uint16_t accel_val2 = - (int16_t)adc_data[ACCELPIN_2] - ACCEL2_OFFSET <= 0 ? - 0 : - (uint16_t)(adc_data[ACCELPIN_2] - - ACCEL2_OFFSET) * - 100 / (ACCEL2_MAX_VAL - ACCEL2_OFFSET); - // printf("Accel 2: %d\r\n",max_pedal2); + uint16_t accel_val1 = adjust_pedal_val( + adc_data[ACCELPIN_1], ACCEL1_OFFSET, ACCEL1_MAX_VAL); + uint16_t accel_val2 = adjust_pedal_val( + adc_data[ACCELPIN_2], ACCEL2_OFFSET, ACCEL2_MAX_VAL); uint16_t accel_val = (uint16_t)(accel_val1 + accel_val2) / 2; - // printf("Avg Pedal Val: %d\r\n\n", accel_val); - - /* Raw ADC for tuning */ - // printf("Accel 1: %ld\r\n", adc_data[ACCELPIN_1]); - // printf("Accel 2: %ld\r\n", adc_data[ACCELPIN_2]); - - /* Brakelight Control */ - // printf("Brake 1: %ld\r\n", adc_data[BRAKEPIN_1]); - // printf("Brake 2: %ld\r\n", adc_data[BRAKEPIN_2]); - - static float buffer[10] = { 0 }; - uint16_t brake_avg = - (adc_data[BRAKEPIN_1] + adc_data[BRAKEPIN_2]) / 2; - // Add the new value to the buffer - buffer[index] = brake_avg; - - // Increment the index, wrapping around if necessary - index = (index + 1) % buffer_size; - - // Calculate the average of the buffer - float sum = 0.0; - for (int i = 0; i < buffer_size; ++i) { - sum += buffer[i]; - } - float average_brake = sum / buffer_size; - - is_braking = average_brake > PEDAL_BRAKE_THRESH; + is_braking = ((adc_data[BRAKEPIN_1] + adc_data[BRAKEPIN_2]) / + 2) > PEDAL_BRAKE_THRESH; brake_state = is_braking; osMessageQueuePut(brakelight_signal, &is_braking, 0U, 0U); - // osMessageQueueReset(break_state_queue); - // osMessageQueuePut(break_state_queue, &is_braking, 0U, 0U); /* Low Pass Filter */ sensor_data.accelerator_value = - (sensor_data.accelerator_value + (accel_val)) / - num_samples; + (sensor_data.accelerator_value + (accel_val)) / 2; sensor_data.brake_value = - average_brake / - 10.0; // still divide by 10 since we multiple by 10 on other end + (sensor_data.brake_value + + (adc_data[BRAKEPIN_1] + adc_data[BRAKEPIN_2]) / 2) / + num_samples; /* Publish to Onboard Pedals Queue */ - // printf("Accel pedal queue %d", sensor_data.accelerator_value); osStatus_t check = osMessageQueuePut(pedal_data_queue, &sensor_data, 0U, 0U); @@ -429,8 +401,6 @@ void vTsmsMonitor(void *pv_params) for (;;) { /* If we got a reliable TSMS reading, handle transition to and out of ACTIVE*/ if (!read_tsms_sense(pdu, &tsms_status)) { - printf("Checking pdu"); - // Timer has not been started, and there is a change in TSMS, so start the timer if (tsms != tsms_status && !is_timer_active(&tsms_debounce_timer)) { @@ -477,6 +447,8 @@ void vFusingMonitor(void *pv_params) uint8_t fuse_2; } fuse_data; + bool tsms_status = false; + for (;;) { fuse_buf = 0; @@ -506,6 +478,14 @@ void vFusingMonitor(void *pv_params) queue_fault(&fault_data); } + /* If we got a reliable TSMS reading, handle transition to and out of ACTIVE*/ + if (!read_tsms_sense(pdu, &tsms_status)) { + tsms = tsms_status; + if (get_func_state() == ACTIVE && tsms == 0) { + set_home_mode(); + } + } + osDelay(FUSES_SAMPLE_DELAY); } } diff --git a/Core/Src/state_machine.c b/Core/Src/state_machine.c index 39bcb73..30aecf2 100644 --- a/Core/Src/state_machine.c +++ b/Core/Src/state_machine.c @@ -6,7 +6,9 @@ #include "queues.h" #include "monitor.h" #include "serial_monitor.h" -#include +#include "nero.h" +#include "pdu.h" +#include "queues.h" #include #include @@ -91,7 +93,6 @@ static int transition_drive_state(drive_state_t new_state) if (!get_brake_state()) { return 0; } - serial_print("CALLING RTDS"); sound_rtds(pdu); } diff --git a/Core/Src/steeringio.c b/Core/Src/steeringio.c index 6abaab9..c828987 100644 --- a/Core/Src/steeringio.c +++ b/Core/Src/steeringio.c @@ -106,7 +106,6 @@ static void debounce_cb(void *arg) steeringio_t *wheel = args->wheel; if (wheel->raw_buttons[button]) { - serial_print("%d index pressed \r\n", button); switch (button) { case STEERING_PADDLE_LEFT: paddle_left_cb(); @@ -198,7 +197,6 @@ void vSteeringIORouter(void *pv_params) status = osMessageQueueGet(steeringio_router_queue, &message, NULL, osWaitForever); if (status == osOK) { - printf("joe mama"); steeringio_update(wheel, message.data); } } diff --git a/Core/Src/torque.c b/Core/Src/torque.c index c00db1f..466eea5 100644 --- a/Core/Src/torque.c +++ b/Core/Src/torque.c @@ -20,10 +20,8 @@ #include #include #include - -// TODO: Might want to make these more dynamic to account for MechE tuning -#define MIN_PEDAL_VAL 0x1DC /* Raw ADC */ -#define MAX_PEDAL_VAL 0x283 /* Raw ADC */ +#include "bms.h" +#include "emrax.h" /* DO NOT ATTEMPT TO SEND TORQUE COMMANDS LOWER THAN THIS VALUE */ #define MIN_COMMAND_FREQ 60 /* Hz */ @@ -38,10 +36,11 @@ const osThreadAttr_t torque_calc_attributes = { .priority = (osPriority_t)osPriorityRealtime2 }; -static void linear_accel_to_torque(float accel, uint16_t *torque) +static void linear_accel_to_torque(float accel) { /* Linearly map acceleration to torque */ - *torque = (uint16_t)(accel * MAX_TORQUE); + int16_t torque = (int16_t)(accel * MAX_TORQUE); + dti_set_torque(torque); } static float rpm_to_mph(int32_t rpm) @@ -55,49 +54,31 @@ static float rpm_to_mph(int32_t rpm) return (rpm / (GEAR_RATIO)) * 60 * (TIRE_DIAMETER / 63360.0) * M_PI; } -static void limit_accel_to_torque(float mph, float accel, uint16_t *torque) -{ - static uint16_t torque_accumulator[ACCUMULATOR_SIZE]; - - uint16_t newVal; - //Results in a value from 0.5 to 0 (at least halving the max torque at all times in pit or reverse) - if (mph > PIT_MAX_SPEED) { - newVal = - 0; // If we are going too fast, we don't want to apply any torque to the moving average - } else { - float torque_derating_factor = fabs( - 0.5 + - ((-0.5 / PIT_MAX_SPEED) * - mph)); // Linearly derate torque from 0.5 to 0 as speed increases - newVal = accel * torque_derating_factor; - } - - // The following code is a moving average filter - uint16_t ave = 0; - uint16_t temp[ACCUMULATOR_SIZE]; - memcpy(torque_accumulator, temp, - ACCUMULATOR_SIZE); // Copy the old values to the new array and shift them by one - for (int i = 0; i < ACCUMULATOR_SIZE - 1; i++) { - temp[i + 1] = torque_accumulator[i]; - ave += torque_accumulator[i + 1]; - } - ave += newVal; // Add the new value to the sum - ave /= ACCUMULATOR_SIZE; // Divide by the number of values to get the average - temp[0] = newVal; // Add the new value to the array - if (*torque > ave) { - *torque = - ave; // If the new value is greater than the average, set the torque to the average - } - memcpy(temp, torque_accumulator, - ACCUMULATOR_SIZE); // Copy the new array back to the old array to set the moving average -} - -static void paddle_accel_to_torque(float accel, uint16_t *torque) -{ - *torque = (uint16_t)torque_limit_percentage * - ((accel / MAX_TORQUE) * 0xFFFF); - //TODO add regen logic -} +// static void limit_accel_to_torque(float mph, float accel, uint16_t* torque) +// { +// static uint16_t torque_accumulator[ACCUMULATOR_SIZE]; + +// uint16_t newVal; +// //Results in a value from 0.5 to 0 (at least halving the max torque at all times in pit or reverse) +// if (mph > PIT_MAX_SPEED) { +// newVal = 0; // If we are going too fast, we don't want to apply any torque to the moving average +// } +// else { +// float torque_derating_factor = fabs(0.5 + ((-0.5/PIT_MAX_SPEED) * mph)); // Linearly derate torque from 0.5 to 0 as speed increases +// newVal = accel * torque_derating_factor; +// } + +// // The following code is a moving average filter +// uint16_t ave = 0; +// uint16_t temp[ACCUMULATOR_SIZE]; +// ave += newVal; // Add the new value to the sum +// ave /= ACCUMULATOR_SIZE; // Divide by the number of values to get the average +// temp[0] = newVal; // Add the new value to the array +// if(*torque > ave) { +// *torque = ave; // If the new value is greater than the average, set the torque to the average +// } +// memcpy(temp, torque_accumulator, ACCUMULATOR_SIZE); // Copy the new array back to the old array to set the moving average +// } void increase_torque_limit() { @@ -117,16 +98,82 @@ void decrease_torque_limit() } } +/* Comment out to use single pedal mode */ +//#define USE_BRAKE_REGEN 1 + +/** + * @brief Torque calculations for efficiency mode. If the driver is braking, do regenerative braking. + * + * @param mc pointer to struct containing dti data + * @param mph mph of the car + * @param accel_val adjusted value of the acceleration pedal + * @param brake_val adjusted value of the brake pedal + * @param torque pointer to torque value + */ +void handle_endurance(dti_t *mc, float mph, float accel_val, float brake_val) +{ + /* Maximum AC braking current */ + static const int8_t max_curr = 20; +#ifdef USE_BRAKE_REGEN + // The brake travel ADC value at which we want maximum regen + static const float travel_scaling_max = 1000; + if (brake_val > 650 && (mph * 1.609) > 6) { + // % of max brake pressure * ac current limit + float brake_current = + (brake_val / travel_scaling_max) * max_ac_brake; + if (brake_current > max_ac_brake) { + // clamp for safety + brake_current = max_ac_brake; + } + + // current must be delivered to DTI as a multiple of 10 + dti_set_brake_current((uint16_t)(brake_current * 10)); + *torque = 0; + } else { + // accelerating, limit torque + linear_accel_to_torque(accel_val, torque); + *torque = (uint16_t)(*torque); + } +#else + // percent of accel pedal to be used for regen + static const float regen_thresh = 0.01; + static const float accel_thresh = 0.05; + static const float mph_to_kmh = 1.609; + /* Pedal is in acceleration range. Set forward torque target. */ + if (accel_val >= accel_thresh) { + /* Coefficient to map accel pedal travel % to the max torque */ + const float coeff = MAX_TORQUE / (1 - accel_thresh); + /* Makes acceleration pedal more sensitive since domain is compressed but range is the same */ + uint16_t torque = + coeff * accel_val - (accel_val * accel_thresh); + + if (torque > MAX_TORQUE) { + torque = MAX_TORQUE; + } + + dti_set_torque(torque); + } else if (mph * mph_to_kmh > 2 && accel_val <= regen_thresh) { + float regen_current = + (max_curr / regen_thresh) * (regen_thresh - accel_val); + /* Send regen current to motor controller */ + dti_set_regen((uint16_t)(regen_current * 10)); + } else { + /* Pedal travel is between thresholds, so there should not be acceleration or braking */ + dti_set_torque(0); + } + +#endif +} + 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 = 0; float mph = 0; osStatus_t stat; - bool motor_disabled = false; + // bool motor_disabled = false; dti_t *mc = (dti_t *)pv_params; @@ -134,22 +181,25 @@ void vCalcTorque(void *pv_params) stat = osMessageQueueGet(pedal_data_queue, &pedal_data, 0U, delay_time); + /* Sometimes, the pedal travel jumps to 1% even if it is not pressed. */ + if (pedal_data.accelerator_value == 1) { + pedal_data.accelerator_value = 0; + } + float accelerator_value = - (float)pedal_data.accelerator_value / 10.0; // 0 to 1 + (float)pedal_data.accelerator_value / 100.0; // 0 to 1 float brake_value = (float)pedal_data.brake_value * 10.0; // ACTUAL PSI /* If we receive a new message within the time frame, calc new torque */ if (stat == osOK) { int32_t rpm = dti_get_rpm(mc); - //printf("rpm %ld", rpm); mph = rpm_to_mph(rpm); - //printf("mph %d", (int8_t) mph); set_mph(mph); func_state_t func_state = get_func_state(); if (func_state != ACTIVE) { - torque = 0; + dti_set_torque(0); continue; } @@ -157,55 +207,45 @@ void vCalcTorque(void *pv_params) to the motor(s). Re-enable when accelerator has less than 5% pedal travel. */ //fault_data_t fault_data = { .id = BSPD_PREFAULT, .severity = DEFCON5 }; /* 600 is an arbitrary threshold to consider the brakes mechanically activated */ - if (brake_value > 600 && (accelerator_value) > 0.25) { - printf("\n\n\n\rENTER MOTOR DISABLED\r\n\n\n"); - motor_disabled = true; - torque = 0; - } - - if (motor_disabled) { - printf("\nMotor disabled\n"); - if (accelerator_value < 0.05) { - motor_disabled = false; - printf("\n\nMotor reenabled, queuing fault\n\n"); - //queue_fault(&fault_data); - } else { - torque = 0; - dti_set_torque(torque); - continue; - } - } - - drive_state_t drive_state = - AUTOCROSS; //get_drive_state(); + // if (brake_value > 600 && (accelerator_value) > 0.25) + // { + // printf("\n\n\n\rENTER MOTOR DISABLED\r\n\n\n"); + // motor_disabled = true; + // dti_set_torque(0); + // //queue_fault(&fault_data); + // } + + // if (motor_disabled) + // { + // printf("\nMotor disabled\n"); + // if (accelerator_value < 0.05) + // { + // motor_disabled = false; + // printf("\n\nMotor reenabled\n\n"); + // } + // } + + drive_state_t drive_state = get_drive_state(); switch (drive_state) { - case REVERSE: - limit_accel_to_torque(mph, accelerator_value, - &torque); - break; - case SPEED_LIMITED: - limit_accel_to_torque(mph, accelerator_value, - &torque); - break; + // case REVERSE: + // limit_accel_to_torque(mph, accelerator_value, &torque); + // dti_set_torque(torque); + // break; + // case SPEED_LIMITED: + // limit_accel_to_torque(mph, accelerator_value, &torque); + // break; case ENDURANCE: - paddle_accel_to_torque(accelerator_value, - &torque); + handle_endurance(mc, mph, accelerator_value, + brake_value); break; case AUTOCROSS: - linear_accel_to_torque(accelerator_value, - &torque); + linear_accel_to_torque(accelerator_value); break; default: - torque = 0; + dti_set_torque(0); break; } - - // serial_print("accel val: %d\r\n", pedal_data.accelerator_value); - - serial_print("torque: %d\r\n", torque); - /* Send whatever torque command we have on record */ - dti_set_torque(torque); } } } diff --git a/Drivers/Embedded-Base b/Drivers/Embedded-Base index e3359df..78ae0e0 160000 --- a/Drivers/Embedded-Base +++ b/Drivers/Embedded-Base @@ -1 +1 @@ -Subproject commit e3359dff1d54a7abdb0890911828ce2fc4f86513 +Subproject commit 78ae0e095153757b5d0e00592e18d4aed4e8fd4d diff --git a/torque_calc_sim.py b/torque_calc_sim.py new file mode 100644 index 0000000..8f5dd6d --- /dev/null +++ b/torque_calc_sim.py @@ -0,0 +1,100 @@ +# copy constants from code cerberus_conf +ACCEL1_MAX_VAL = 1866 +ACCEL1_OFFSET = 980 +ACCEL2_MAX_VAL = 3365 +ACCEL2_OFFSET = 1780 + +# pedal values +pedal_value_1 = 1866 +pedal_value_2 = 3365 + +# PEDAL1_TEN_PERCENT_OF_MAX = (ACCEL1_MAX_VAL - ACCEL1_OFFSET) * 0.00 + ACCEL1_OFFSET +# PEDAL2_TEN_PERCENT_OF_MAX = (ACCEL2_MAX_VAL - ACCEL2_OFFSET) * 0.00 + ACCEL2_OFFSET +# +# pedal_value_1 = PEDAL1_TEN_PERCENT_OF_MAX +# pedal_value_2 = PEDAL2_TEN_PERCENT_OF_MAX + + +# what is done in monitor.c, should clamp to 0 to 100 +pedal_value_1 = 0 if (pedal_value_1 - ACCEL1_OFFSET <= 0) else (pedal_value_1 - ACCEL1_OFFSET )*100 / (ACCEL1_MAX_VAL - ACCEL1_OFFSET) +print("Clamped 1", pedal_value_1) +pedal_value_2= 0 if (pedal_value_2 - ACCEL2_OFFSET <= 0) else (pedal_value_2 - ACCEL2_OFFSET )*100 / (ACCEL2_MAX_VAL - ACCEL2_OFFSET) +print("Clamped 2", pedal_value_2) + +accelerator_value = (pedal_value_1+pedal_value_2) /2.0 + + + +# some stuff done in torque calc main thread +accelerator_value = accelerator_value/100.0 +print("Accel val", accelerator_value) + +# copy the torque multipliers, one for forwards and one for regen +TORQUE_FACTOR_FORWARDS = 220 +TORQUE_FACTOR_BACKWARDS = 50 + +# AUTOCROSS +torque = accelerator_value*TORQUE_FACTOR_FORWARDS +print("Normal torque", accelerator_value) + +#print("Ac amps", (torque/0.61) ) + +# ENDURANCE +max_ac_brake = 15; # Amps AC +OFFSET_VALUE_REGEN = 0.2 +OFFSET_VALUE_ACCEL = 0.25 + +regen_torque = -1; +if accelerator_value >= OFFSET_VALUE_ACCEL: + print("Offset accel go forward", accelerator_value) + accelerator_value *= (1 / (1 - OFFSET_VALUE_ACCEL)) + torque = accelerator_value*TORQUE_FACTOR_FORWARDS +elif accelerator_value < OFFSET_VALUE_REGEN: + regen_factor = max_ac_brake / (OFFSET_VALUE_REGEN*100) + print("Offset accel go backward", accelerator_value) + regen_torue = (accelerator_value*-1.0*regen_factor) * ((1 / OFFSET_VALUE) + (1 / (1 - OFFSET_VALUE))) *10 + if (torque/0.61) > max_ac_brake: + torque = max_ac_brake * 0.61; +else: + torque = 0 + regen = 0 + +print("Ac amps", (torque/0.61) ) + +# for num in range(-1,101,1): +# PEDAL1_TEN_PERCENT_OF_MAX = (ACCEL1_MAX_VAL - ACCEL1_OFFSET) * (num / 100.0) + ACCEL1_OFFSET +# PEDAL2_TEN_PERCENT_OF_MAX = (ACCEL2_MAX_VAL - ACCEL2_OFFSET) * (num / 100.0) + ACCEL2_OFFSET +# +# pedal_value_1 = PEDAL1_TEN_PERCENT_OF_MAX +# pedal_value_2 = PEDAL2_TEN_PERCENT_OF_MAX +# +# +# # what is done in monitor.c, should clamp to 0 to 100 +# pedal_value_1 = 0 if (pedal_value_1 - ACCEL1_OFFSET <= 0) else (pedal_value_1 - ACCEL1_OFFSET )*100 / (ACCEL1_MAX_VAL - ACCEL1_OFFSET) +# pedal_value_2= 0 if (pedal_value_2 - ACCEL2_OFFSET <= 0) else (pedal_value_2 - ACCEL2_OFFSET )*100 / (ACCEL2_MAX_VAL - ACCEL2_OFFSET) +# +# accelerator_value = (pedal_value_1+pedal_value_2) /2.0 +# print("Accel val", accelerator_value) +# +# # copy the torque multipliers, one for forwards and one for regen +# TORQUE_FACTOR_FORWARDS = 22 +# +# # AUTOCROSS +# torque = accelerator_value*TORQUE_FACTOR_FORWARDS +# +# # ENDURANCE +# max_ac_brake = 25; # Amps AC +# OFFSET_VALUE = 0.2 +# regen_factor = max_ac_brake / (OFFSET_VALUE * 100) +# +# accelerator_value = accelerator_value - (100*OFFSET_VALUE) +# if accelerator_value >= 0: +# accelerator_value *= (1 / (1 - OFFSET_VALUE)) +# torque = accelerator_value*TORQUE_FACTOR_FORWARDS +# else: +# torque = (accelerator_value*-1.0*regen_factor) * ((1 / OFFSET_VALUE) + (1 / (1 - OFFSET_VALUE))) +# if (torque/0.61)/10 > max_ac_brake: +# torque = max_ac_brake * 0.61 * 10; +# +# +# print("Ac amps", (torque/10.0)/0.61 , "\n")