From 1589801ab6896abef64d533fc4cadab40d111bcf Mon Sep 17 00:00:00 2001 From: saitenntaisei Date: Wed, 28 Aug 2024 16:55:07 +0900 Subject: [PATCH] use potentiomenter for pid pos offset modification --- confgenerator.c | 3 +++ confgenerator.h | 2 +- datatypes.h | 1 + hwconf/luna/m600/mcconf_luna_m600_48V.h | 3 +++ hwconf/luna/m600/mcconf_luna_m600_60V.h | 3 +++ motor/mc_interface.c | 21 ++++++++++++++++++++- motor/mcconf_default.h | 3 +++ 7 files changed, 34 insertions(+), 2 deletions(-) diff --git a/confgenerator.c b/confgenerator.c index 5f0547a47..5f6118c1b 100644 --- a/confgenerator.c +++ b/confgenerator.c @@ -159,6 +159,7 @@ int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration * buffer_append_float32_auto(buffer, conf->p_pid_ang_div, &ind); buffer_append_float16(buffer, conf->p_pid_gain_dec_angle, 10, &ind); buffer_append_float32_auto(buffer, conf->p_pid_offset, &ind); + buffer[ind++] = conf->p_pid_offset_pot_calib; buffer_append_float16(buffer, conf->cc_startup_boost_duty, 10000, &ind); buffer_append_float32_auto(buffer, conf->cc_min_current, &ind); buffer_append_float32_auto(buffer, conf->cc_gain, &ind); @@ -495,6 +496,7 @@ bool confgenerator_deserialize_mcconf(const uint8_t *buffer, mc_configuration *c conf->p_pid_ang_div = buffer_get_float32_auto(buffer, &ind); conf->p_pid_gain_dec_angle = buffer_get_float16(buffer, 10, &ind); conf->p_pid_offset = buffer_get_float32_auto(buffer, &ind); + conf->p_pid_offset_pot_calib = buffer[ind++]; conf->cc_startup_boost_duty = buffer_get_float16(buffer, 10000, &ind); conf->cc_min_current = buffer_get_float32_auto(buffer, &ind); conf->cc_gain = buffer_get_float32_auto(buffer, &ind); @@ -827,6 +829,7 @@ void confgenerator_set_defaults_mcconf(mc_configuration *conf) { conf->p_pid_ang_div = MCCONF_P_PID_ANG_DIV; conf->p_pid_gain_dec_angle = MCCONF_P_PID_GAIN_DEC_ANGLE; conf->p_pid_offset = MCCONF_P_PID_OFFSET; + conf->p_pid_offset_pot_calib = MCCONF_P_PID_OFFSET_POT_CALIB; conf->cc_startup_boost_duty = MCCONF_CC_STARTUP_BOOST_DUTY; conf->cc_min_current = MCCONF_CC_MIN_CURRENT; conf->cc_gain = MCCONF_CC_GAIN; diff --git a/confgenerator.h b/confgenerator.h index 22401abe9..8eb000858 100644 --- a/confgenerator.h +++ b/confgenerator.h @@ -8,7 +8,7 @@ #include // Constants -#define MCCONF_SIGNATURE 2490415037 +#define MCCONF_SIGNATURE 1439595948 #define APPCONF_SIGNATURE 2099347128 // Functions diff --git a/datatypes.h b/datatypes.h index 0c756ff8f..a82058ac2 100644 --- a/datatypes.h +++ b/datatypes.h @@ -510,6 +510,7 @@ typedef struct { float p_pid_ang_div; float p_pid_gain_dec_angle; float p_pid_offset; + bool p_pid_offset_pot_calib; // Current controller float cc_startup_boost_duty; diff --git a/hwconf/luna/m600/mcconf_luna_m600_48V.h b/hwconf/luna/m600/mcconf_luna_m600_48V.h index 625ae061d..5661184a2 100644 --- a/hwconf/luna/m600/mcconf_luna_m600_48V.h +++ b/hwconf/luna/m600/mcconf_luna_m600_48V.h @@ -459,6 +459,9 @@ // Position PID Offset Angle #define MCCONF_P_PID_OFFSET 0 +// Position PID Use Potentiometer for Offset Angle Calibration +#define MCCONF_P_PID_POT_OFFSET_CALIB 0 + // Startup boost #define MCCONF_CC_STARTUP_BOOST_DUTY 0.01 diff --git a/hwconf/luna/m600/mcconf_luna_m600_60V.h b/hwconf/luna/m600/mcconf_luna_m600_60V.h index 30ca05cc6..f53d783fe 100644 --- a/hwconf/luna/m600/mcconf_luna_m600_60V.h +++ b/hwconf/luna/m600/mcconf_luna_m600_60V.h @@ -459,6 +459,9 @@ // Position PID Offset Angle #define MCCONF_P_PID_OFFSET 0 +// Position PID Use Potentiometer for Offset Angle Calibration +#define MCCONF_P_PID_POT_OFFSET_CALIB 0 + // Startup boost #define MCCONF_CC_STARTUP_BOOST_DUTY 0.01 diff --git a/motor/mc_interface.c b/motor/mc_interface.c index a2e3519d3..60071a672 100644 --- a/motor/mc_interface.c +++ b/motor/mc_interface.c @@ -1468,8 +1468,27 @@ float mc_interface_get_pid_pos_now(void) { void mc_interface_update_pid_pos_offset(float angle_now, bool store) { mc_configuration *mcconf = mempools_alloc_mcconf(); *mcconf = *mc_interface_get_configuration(); + + // Use potentiometer to calibrate the offset if enabled + if (mcconf->p_pid_offset_pot_calib) { + const app_configuration *appconf = app_get_configuration(); + const int adc_avg_window_size = 100; + float angle_potentiometer_sum = 0.0; + // moving average as a low pass filter + for (int i = 0; i < adc_avg_window_size; i++) { + float pwr = ADC_VOLTS(ADC_IND_EXT); + angle_potentiometer_sum += + utils_map(pwr, appconf->app_adc_conf.voltage_start, + appconf->app_adc_conf.voltage_end, 0.0, 360.0); + chThdSleepMilliseconds(10); + } + float angle_potentiometer = + angle_potentiometer_sum / adc_avg_window_size; + mcconf->p_pid_offset = angle_potentiometer-angle_now; + }else{ + mcconf->p_pid_offset += mc_interface_get_pid_pos_now() - angle_now; + } - mcconf->p_pid_offset += mc_interface_get_pid_pos_now() - angle_now; utils_norm_angle(&mcconf->p_pid_offset); if (store) { diff --git a/motor/mcconf_default.h b/motor/mcconf_default.h index a54f1fa5a..51a3139b7 100644 --- a/motor/mcconf_default.h +++ b/motor/mcconf_default.h @@ -185,6 +185,9 @@ #ifndef MCCONF_P_PID_OFFSET #define MCCONF_P_PID_OFFSET 0.0 // Angle offset #endif +#ifndef MCCONF_P_PID_OFFSET_POT_CALIB +#define MCCONF_P_PID_OFFSET_POT_CALIB false +#endif // Current control parameters #ifndef MCCONF_CC_GAIN