Add cascading PID to the balance app

[vedderb]

I have been thinking about how to deal with all the parameters, especially after realizing that @surfdado also needs a whole set of new parameters and probably a different app for balance skateboards as they are quite different. All of that will not fit in the appconfig the way it is now and changing the sizes will slowly eat all the ram that is left as each parameter in appconfig needs to have probably at least 10x the space as there are so many interfaces that can send things at the same time. It would also break ble-modules out there now as they also need larger buffers.

One way to approach this would be to only expose the very generic parameters in the appconf that normal users are likely to tweak (which probably includes 90% of all parameters). The rest of the parameters could be configured using a lisp script that runs on each boot (all of them do). All you have to do then is to add a few lisp extensions to the balance app similar to this: https://github.com/vedderb/bldc/blob/master/lispBM/README.md#configuration Then when you have a config that works and you want to share it you just share that script as a text file.

This also has a few other advantages:

• You don't need to make changes to VESC Tool when you add or change parameters.
• You won't break compatibility with the firmware version, meaning that you can change the balance app on the stable release with new parameters when you feel like it without impacting the rest.
• You can make it so that you can update the parameters without stopping the motor, which could be interesting for tuning parameters live. One way to do that would be to add some potentiometers to the ADCs that you read from lisp and update the parameters as you go. You can also make a qml-app that interacts with the lisp app with some sliders for tuning.
• If you want to make a balance setup ui in qml it is quite easy to make that interact with the lisp code to change parameters, and as it is completely independent you can update and maintain it without being bound by the firmware releases.

I can start on it for a few parameters if it sounds interesting so that you can see what I mean, and if it looks good you can proceed with the rest of them.

[Mitchlol]

I'm not really sure I see a clean way to split parameters. Would it make sense to move all the parameters? What about rewriting the balance app as a lisp script? Can they be apps or are they just configuration scripts? I don't know anything about this stuff 🤯

I don't think I plan on adding a ton of more parameters. In terms of features it's pretty good, really just trying to get performance as good as I can. If we split onewheeled skateboards and unicycles into 2 apps, I can remove a bunch of features from this one. Tho I'm not so sure that it needs to be 2 different apps.

[vedderb]

The entire app can be a lisp-script too, but I think performance will be a bit of a problem since you are already pushing it a bit in C with the high rates and all the calculations. Best is probably a combination where you have the fast control loops running in C and the parameters and maybe some other code running in lisp that you want to be able to tweak conveniently. The nice thing with the lisp scripts is that you can edit and debug them live from VESC Tool without recompiling the STM code.

If both can be the same app and you and @surfdado can work out how to make that happen that would be the best.

So I have been playing a bit with lisp and a way to set the parameters. Here is a lisp script where I describe all of the parameters, their values and their type. I also made some code generators in lisp so that you can generate C code from them to avoid copy-paste-errors.

(def bal-param '(
    ; You can write comments in here to describe the parameters if you want to
    (f32 "kp" 0)
    (f32 "ki" 0)
    (f32 "kd" 0)
    (u16 "hertz" 1000)
    (u16 "loop_time_filter" 0)
    (f32 "fault_pitch" 20)
    (f32 "fault_roll" 45)
    (f32 "fault_duty" 0)
    (f32 "fault_adc1" 0)
    (f32 "fault_adc2" 0)
    (u16 "fault_delay_pitch" 0)
    (u16 "fault_delay_roll" 0)
    (u16 "fault_delay_duty" 0)
    (u16 "fault_delay_switch_half" 0)
    (u16 "fault_delay_switch_full" 0)
    (u16 "fault_adc_half_erpm" 1000)
    (f32 "tiltback_duty_angle" 10)
    (f32 "tiltback_duty_speed" 3)
    (f32 "tiltback_duty" 0.75)
    (f32 "tiltback_hv_angle" 10)
    (f32 "tiltback_hv_speed" 3)
    (f32 "tiltback_hv" 200)
    (f32 "tiltback_lv_angle" 10)
    (f32 "tiltback_lv_speed" 3)
    (f32 "tiltback_lv" 0)
    (f32 "tiltback_return_speed" 2)
    (f32 "tiltback_constant" 0)
    (u16 "tiltback_constant_erpm" 500)
    (f32 "tiltback_variable" 0)
    (f32 "tiltback_variable_max" 0)
    (f32 "noseangling_speed" 3)
    (f32 "startup_pitch_tolerance" 20)
    (f32 "startup_roll_tolerance" 8)
    (f32 "startup_speed" 30)
    (f32 "deadzone" 0)
    (b "multi_esc" 0)
    (f32 "yaw_kp" 0)
    (f32 "yaw_ki" 0)
    (f32 "yaw_kd" 0)
    (f32 "roll_steer_kp" 0)
    (f32 "roll_steer_erpm_kp" 0)
    (f32 "brake_current" 0)
    (u16 "brake_timeout" 10)
    (f32 "yaw_current_clamp" 0)
    (u16 "kd_pt1_lowpass_frequency" 0)
    (u16 "kd_pt1_highpass_frequency" 0)
    (f32 "kd_biquad_lowpass" 0)
    (f32 "kd_biquad_highpass" 0)
    (f32 "booster_angle" 8)
    (f32 "booster_ramp" 1)
    (f32 "booster_current" 0)
    (f32 "torquetilt_start_current" 10)
    (f32 "torquetilt_angle_limit" 5)
    (f32 "torquetilt_on_speed" 5)
    (f32 "torquetilt_off_speed" 3)
    (f32 "torquetilt_strength" 0)
    (f32 "torquetilt_filter" 2)
    (f32 "turntilt_strength" 0)
    (f32 "turntilt_angle_limit" 5)
    (f32 "turntilt_start_angle" 1)
    (u16 "turntilt_start_erpm" 100)
    (f32 "turntilt_speed" 5)
    (u16 "turntilt_erpm_boost" 20)
    (u16 "turntilt_erpm_boost_end" 20000)
))

(defun name-to-index (name)
    (looprange i 0 (length bal-param)
        (if (eq name (ix (ix bal-param i) 1)) (break i) -1)
))

; Function to set all parameters. Could be used at boot
(defun set-all-parameters ()
    (looprange i 0 (length bal-param)
        (bal-set-param i (ix (ix bal-param i) 2))
))

; Set single parameter. Could be used during tuning
(defun set-single-para (name val)
    (bal-set-param (name-to-index name) val)
)

; Functions below can be used to generate C code from the parameters

(defun gen-decoder ()
    (progn
        (print "switch (lbm_dec_as_i32(args[0])) {")
        (looprange i 0 (length bal-param)
            (let (
                (p-type (ix (ix bal-param i) 0))
                (p-name (ix (ix bal-param i) 1))
                (setter (match p-type
                            (u16 "lbm_dec_as_i32(args[1])")
                            (f32 "lbm_dec_as_float(args[1])")
                            (b "lbm_dec_as_i32(args[1])")
                )))
                (print (str-merge (str-from-n i "case %d: balance_conf.") p-name " = " setter "; break;"))
        ))
        (print "default: break;")
        (print "}")
))

(defun gen-struct ()
    (progn
        (print "typedef struct {")
        (loopforeach p bal-param
            (let (
            (p-type (ix p 0))
            (p-name (ix p 1))
            (type-name (match p-type
                            (f32 "float")
                            (u16 "uint16_t")
                            (b "bool")
            )))
        (print (str-merge "\t" type-name " " p-name ";"))
        ))
        (print "} balance_config;")
))

(defun gen-initializer ()
    (loopforeach p bal-param
        (print (str-merge "balance_conf." (ix p 1) " = " (str-from-n (ix p 2)) ";"))
))

; Optional signature by summing the sum of all characters multiplied with their index
; could use this to make sure that the order of the parameters is ok. The character sum
; below is very inefficient this way, but that does not really matter if you only do this
; once.
(def signature
    (let (
        (sum-chars (fn (str)
            (apply + (map (fn (x) (bufget-u8 x 0)) (str-split str 0)))))

        (calc-helper (fn (sum i)
            (if (= i (length bal-param))
                sum
                (calc-helper
                    (+ sum (* (+ i 1) (sum-chars (ix (ix bal-param i) 1))))
                    (+ i 1))
)))) (calc-helper 0 0)))

In the balance-app you just need to add an extension to get the bal-set-param function from lisp. Here is what you need for that:

#include "lispif.h"
#include "lispbm.h"

static lbm_value ext_set_param(lbm_value *args, lbm_uint argn) {
    if (argn != 2 || !lbm_is_number(args[0]) || !lbm_is_number(args[1])) {
        return lbm_enc_sym(SYM_EERROR);
    }

    // This switch-statement is generated from the lisp-code, so it should be consistent
    switch (lbm_dec_as_i32(args[0])) {
    case 0: balance_conf.kp = lbm_dec_as_float(args[1]); break;
    case 1: balance_conf.ki = lbm_dec_as_float(args[1]); break;
    case 2: balance_conf.kd = lbm_dec_as_float(args[1]); break;
    case 3: balance_conf.hertz = lbm_dec_as_i32(args[1]); break;
    case 4: balance_conf.loop_time_filter = lbm_dec_as_i32(args[1]); break;
    case 5: balance_conf.fault_pitch = lbm_dec_as_float(args[1]); break;
    case 6: balance_conf.fault_roll = lbm_dec_as_float(args[1]); break;
    case 7: balance_conf.fault_duty = lbm_dec_as_float(args[1]); break;
    case 8: balance_conf.fault_adc1 = lbm_dec_as_float(args[1]); break;
    case 9: balance_conf.fault_adc2 = lbm_dec_as_float(args[1]); break;
    case 10: balance_conf.fault_delay_pitch = lbm_dec_as_i32(args[1]); break;
    case 11: balance_conf.fault_delay_roll = lbm_dec_as_i32(args[1]); break;
    case 12: balance_conf.fault_delay_duty = lbm_dec_as_i32(args[1]); break;
    case 13: balance_conf.fault_delay_switch_half = lbm_dec_as_i32(args[1]); break;
    case 14: balance_conf.fault_delay_switch_full = lbm_dec_as_i32(args[1]); break;
    case 15: balance_conf.fault_adc_half_erpm = lbm_dec_as_i32(args[1]); break;
    case 16: balance_conf.tiltback_duty_angle = lbm_dec_as_float(args[1]); break;
    case 17: balance_conf.tiltback_duty_speed = lbm_dec_as_float(args[1]); break;
    case 18: balance_conf.tiltback_duty = lbm_dec_as_float(args[1]); break;
    case 19: balance_conf.tiltback_hv_angle = lbm_dec_as_float(args[1]); break;
    case 20: balance_conf.tiltback_hv_speed = lbm_dec_as_float(args[1]); break;
    case 21: balance_conf.tiltback_hv = lbm_dec_as_float(args[1]); break;
    case 22: balance_conf.tiltback_lv_angle = lbm_dec_as_float(args[1]); break;
    case 23: balance_conf.tiltback_lv_speed = lbm_dec_as_float(args[1]); break;
    case 24: balance_conf.tiltback_lv = lbm_dec_as_float(args[1]); break;
    case 25: balance_conf.tiltback_return_speed = lbm_dec_as_float(args[1]); break;
    case 26: balance_conf.tiltback_constant = lbm_dec_as_float(args[1]); break;
    case 27: balance_conf.tiltback_constant_erpm = lbm_dec_as_i32(args[1]); break;
    case 28: balance_conf.tiltback_variable = lbm_dec_as_float(args[1]); break;
    case 29: balance_conf.tiltback_variable_max = lbm_dec_as_float(args[1]); break;
    case 30: balance_conf.noseangling_speed = lbm_dec_as_float(args[1]); break;
    case 31: balance_conf.startup_pitch_tolerance = lbm_dec_as_float(args[1]); break;
    case 32: balance_conf.startup_roll_tolerance = lbm_dec_as_float(args[1]); break;
    case 33: balance_conf.startup_speed = lbm_dec_as_float(args[1]); break;
    case 34: balance_conf.deadzone = lbm_dec_as_float(args[1]); break;
    case 35: balance_conf.multi_esc = lbm_dec_as_i32(args[1]); break;
    case 36: balance_conf.yaw_kp = lbm_dec_as_float(args[1]); break;
    case 37: balance_conf.yaw_ki = lbm_dec_as_float(args[1]); break;
    case 38: balance_conf.yaw_kd = lbm_dec_as_float(args[1]); break;
    case 39: balance_conf.roll_steer_kp = lbm_dec_as_float(args[1]); break;
    case 40: balance_conf.roll_steer_erpm_kp = lbm_dec_as_float(args[1]); break;
    case 41: balance_conf.brake_current = lbm_dec_as_float(args[1]); break;
    case 42: balance_conf.brake_timeout = lbm_dec_as_i32(args[1]); break;
    case 43: balance_conf.yaw_current_clamp = lbm_dec_as_float(args[1]); break;
    case 44: balance_conf.kd_pt1_lowpass_frequency = lbm_dec_as_i32(args[1]); break;
    case 45: balance_conf.kd_pt1_highpass_frequency = lbm_dec_as_i32(args[1]); break;
    case 46: balance_conf.kd_biquad_lowpass = lbm_dec_as_float(args[1]); break;
    case 47: balance_conf.kd_biquad_highpass = lbm_dec_as_float(args[1]); break;
    case 48: balance_conf.booster_angle = lbm_dec_as_float(args[1]); break;
    case 49: balance_conf.booster_ramp = lbm_dec_as_float(args[1]); break;
    case 50: balance_conf.booster_current = lbm_dec_as_float(args[1]); break;
    case 51: balance_conf.torquetilt_start_current = lbm_dec_as_float(args[1]); break;
    case 52: balance_conf.torquetilt_angle_limit = lbm_dec_as_float(args[1]); break;
    case 53: balance_conf.torquetilt_on_speed = lbm_dec_as_float(args[1]); break;
    case 54: balance_conf.torquetilt_off_speed = lbm_dec_as_float(args[1]); break;
    case 55: balance_conf.torquetilt_strength = lbm_dec_as_float(args[1]); break;
    case 56: balance_conf.torquetilt_filter = lbm_dec_as_float(args[1]); break;
    case 57: balance_conf.turntilt_strength = lbm_dec_as_float(args[1]); break;
    case 58: balance_conf.turntilt_angle_limit = lbm_dec_as_float(args[1]); break;
    case 59: balance_conf.turntilt_start_angle = lbm_dec_as_float(args[1]); break;
    case 60: balance_conf.turntilt_start_erpm = lbm_dec_as_i32(args[1]); break;
    case 61: balance_conf.turntilt_speed = lbm_dec_as_float(args[1]); break;
    case 62: balance_conf.turntilt_erpm_boost = lbm_dec_as_i32(args[1]); break;
    case 63: balance_conf.turntilt_erpm_boost_end = lbm_dec_as_i32(args[1]); break;
    default: break;
    }

    return lbm_enc_sym(SYM_TRUE);
}

static void load_lbm_extensions(void) {
    lbm_add_extension("bal-set-param", ext_set_param);
    // If you want to load more extensions than this one for other things that can be done from here
}

// You also have to register your extension loader from app_balance_configure
lispif_set_ext_load_callback(load_lbm_extensions);

You can play around with this code and see how that works for you. Otherwise we can think of some other solution.