Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix diagnostic for AEA encoder and calibration type 10 #540

Merged
merged 2 commits into from
Dec 10, 2024
Merged

Fix diagnostic for AEA encoder and calibration type 10 #540

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
dddd255
AMO encoder raw value is always sent with full resolution
MSECode Nov 8, 2024
d55a491
Update fw executables
MSECode Dec 2, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions emBODY/eBcode/arch-arm/board/amc/application/v2/cfg/theApplication_config.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
8 changes: 4 additions & 4 deletions emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -81,19 +81,19 @@ extern "C" {
#define EOMTHEEMSAPPLCFG_VERSION_MAJOR (VERSION_MAJOR_OFFSET+3)
// <o> minor <0-255>
// <o> minor <0-255>
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 95
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 96

// </h>version

// <h> build date
// <o> year <2010-2030>
#define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2024
// <o> month <1-12>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 10
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 12
// <o> day <1-31>
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 28
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 02
// <o> hour <0-23>
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 16
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 18
// <o> minute <0-59>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 00
// </h>build date
Expand Down
6 changes: 3 additions & 3 deletions emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheMotionController.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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; i<p->numofjomos; 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;
Expand All @@ -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;

}
Expand Down
8 changes: 4 additions & 4 deletions emBODY/eBcode/arch-arm/board/mc2plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,19 +75,19 @@ extern "C" {
#define EOMTHEEMSAPPLCFG_VERSION_MAJOR 3
// <o> minor <0-255>
// <o> minor <0-255>
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 75
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 76

// </h>version

// <h> build date
// <o> year <2010-2030>
#define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2024
// <o> month <1-12>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 10
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 12
// <o> day <1-31>
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 28
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 02
// <o> hour <0-23>
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 16
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 18
// <o> minute <0-59>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 0
// </h>build date
Expand Down
8 changes: 4 additions & 4 deletions emBODY/eBcode/arch-arm/board/mc4plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,19 +84,19 @@ extern "C" {
#define EOMTHEEMSAPPLCFG_VERSION_MAJOR (VERSION_MAJOR_OFFSET+3)
// <o> minor <0-255>

#define EOMTHEEMSAPPLCFG_VERSION_MINOR 96
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 97

// </h>version

// <h> build date
// <o> year <2010-2030>
#define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2024
// <o> month <1-12>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 10
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 12
// <o> day <1-31>
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 28
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 02
// <o> hour <0-23>
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 16
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 18
// <o> minute <0-59>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 0

Expand Down
13 changes: 8 additions & 5 deletions emBODY/eBcode/arch-arm/embobj/plus/board/EOappEncodersReader.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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 .....
Expand All @@ -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;

Expand Down Expand Up @@ -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);

}
Expand Down
30 changes: 16 additions & 14 deletions emBODY/eBcode/arch-arm/embobj/plus/mc/AbsEncoder.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;

Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}

Expand All @@ -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;
}
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion emBODY/eBcode/arch-arm/embobj/plus/mc/CalibrationHelperData.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down
2 changes: 1 addition & 1 deletion emBODY/eBcode/arch-arm/embobj/plus/mc/Calibrators.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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))
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hi @MSECode, can it happen that a calibration command is sent before the encoder is configured?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yes, it happened to me sometimes. I'm not sure why, but it was most likely to happen after restarting the yarprobotinterface without switching off and on the boards.
This caused the problem of not allowing the joint to reach the hw limit.
If you wanna dig on that I've collected more details and tests done here: #529 (comment), #529 (comment)

{
Motor_set_pwm_ref(o->motor+m, o->motor[m].calib_pwm);

Expand Down
4 changes: 0 additions & 4 deletions emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
4 changes: 2 additions & 2 deletions emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Expand Down