From dddd255080aaebb3cbbcc4f88371320e7b12aa44 Mon Sep 17 00:00:00 2001 From: Jacopo Date: Fri, 8 Nov 2024 18:58:34 +0100 Subject: [PATCH 1/2] AMO encoder raw value is always sent with full resolution Added debug lines for testing the calibration type 10 Update calibration type 10 procedure making more robust and removing errors at restarting --- .../src/eoappservices/EOtheMotionController.c | 6 ++-- .../embobj/plus/board/EOappEncodersReader.c | 13 ++++---- .../arch-arm/embobj/plus/mc/AbsEncoder.c | 30 ++++++++++--------- .../embobj/plus/mc/CalibrationHelperData.h | 2 +- .../arch-arm/embobj/plus/mc/Calibrators.c | 2 +- emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c | 4 --- .../eBcode/arch-arm/embobj/plus/mc/JointSet.c | 4 +-- 7 files changed, 31 insertions(+), 30 deletions(-) diff --git a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheMotionController.c b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheMotionController.c index 4b883b82d..e77ad0dde 100644 --- a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheMotionController.c +++ b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheMotionController.c @@ -1645,14 +1645,14 @@ static eOresult_t s_eo_motioncontrol_updatedPositionsFromEncoders(EOtheMotionCon return(eores_NOK_timeout); } - // read the encoders + // read the encoders for(uint8_t i=0; inumofjomos; i++) { embot::app::eth::encoder::v1::valueInfo encoder1valueinfo {}; embot::app::eth::encoder::v1::valueInfo encoder2valueinfo {}; embot::app::eth::theEncoderReader::getInstance().Read(i, encoder1valueinfo, encoder2valueinfo); - + if(eores_OK != s_eo_motioncontrol_updatePositionFromEncoder(i, &encoder1valueinfo)) { res = eores_NOK_generic; @@ -1669,7 +1669,7 @@ static eOresult_t s_eo_motioncontrol_updatedPositionsFromEncoders(EOtheMotionCon if(NULL != (jstatus = eo_entities_GetJointStatus(eo_entities_GetHandle(), i))) { jstatus->addinfo.multienc[0] = rawValsArray.rawvalues[0].val; - //jstatus->addinfo.multienc[1] = rawValsArray.rawvalues[1].val; for now do not update the raw value for the motor encoder since we are adding to it the raw planned target --> see lines 292..294 in Joint.c + jstatus->addinfo.multienc[1] = rawValsArray.rawvalues[1].val; jstatus->addinfo.multienc[2] = rawValsArray.rawvalues[0].diagnInfo; } diff --git a/emBODY/eBcode/arch-arm/embobj/plus/board/EOappEncodersReader.c b/emBODY/eBcode/arch-arm/embobj/plus/board/EOappEncodersReader.c index 1a60079d3..0c2250ae6 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/board/EOappEncodersReader.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/board/EOappEncodersReader.c @@ -780,7 +780,8 @@ extern eOresult_t eo_appEncReader_GetValue(EOappEncReader *p, uint8_t jomo, eOen static int32_t cnt = 0; rawValue = cnt++; prop.valueinfo->value[0] = s_eo_appEncReader_rescale2icubdegrees(rawValue, jomo, (eOmc_position_t)prop.descriptor->pos); -#else +#else + hal_spiencoder_position_t aux_rawvalue = 0; // if(hal_res_OK == hal_spiencoder_get_value((hal_spiencoder_t)prop.descriptor->port, &spiRawValue, &flags)) if(hal_res_OK == hal_spiencoder_get_value2((hal_spiencoder_t)prop.descriptor->port, (hal_spiencoder_position_t*)&rawValue, &diagn)) { // the spi raw reading is ok. i just need to rescale it. @@ -792,16 +793,17 @@ extern eOresult_t eo_appEncReader_GetValue(EOappEncReader *p, uint8_t jomo, eOen int16_t sectors = (int16_t)(joint->config.gearbox_E2J + 0.5f); + aux_rawvalue = rawValue; if (sectors == 32) { - rawValue = (rawValue >> 1) & 0x3FFF; + aux_rawvalue = (aux_rawvalue >> 1) & 0x3FFF; } else if (sectors == 64) { - rawValue = rawValue & 0x3FFF; + aux_rawvalue = aux_rawvalue & 0x3FFF; } - prop.valueinfo->value[0] = s_eo_appEncReader_rescale2icubdegrees(rawValue, jomo, (eOmc_position_t)prop.descriptor->pos); + prop.valueinfo->value[0] = s_eo_appEncReader_rescale2icubdegrees(aux_rawvalue, jomo, (eOmc_position_t)prop.descriptor->pos); } else { // we dont even have a valid reading from hal ..... @@ -817,7 +819,7 @@ extern eOresult_t eo_appEncReader_GetValue(EOappEncReader *p, uint8_t jomo, eOen rawdiagn = ((uint32_t)diagn.type << 16) | (diagn.info.value & 0xFF); // and calls the following for amodiag - s_eo_appEncReader_amodiag_Update(jomo, rawValue, &prop, &diagn); + s_eo_appEncReader_amodiag_Update(jomo, aux_rawvalue, &prop, &diagn); } break; @@ -1732,6 +1734,7 @@ static uint32_t s_eo_appEncReader_rescale2icubdegrees(uint32_t val_raw, uint8_t uint64_t aux = (uint64_t)val_raw* 65535; retval = aux /divider; + return(retval); } diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/AbsEncoder.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/AbsEncoder.c index b3783c3c0..7cc92c840 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/AbsEncoder.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/AbsEncoder.c @@ -40,13 +40,13 @@ #include "EOtheMAIS.h" #endif + ///////////////////////////////////////////////////////// // AbsEncoder #define AEA_MIN_SPIKE 16 //4 bitsof zero padding(aea use 12 bits) [ X X X X X X X X X X 1 0 0 0 0] (2^4) #define AEA3_MIN_SPIKE 4 //2 bitsof zero padding(aea3 use 14 bits) [ X X X X X X X X X X X X 1 0 0] (2^2) - AbsEncoder* AbsEncoder_new(uint8_t n) { @@ -77,6 +77,7 @@ void AbsEncoder_init(AbsEncoder* o) o->fake = FALSE; o->offset = 0; + o->zero = 0; o->distance = 0; o->position_last = 0; @@ -147,7 +148,7 @@ void s_AbsEncoder_set_spikes_limis(AbsEncoder* o) if(toleranceIDeg < AEA3_MIN_SPIKE) o->spike_mag_limit = AEA3_MIN_SPIKE * o->div; else - o->spike_mag_limit = toleranceIDeg *o->div; + o->spike_mag_limit = toleranceIDeg * o->div; o->spike_cnt_limit = AEA_DEFAULT_SPIKE_CNT_LIMIT; @@ -209,7 +210,7 @@ void AbsEncoder_config(AbsEncoder* o, uint8_t ID, int32_t resolution, float32_t if (!o->fake) { - o->toleranceCfg = tolerance; + o->toleranceCfg = tolerance; s_AbsEncoder_set_spikes_limis(o); } else @@ -308,20 +309,21 @@ void AbsEncoder_posvel(AbsEncoder* o, int32_t* position, int32_t* velocity) //static void AbsEncoder_position_init(AbsEncoder* o, int32_t position) static void AbsEncoder_position_init_aea(AbsEncoder* o, uint16_t position) { - if (!o->valid_first_data_cnt) { o->position_last = position; } - if (o->position_last != position) + // check difference between last and current position using minimum tolerance given by configuration + // if difference larger than tolerance we need to restart to count minimum number of valid samples (10) necessary for initializing the AbsEncoder + if (((o->position_last - position) >= o->spike_mag_limit) && ((o->position_last - position) <= -o->spike_mag_limit)) { o->valid_first_data_cnt = 0; return; } - if (++o->valid_first_data_cnt >= 3) + if (++o->valid_first_data_cnt >= 10) { o->position_last = position; o->position_sure = position; @@ -362,7 +364,7 @@ static void AbsEncoder_position_init_others(AbsEncoder* o, uint16_t position) } static void AbsEncoder_position_init(AbsEncoder* o, uint16_t position) -{ +{ if (!o) return; if (o->fake) return; @@ -453,7 +455,7 @@ void AbsEncoder_invalid(AbsEncoder* o, ae_errortype_t error_type) o->fault_state.bits.data_error = TRUE; break; } - + o->valid_first_data_cnt = 0; } @@ -466,7 +468,7 @@ BOOL AbsEncoder_is_still(AbsEncoder* o, int32_t space_window, int32_t time_windo if (++o->partial_timer > time_window) { still = abs(o->partial_space) < space_window; - + o->partial_timer = 0; o->partial_space = 0; } @@ -555,7 +557,7 @@ void AbsEncoder_update(AbsEncoder* o, uint16_t position) descriptor.par64 = o->spike_cnt; descriptor.sourcedevice = eo_errman_sourcedevice_localboard; descriptor.sourceaddress = 0; - descriptor.code = eoerror_code_get(eoerror_category_MotionControl, eoerror_value_MC_aea_abs_enc_spikes); + descriptor.code = eoerror_code_get(eoerror_category_MotionControl, eoerror_value_MC_abs_enc_spikes); eo_errman_Error(eo_errman_GetHandle(), eo_errortype_warning, NULL, NULL, &descriptor); if (o->spike_cnt > o->spike_cnt_limit) @@ -653,7 +655,7 @@ void AbsEncoder_update(AbsEncoder* o, int32_t position) descriptor.par64 = o->spike_cnt; descriptor.sourcedevice = eo_errman_sourcedevice_localboard; descriptor.sourceaddress = 0; - descriptor.code = eoerror_code_get(eoerror_category_MotionControl, eoerror_value_MC_aea_abs_enc_spikes); + descriptor.code = eoerror_code_get(eoerror_category_MotionControl, eoerror_value_MC_abs_enc_spikes); eo_errman_Error(eo_errman_GetHandle(), eo_errortype_warning, NULL, NULL, &descriptor); if (o->spike_cnt > o->spike_cnt_limit) @@ -736,17 +738,17 @@ BOOL AbsEncoder_is_in_fault(AbsEncoder* o) || (o->fault_state.bits.data_error && !o->fault_state_prec.bits.data_error) || (o->fault_state.bits.chip_error && !o->fault_state_prec.bits.chip_error)) { - AbsEncoder_send_error(o->ID, eoerror_value_MC_aea_abs_enc_invalid, o->fault_state.bitmask); + AbsEncoder_send_error(o->ID, eoerror_value_MC_abs_enc_invalid, o->fault_state.bitmask); } if (o->fault_state.bits.data_notready) { - AbsEncoder_send_error(o->ID, eoerror_value_MC_aea_abs_enc_timeout, 0); + AbsEncoder_send_error(o->ID, eoerror_value_MC_abs_enc_timeout, 0); } if (o->fault_state.bits.spikes) { - AbsEncoder_send_error(o->ID, eoerror_value_MC_aea_abs_enc_spikes, 0); + AbsEncoder_send_error(o->ID, eoerror_value_MC_abs_enc_spikes, 0); } o->fault_state_prec.bitmask = o->fault_state.bitmask; diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/CalibrationHelperData.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/CalibrationHelperData.h index 1a7773cc6..1152269bc 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/CalibrationHelperData.h +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/CalibrationHelperData.h @@ -41,7 +41,7 @@ typedef struct // CableCalib typedef struct // HardStopCalib { - int32_t pwm; // [2FOC PWM units] (-32000 : +32000) = (-100% : +100%) + int32_t pwm; // [2FOC PWM units] (-32000 : +32000) = (-100% : +100%) int32_t zero; // [icubdegrees] int32_t space_thr; // [icubdegrees] int32_t time_thr; // [milliseconds] diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Calibrators.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/Calibrators.c index e3c1f61b4..509467180 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Calibrators.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Calibrators.c @@ -626,7 +626,7 @@ BOOL JointSet_do_wait_calibration_10(JointSet* o) } else { - if (AbsEncoder_is_hard_stop_calibrating(encoder)) + if (AbsEncoder_is_hard_stop_calibrating(encoder) && (!encoder->state.bits.not_initialized)) { Motor_set_pwm_ref(o->motor+m, o->motor[m].calib_pwm); diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c index f60d09f41..533f06ad1 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c @@ -288,10 +288,6 @@ void Joint_update_status_reference(Joint* o) default: ; } - - // Debug for temporary adding the raw planned target position to the motor encoder value which - o->eo_joint_ptr->status.addinfo.multienc[1] = (int32_t)Trajectory_get_target_position(&(o->trajectory)); - //ends here } static void Joint_send_debug_message(const char *message, uint8_t jid, uint16_t par16, uint64_t par64) diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c index 150a3c22c..42b1e1187 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c @@ -1920,8 +1920,8 @@ void JointSet_calibrate(JointSet* o, uint8_t e, eOmc_calibrator_t *calibrator) { o->hard_stop_calib.zero = calibrator->params.type10.calibrationZero; o->hard_stop_calib.pwm = calibrator->params.type10.pwmlimit; - o->hard_stop_calib.space_thr = 12000; // we can make them configurable (probably not needed) - o->hard_stop_calib.time_thr = 1000; // we can make them configurable (probably not needed) + o->hard_stop_calib.space_thr = 1000; // we can make them configurable (probably not needed) + o->hard_stop_calib.time_thr = 200; // we can make them configurable (probably not needed) AbsEncoder_still_check_reset(o->absEncoder+e); AbsEncoder_start_hard_stop_calibrate(o->absEncoder+e, calibrator->params.type10.calibrationZero); } From d55a491d8170756081e5f487c0c4e8e57f2a195b Mon Sep 17 00:00:00 2001 From: Jacopo Date: Mon, 2 Dec 2024 18:06:51 +0100 Subject: [PATCH 2/2] Update fw executables --- .../board/amc/application/v2/cfg/theApplication_config.h | 4 ++-- .../ems004/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h | 8 ++++---- .../mc2plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h | 8 ++++---- .../mc4plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h | 8 ++++---- 4 files changed, 14 insertions(+), 14 deletions(-) diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v2/cfg/theApplication_config.h b/emBODY/eBcode/arch-arm/board/amc/application/v2/cfg/theApplication_config.h index 89d37464b..e543b2378 100644 --- a/emBODY/eBcode/arch-arm/board/amc/application/v2/cfg/theApplication_config.h +++ b/emBODY/eBcode/arch-arm/board/amc/application/v2/cfg/theApplication_config.h @@ -38,8 +38,8 @@ namespace embot { namespace app { namespace eth { .property = { Process::eApplication, - {2, 12}, - {2024, Month::Oct, Day::thirthyone, 13, 25} + {2, 13}, + {2024, Month::Dec, Day::two, 18, 00} }, .OStick = 1000*embot::core::time1microsec, .OSstacksizeinit = 10*1024, diff --git a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h index 802211531..d3ce32c59 100644 --- a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h +++ b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h @@ -81,7 +81,7 @@ extern "C" { #define EOMTHEEMSAPPLCFG_VERSION_MAJOR (VERSION_MAJOR_OFFSET+3) // minor <0-255> // minor <0-255> -#define EOMTHEEMSAPPLCFG_VERSION_MINOR 95 +#define EOMTHEEMSAPPLCFG_VERSION_MINOR 96 // version @@ -89,11 +89,11 @@ extern "C" { // year <2010-2030> #define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2024 // month <1-12> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 10 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 12 // day <1-31> -#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 28 +#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 02 // hour <0-23> -#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 16 +#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 18 // minute <0-59> #define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 00 // build date diff --git a/emBODY/eBcode/arch-arm/board/mc2plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h b/emBODY/eBcode/arch-arm/board/mc2plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h index f64da4dcc..189440186 100644 --- a/emBODY/eBcode/arch-arm/board/mc2plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h +++ b/emBODY/eBcode/arch-arm/board/mc2plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h @@ -75,7 +75,7 @@ extern "C" { #define EOMTHEEMSAPPLCFG_VERSION_MAJOR 3 // minor <0-255> // minor <0-255> -#define EOMTHEEMSAPPLCFG_VERSION_MINOR 75 +#define EOMTHEEMSAPPLCFG_VERSION_MINOR 76 // version @@ -83,11 +83,11 @@ extern "C" { // year <2010-2030> #define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2024 // month <1-12> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 10 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 12 // day <1-31> -#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 28 +#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 02 // hour <0-23> -#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 16 +#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 18 // minute <0-59> #define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 0 // build date diff --git a/emBODY/eBcode/arch-arm/board/mc4plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h b/emBODY/eBcode/arch-arm/board/mc4plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h index 6145defe7..3c00514a9 100644 --- a/emBODY/eBcode/arch-arm/board/mc4plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h +++ b/emBODY/eBcode/arch-arm/board/mc4plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h @@ -84,7 +84,7 @@ extern "C" { #define EOMTHEEMSAPPLCFG_VERSION_MAJOR (VERSION_MAJOR_OFFSET+3) // minor <0-255> -#define EOMTHEEMSAPPLCFG_VERSION_MINOR 96 +#define EOMTHEEMSAPPLCFG_VERSION_MINOR 97 // version @@ -92,11 +92,11 @@ extern "C" { // year <2010-2030> #define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2024 // month <1-12> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 10 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 12 // day <1-31> -#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 28 +#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 02 // hour <0-23> -#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 16 +#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 18 // minute <0-59> #define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 0