Skip to content

Commit

Permalink
Regen Braking (#188)
Browse files Browse the repository at this point in the history
* 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 <[email protected]>

* 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 <[email protected]>

* 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 <[email protected]>

* 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 <[email protected]>
Co-authored-by: dyldonahue <[email protected]>
  • Loading branch information
3 people authored Jul 31, 2024
1 parent 4362198 commit 24a675c
Show file tree
Hide file tree
Showing 11 changed files with 340 additions and 173 deletions.
4 changes: 3 additions & 1 deletion Core/Inc/bms.h
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
11 changes: 10 additions & 1 deletion Core/Inc/cerberus_conf.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down
29 changes: 23 additions & 6 deletions Core/Inc/dti.h
Original file line number Diff line number Diff line change
Expand Up @@ -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 */
Expand All @@ -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;
Expand All @@ -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
Expand Down
4 changes: 0 additions & 4 deletions Core/Src/bms.c
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
}
}
38 changes: 31 additions & 7 deletions Core/Src/dti.c
Original file line number Diff line number Diff line change
Expand Up @@ -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()
{
Expand Down Expand Up @@ -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 } };
Expand All @@ -77,8 +100,6 @@ void dti_set_current(int16_t current)

//endian_swap(&current, sizeof(current));
//memcpy(msg.data, &current, msg.len);
serial_print("current: %d\r\n", current);

int8_t msb = (int8_t)((current >> 8) & 0xFF);
int8_t lsb = (uint8_t)(current & 0xFF);

Expand All @@ -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);
}

Expand Down
88 changes: 34 additions & 54 deletions Core/Src/monitor.c
Original file line number Diff line number Diff line change
Expand Up @@ -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 =
Expand Down Expand Up @@ -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;
Expand All @@ -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);
Expand All @@ -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);

Expand Down Expand Up @@ -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)) {
Expand Down Expand Up @@ -477,6 +447,8 @@ void vFusingMonitor(void *pv_params)
uint8_t fuse_2;
} fuse_data;

bool tsms_status = false;

for (;;) {
fuse_buf = 0;

Expand Down Expand Up @@ -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);
}
}
Expand Down
5 changes: 3 additions & 2 deletions Core/Src/state_machine.c
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,9 @@
#include "queues.h"
#include "monitor.h"
#include "serial_monitor.h"
#include <assert.h>
#include "nero.h"
#include "pdu.h"
#include "queues.h"
#include <stdbool.h>
#include <stdio.h>

Expand Down Expand Up @@ -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);
}

Expand Down
2 changes: 0 additions & 2 deletions Core/Src/steeringio.c
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -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);
}
}
Expand Down
Loading

0 comments on commit 24a675c

Please sign in to comment.