Hello.
I'm coding an app based on ADC. It uses Current no reverse brake center Mode with a hand throttle.
The idea is that when you release the hand throttle, the VESC brakes (the ADC app does it quite well), and if a movement is detected after a time out, then it brakes until the vehicle is stopped.
The code I did works quite well except in one case. If the motors are running at higher velocity than the maximum (because a slope for instance) then the initial braking does not work at all. With the ADC app it does, but with mine does not.
I will use asterisks ******************************************** pointing the place in which the braking is done.
*** This function if for applying brake current to main controller and the secondary one:
void app_set_brake_current_rel(float current_rel){
mc_interface_set_brake_current_rel(current_rel);
// Send brake command to all ESCs seen recently on the CAN bus
if (config.multi_esc) {
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
comm_can_set_current_brake_rel(msg->id, current_rel);
}
}
}
}
*** This function if for applying current to main controller and the secondary one:
void app_set_current(void){
mc_interface_set_current(brake.current_M);
if (config.multi_esc) {
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
comm_can_set_current(msg->id, brake.current_S);
}
}
}
}
static THD_FUNCTION(clean_thread, arg) {
(void)arg;
float current_rel = 0.0;
chRegSetThreadName("APP_CLEAN");
palSetPadMode(HW_UART_TX_PORT, HW_UART_TX_PIN, PAL_MODE_INPUT_PULLDOWN);
palSetPadMode(HW_UART_RX_PORT, HW_UART_RX_PIN, PAL_MODE_INPUT_PULLDOWN); // brake
is_running = true;
current_time = previous_time = chVTGetSystemTimeX();
reset_brake();
for(;;) {
contador++;
// Sleep for a time according to the specified rate
systime_t sleep_time = CH_CFG_ST_FREQUENCY / config.update_rate_hz;
// At least one tick should be slept to not block the other threads
if (sleep_time == 0) {
sleep_time = 1;
}
chThdSleep(sleep_time);
// current_time para temporizador del freno
previous_time = current_time;
current_time = chVTGetSystemTimeX();
dt = (float) ST2S(current_time-previous_time);
if (stop_now) {
is_running = false;
return;
}
// For safe start when fault codes occur
if (mc_interface_get_fault() != FAULT_CODE_NONE && config.safe_start != SAFE_START_NO_FAULT) {
ms_without_power = 0;
}
// Read the external ADC pin voltage
float pwr = ADC_VOLTS(ADC_IND_EXT);
// Optionally apply a filter
static float filter_val = 0.0;
UTILS_LP_MOVING_AVG_APPROX(filter_val, pwr, FILTER_SAMPLES);
if (config.use_filter) {
pwr = filter_val;
}
if (pwr < config.voltage_center) {
pwr = utils_map(pwr, config.voltage_start, config.voltage_center, 0.0, 0.5);
} else {
pwr = utils_map(pwr, config.voltage_center, config.voltage_end, 0.5, 1.0);
}
// Truncate the read voltage
utils_truncate_number(&pwr, 0.0, 1.0);
// Optionally invert the read voltage
if (config.voltage_inverted) {
pwr = 1.0 - pwr;
}
pwr *= 2.0;
pwr -= 1.0;
// Apply deadband
utils_deadband(&pwr, config.hyst, 1.0);
// Apply throttle curve
pwr = utils_throttle_curve(pwr, config.throttle_exp, config.throttle_exp_brake, config.throttle_exp_mode);
// para frenado leo las rpm de ambas controladoras y saco la media;
brake.erpm_M_last = brake.erpm_M;
brake.erpm_S_last = brake.erpm_S;
brake.erpm_M = mc_interface_get_rpm();
if(config.multi_esc){
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
brake.erpm_S = msg->rpm;
}
}
}
UTILS_LP_MOVING_AVG_APPROX(brake.erpm_M_filtered, brake.erpm_M, RPM_FILTER_SAMPLES);
UTILS_LP_MOVING_AVG_APPROX(brake.erpm_S_filtered, brake.erpm_S, RPM_FILTER_SAMPLES);
brake.erpm_M = brake.erpm_M_filtered;
brake.erpm_S = brake.erpm_S_filtered;
brake.abs_max_rpm = (fabsf(brake.erpm_M) > fabsf(brake.erpm_S)) ? fabsf(brake.erpm_M) : fabsf(brake.erpm_S);
//brakeSignal = palReadPad(HW_UART_RX_PORT, HW_UART_RX_PIN); // Leo la señal de frenado procedente de la MCU
if (pwr < 0.0){
brakeSignal = 1;
current_rel = fabsf(pwr);
} else {
brakeSignal = 0;
}
if (brakeSignal) {
switch (brake.state){
case FREE:
brake.state = BRAKING;
brake.timeout = current_time + S2ST(balance_conf.brake_timeout);
********** HERE IS BRAKING
*************************************************************************************************
app_set_brake_current_rel(current_rel);
*************************************************************************************************
break;
case BRAKING:
//app_set_brake_current(balance_conf.brake_current);
app_set_brake_current_rel(current_rel);
if (current_time > brake.timeout) {
if (brake.abs_max_rpm > MIN_RPM) {
brake.state = BRAKE_FAULT;
} else {
app_set_brake_current(0);
reset_brake();
brake.state = STOPPED;
}
} else {
if (brake.abs_max_rpm < MIN_RPM){
app_set_brake_current(0);
reset_brake();
brake.state = STOPPED;
}
}
break;
case STOPPED:
if (brake.abs_max_rpm > MIN_RPM) {
brake.state = BRAKE_FAULT;
}
break;
case BRAKE_FAULT:
if (brake.abs_max_rpm < MIN_RPM && brake.current_max < MIN_CURR) { //estoy parado y no tengo que hacer casi fuerza
reset_brake();
brake.state = STOPPED;
} else if (brake.abs_max_rpm < MIN_RPM){ // parado, pero en cuesta
} else { // todavía no estoy parado
brake.p_term_M = -balance_conf.kp*brake.erpm_M;
brake.p_term_S = -balance_conf.kp*brake.erpm_S;
brake.d_term_M = -balance_conf.kd*(brake.erpm_M-brake.erpm_M_last)/dt;
brake.d_term_S = -balance_conf.kd*(brake.erpm_S-brake.erpm_S_last)/dt;
brake.i_term_M -= balance_conf.ki*brake.erpm_M*dt;
brake.i_term_S -= balance_conf.ki*brake.erpm_S*dt;
// Filter D
UTILS_LP_FAST(brake.d_term_M_filtered, brake.d_term_M, balance_conf.kd_biquad_lowpass);
brake.d_term_M = brake.d_term_M_filtered;
UTILS_LP_FAST(brake.d_term_S_filtered, brake.d_term_S, balance_conf.kd_biquad_lowpass);
brake.d_term_S = brake.d_term_S_filtered;
float output_M = brake.p_term_M + brake.i_term_M + brake.d_term_M;
float output_S = brake.p_term_S + brake.i_term_S + brake.d_term_S;
utils_truncate_number_abs(&output_M, 1.0);
utils_truncate_number_abs(&output_S, 1.0);
brake.current_M = output_M*balance_conf.brake_current;
brake.current_S = output_S*balance_conf.brake_current;
brake.current_max = (brake.current_M > brake.current_S) ? brake.current_M: brake.current_S;
}
//commands_printf("\n\r M: %f S: %f", (double)brake.current_M, (double)brake.current_S);
app_set_current();
break;
}
} else { // si no está aplicado el freno, entonces algoritmo normal
// Cambio estado del freno a libre y el timeout a 0
if (brake.state != FREE){
brake.state = FREE;
reset_brake();
}
// Apply ramping
static systime_t last_time = 0;
static float pwr_ramp = 0.0;
float ramp_time = fabsf(pwr) > fabsf(pwr_ramp) ? config.ramp_time_pos : config.ramp_time_neg;
if (ramp_time > 0.01) {
const float ramp_step = (float)ST2MS(chVTTimeElapsedSinceX(last_time)) / (ramp_time * 1000.0);
utils_step_towards(&pwr_ramp, pwr, ramp_step);
last_time = chVTGetSystemTimeX();
pwr = pwr_ramp;
}
current_rel = pwr;
if (fabsf(pwr) < 0.001) {
ms_without_power += (1000.0 * (float)sleep_time) / (float)CH_CFG_ST_FREQUENCY;
}
// If safe start is enabled and the output has not been zero for long enough
if (ms_without_power < MIN_MS_WITHOUT_POWER && config.safe_start) {
static int pulses_without_power_before = 0;
if (ms_without_power == pulses_without_power_before) {
ms_without_power = 0;
}
pulses_without_power_before = ms_without_power;
mc_interface_set_brake_current(timeout_get_brake_current());
if (config.multi_esc) {
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
comm_can_set_current_brake(msg->id, timeout_get_brake_current());
}
}
}
}
// Reset timeout
timeout_reset();
// Find lowest RPM (for traction control)
float rpm_local = mc_interface_get_rpm();
float rpm_lowest = rpm_local;
if (config.multi_esc) {
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
float rpm_tmp = msg->rpm;
if (fabsf(rpm_tmp) < fabsf(rpm_lowest)) {
rpm_lowest = rpm_tmp;
}
}
}
}
float current_out = current_rel;
bool is_reverse = false;
if (current_out < 0.0) {
is_reverse = true;
current_out = -current_out;
current_rel = -current_rel;
rpm_local = -rpm_local;
rpm_lowest = -rpm_lowest;
}
// Traction control
if (config.multi_esc) {
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
if (config.tc && config.tc_max_diff > 1.0) {
float rpm_tmp = msg->rpm;
if (is_reverse) {
rpm_tmp = -rpm_tmp;
}
float diff = rpm_tmp - rpm_lowest;
if (diff < TC_DIFF_MAX_PASS) diff = 0;
if (diff > config.tc_max_diff) diff = config.tc_max_diff;
current_out = utils_map(diff, 0.0, config.tc_max_diff, current_rel, 0.0);
}
if (is_reverse) {
comm_can_set_current_rel(msg->id, -current_out);
} else {
comm_can_set_current_rel(msg->id, current_out);
}
}
}
if (config.tc) {
float diff = rpm_local - rpm_lowest;
if (diff < TC_DIFF_MAX_PASS) diff = 0;
if (diff > config.tc_max_diff) diff = config.tc_max_diff;
current_out = utils_map(diff, 0.0, config.tc_max_diff, current_rel, 0.0);
}
}
if (is_reverse) {
mc_interface_set_current_rel(-current_out);
} else {
mc_interface_set_current_rel(current_out);
}
}
//if (contador % CUANTOS_PRINT) terminal_send();
//plot_variables();
}
}As commented, the ADC app uses the same procedure
if (pwr >= 0.0) {
// if pedal assist (PAS) thread is running, use the highest current command
if (app_pas_is_running()) {
pwr = utils_max_abs(pwr, app_pas_get_current_target_rel());
}
current_rel = pwr;
} else {
current_rel = fabsf(pwr);
current_mode_brake = true;
}And after a while:
if (current_mode) {
if (current_mode_brake) {
mc_interface_set_brake_current_rel(current_rel);
// Send brake command to all ESCs seen recently on the CAN bus
if (config.multi_esc) {
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
comm_can_set_current_brake_rel(msg->id, current_rel);
}
}
}
} else {
I do not know where can be the problem, unless there is something I do not know in the main loop that makes that the braking current is not applied unless something has to be done. The point is that when the ERPMs are lower than the maximum, everything goes ok.
If somebody can help me, it I would be very grateful.
Thanks in advance, best regards.
Diego.
I continue doing tests and changes in the code, and the result is the same. mc_interface_set_brake_current_rel is called with a value of 1.0 (mc_interface_set_brake_current_rel(1.0)), but the actual braking never occurs if the erpm is more than the maximum I set in VESC-Tool, then the braking is not done. Perhaps it has to do with the algorithm related to max ERPM, but surpringsingly the ADC app works quite well. I will search some function calling that makes some actualization on overriding variables
Now I have some important clues.
The experiment is as follows. I run the motor with limited velocity. Then I give impulse to the wheel with my hand, and in the realtime data is seen two important things: one is that the duty cycle goes up the maximum value reached with full throttle. The second one is that the current drops to zero.
When using ADC app, the duty cycle drops immediately when the throttle is released. When using my custom app, the duty cycle is mantained high while giving impulse with my hands until the limited value of the duty cycle is reached.
This is the graphics of the ADC app
This is the graphic of the custom app.
Anybody has a clue of how to fix it?
Thanks.
Ok, problem solved. I got the ADC app that works nicely and then I introduced my code inside this app trying to modify as little as possible and it worked (new part was exactly the same than in the other version). I do not know yet why the other app did not work, but the last one does. So ... lesson leart ... vesc apps are meticulous crafted, so do not change anything but the essential when trying to modify one app.
I hope it serves to other people to save a lot of time!!!! Thanks,
thanks i will try iy