I do not understand what you mean with "I clamped foc to stop it shuddering". Did you assign to foc angle a fixed value or did you limit the min and max to some ranges? Which value/limits did you used?
Here's the exact sed I applied to the source files
Might be different for you since I've already made some modifications
sed -i '/uint8_t[[:space:]]\+foc_tmp[[:space:]]*=/,/ui8_g_foc_angle[[:space:]]*=[[:space:]]*foc_tmp[[:space:]]*;'/c'\
uint8_t foc_tmp = (uint8_t)((((uint16_t)ui8_adc_battery_current_filtered * ui8_foc_angle_multiplicator) + 128) >> 8);\
/* HAZZA HYBRID FOC: bias + hybrid trim + cap + slew */\
const uint16_t ERPS_KNEE = (uint16_t)(MOTOR_SPEED_FIELD_WEAKENING_MIN);\
/* treat as "real load" only if duty high OR batt amps a few counts */\
uint8_t high_load = ((ui8_g_duty_cycle >= (uint8_t)(PWM_DUTY_CYCLE_MAX * 70 / 100)) || (ui8_adc_battery_current_filtered >= 6));\
/* ---------- ERPS-based trim (only when high_load) ---------- */\
uint8_t scale_pct = 100;\
if (high_load && (ui16_motor_speed_erps > ERPS_KNEE)) {\
uint16_t span = ui16_motor_speed_erps - ERPS_KNEE;\
if (span > ERPS_KNEE) span = ERPS_KNEE;\
/* 100% → 60% across ERPS_KNEE..2*ERPS_KNEE; floor 70% for stability */\
scale_pct = (uint8_t)(100 - (40 * span) / ERPS_KNEE);\
if (scale_pct < 70) scale_pct = 70;\
}\
/* ---------- low-speed base bias: extra punch with low multiplier ---------- */\
const uint16_t ERPS_BOOST_LO = (uint16_t)(ERPS_KNEE * 3 / 10); /* full bias ≤ ~30% of knee */\
const uint16_t ERPS_BOOST_HI = (uint16_t)(ERPS_KNEE * 9 / 10); /* fades to 0 by ~90% of knee */\
const uint8_t BASE_BIAS_MAX = 7; /* try 6–8 to taste */\
uint8_t base_bias = 0;\
if (high_load) {\
if (ui16_motor_speed_erps <= ERPS_BOOST_LO) base_bias = BASE_BIAS_MAX;\
else if (ui16_motor_speed_erps < ERPS_BOOST_HI) {\
uint16_t span2 = ui16_motor_speed_erps - ERPS_BOOST_LO;\
uint16_t width2 = ERPS_BOOST_HI - ERPS_BOOST_LO;\
base_bias = (uint8_t)(BASE_BIAS_MAX - (BASE_BIAS_MAX * span2) / (width2 ? width2 : 1));\
}\
}\
/* apply trim then add bias to existing foc_tmp */\
foc_tmp = (uint8_t)(((uint16_t)foc_tmp * scale_pct) / 100);\
foc_tmp = (uint8_t)(foc_tmp + base_bias);\
/* ---------- low cap + tiny slew limiter ---------- */\
uint8_t cap = 18; /* bump to 19–20 if too tame up top */\
if (foc_tmp > cap) foc_tmp = cap;\
static uint8_t hazza_prev_angle = 0;\
int8_t hazza_df = (int8_t)foc_tmp - (int8_t)hazza_prev_angle;\
if (hazza_df > 1) foc_tmp = (uint8_t)(hazza_prev_angle + 1); /* slow rise */\
else if (hazza_df < -2) foc_tmp = (uint8_t)(hazza_prev_angle - 2); /* faster pull-down */\
ui8_g_foc_angle = foc_tmp;\
hazza_prev_angle = foc_tmp;' $f
I'm going to experiment with it more see if I can make it more efficient as when it was in this go I'll wack mode I saw motor amps at 20 (not sure if this is accurate reading or)