You are here

ADC modified app, problem with braking

5 posts / 0 new
Last post
dpeinado
Offline
Last seen: 3 weeks 5 days ago
Joined: 2022-01-14 11:59
Posts: 17
ADC modified app, problem with braking

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.

dpeinado
Offline
Last seen: 3 weeks 5 days ago
Joined: 2022-01-14 11:59
Posts: 17

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

dpeinado
Offline
Last seen: 3 weeks 5 days ago
Joined: 2022-01-14 11:59
Posts: 17

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

ADC_app.png

This is the graphic of the custom app.

Custom_app.png

 

Anybody has a clue of how to fix it?

Thanks.

dpeinado
Offline
Last seen: 3 weeks 5 days ago
Joined: 2022-01-14 11:59
Posts: 17

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,

261122
Offline
Last seen: 8 months 3 weeks ago
Joined: 2022-07-04 20:23
Posts: 1

thanks i will try iy