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/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/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 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); }