extern eOresult_t eo_skin_Activate(EOtheSKIN *p, const eOmn_serv_configuration_t * servcfg)
{
if((NULL == p) || (NULL == servcfg))
{
return(eores_NOK_nullpointer);
}
if(eomn_serv_SK_skin != servcfg->type)
{
return(eores_NOK_generic);
}
if(eobool_true == p->service.active)
{
eo_skin_Deactivate(p);
}
eo_entities_SetNumOfSkins(eo_entities_GetHandle(), servcfg->data.sk.skin.numofpatches);
if(0 == eo_entities_NumOfSkins(eo_entities_GetHandle()))
{
p->service.active = eobool_false;
return(eores_NOK_generic);
}
else
{
memcpy(&p->service.servconfig, servcfg, sizeof(eOmn_serv_configuration_t));
p->numofskinpatches = servcfg->data.sk.skin.numofpatches;
uint8_t i = 0;
for(i=0; i<p->numofskinpatches; i++)
{
p->skinpatches[i] = eo_entities_GetSkin(eo_entities_GetHandle(), i);
}
// now i must add all the mtb boards. i iterate per patch and then per canbus
eObrd_canproperties_t prop =
{
.type = eobrd_cantype_mtb,
.location = { .port = 0, .addr = 0, .insideindex = eobrd_caninsideindex_none },
.requiredprotocol = { .major = servcfg->data.sk.skin.version.protocol.major, .minor = servcfg->data.sk.skin.version.protocol.minor }
};
eOcanmap_entitydescriptor_t des =
{
.location = { .port = 0, .addr = 0, .insideindex = eobrd_caninsideindex_none },
.index = entindexNONE
};
extern EOCurrentsWatchdog* eo_currents_watchdog_Initialise(void)
{
uint8_t m;
//reserve memory for the number of thresholds needed
s_eo_currents_watchdog.numberofmotors = eo_entities_NumOfMotors(eo_entities_GetHandle());
if (s_eo_currents_watchdog.numberofmotors == 0)
return NULL;
s_eo_currents_watchdog.themotors = (eOmc_motor_t**) eo_mempool_GetMemory(eo_mempool_GetHandle(), eo_mempool_align_auto, sizeof(eOmc_motor_t*), s_eo_currents_watchdog.numberofmotors);
// retrieve pointers to motors
for(m=0; m<s_eo_currents_watchdog.numberofmotors; m++)
{
s_eo_currents_watchdog.themotors[m] = eo_entities_GetMotor(eo_entities_GetHandle(), m);
}
s_eo_currents_watchdog.nominalCurrent2 = (float*) eo_mempool_GetMemory(eo_mempool_GetHandle(), eo_mempool_align_auto, sizeof(float), s_eo_currents_watchdog.numberofmotors);
memset(s_eo_currents_watchdog.nominalCurrent2, 0, s_eo_currents_watchdog.numberofmotors*sizeof(float));
s_eo_currents_watchdog.I2T_threshold = (float*) eo_mempool_GetMemory(eo_mempool_GetHandle(), eo_mempool_align_auto, sizeof(float), s_eo_currents_watchdog.numberofmotors);
memset(s_eo_currents_watchdog.I2T_threshold, 0, s_eo_currents_watchdog.numberofmotors*sizeof(float));
s_eo_currents_watchdog.avgCurrent = (eoCurrentWD_averageData_t*) eo_mempool_GetMemory(eo_mempool_GetHandle(), eo_mempool_align_auto, sizeof(eoCurrentWD_averageData_t), s_eo_currents_watchdog.numberofmotors);
memset(s_eo_currents_watchdog.avgCurrent, 0, s_eo_currents_watchdog.numberofmotors*sizeof(eoCurrentWD_averageData_t));
s_eo_currents_watchdog.accomulatorEp = (float*) eo_mempool_GetMemory(eo_mempool_GetHandle(), eo_mempool_align_auto, sizeof(float), s_eo_currents_watchdog.numberofmotors);
memset(s_eo_currents_watchdog.accomulatorEp, 0, s_eo_currents_watchdog.numberofmotors*sizeof(float));
s_eo_currents_watchdog.motorinI2Tfault = (eObool_t*) eo_mempool_GetMemory(eo_mempool_GetHandle(), eo_mempool_align_auto, sizeof(eObool_t), s_eo_currents_watchdog.numberofmotors);
memset(s_eo_currents_watchdog.motorinI2Tfault, 0, s_eo_currents_watchdog.numberofmotors*sizeof(eObool_t)); //all motors are not in I2t fault
s_eo_currents_watchdog.initted = eobool_true;
suppliedVoltage_counter = 0;
nv_controller_ptr = (eOmc_controller_t*) eoprot_entity_ramof_get(eoprot_board_localboard, eoprot_endpoint_motioncontrol, eoprot_entity_mc_controller, 0);
if(nv_controller_ptr == NULL)
return NULL;
return(&s_eo_currents_watchdog);
}
extern eOresult_t eo_skin_Deactivate(EOtheSKIN *p)
{
if(NULL == p)
{
return(eores_NOK_nullpointer);
}
if(eobool_false == p->service.active)
{
// i force to eomn_serv_state_idle because it may be that state was eomn_serv_state_verified or eomn_serv_state_failureofverify
p->service.state = eomn_serv_state_idle;
eo_service_hid_SynchServiceState(eo_services_GetHandle(), eomn_serv_category_skin, p->service.state);
return(eores_OK);
}
if(eobool_true == p->service.started)
{
eo_skin_Stop(p);
}
eo_skin_SetRegulars(p, NULL, NULL);
eo_canmap_DeconfigEntity(eo_canmap_GetHandle(), eoprot_endpoint_skin, eoprot_entity_sk_skin, p->sharedcan.entitydescriptor);
eo_canmap_UnloadBoards(eo_canmap_GetHandle(), p->sharedcan.boardproperties);
eo_entities_SetNumOfSkins(eo_entities_GetHandle(), 0);
p->numofmtbs = 0;
uint8_t i=0;
for(i=0; i<eomn_serv_skin_maxpatches; i++)
{
p->skinpatches[i] = NULL;
p->patchisrunning[i] = eobool_false;
eo_vector_Clear(p->rxdata[i]);
}
memset(&p->service.servconfig, 0, sizeof(eOmn_serv_configuration_t));
p->service.servconfig.type = eomn_serv_NONE;
eo_vector_Clear(p->sharedcan.boardproperties);
eo_vector_Clear(p->sharedcan.entitydescriptor);
// make sure the timer is not running
eo_timer_Stop(p->diagnostics.reportTimer);
p->service.active = eobool_false;
p->service.state = eomn_serv_state_idle;
eo_service_hid_SynchServiceState(eo_services_GetHandle(), eomn_serv_category_skin, p->service.state);
return(eores_OK);
}
extern EOCurrentsWatchdog* eo_currents_watchdog_Initialise(void)
{
uint8_t m;
//reserve memory for the number of thresholds needed
s_eo_currents_watchdog.numberofmotors = eo_entities_NumOfJoints(eo_entities_GetHandle());
if (s_eo_currents_watchdog.numberofmotors == 0)
return NULL;
s_eo_currents_watchdog.themotors = eo_mempool_GetMemory(eo_mempool_GetHandle(), eo_mempool_align_auto, sizeof(eOmc_motor_t*), s_eo_currents_watchdog.numberofmotors);
// retrieve pointers to motors
for(m=0; m<s_eo_currents_watchdog.numberofmotors; m++)
{
s_eo_currents_watchdog.themotors[m] = eoprot_entity_ramof_get(eoprot_board_localboard, eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m);
}
s_eo_currents_watchdog.nominalCurrent2 = eo_mempool_GetMemory(eo_mempool_GetHandle(), eo_mempool_align_auto, sizeof(float), s_eo_currents_watchdog.numberofmotors);
memset(s_eo_currents_watchdog.nominalCurrent2, 0, s_eo_currents_watchdog.numberofmotors*sizeof(float));
s_eo_currents_watchdog.I2T_threshold = eo_mempool_GetMemory(eo_mempool_GetHandle(), eo_mempool_align_auto, sizeof(float), s_eo_currents_watchdog.numberofmotors);
memset(s_eo_currents_watchdog.I2T_threshold, 0, s_eo_currents_watchdog.numberofmotors*sizeof(float));
//s_eo_currents_watchdog.filter_reg = eo_mempool_GetMemory(eo_mempool_GetHandle(), eo_mempool_align_auto, sizeof(float), s_eo_currents_watchdog.numberofmotors);
//memset(s_eo_currents_watchdog.filter_reg, 0, s_eo_currents_watchdog.numberofmotors*sizeof(float));
s_eo_currents_watchdog.avgCurrent = eo_mempool_GetMemory(eo_mempool_GetHandle(), eo_mempool_align_auto, sizeof(eoCurrentWD_averageData_t), s_eo_currents_watchdog.numberofmotors);
memset(s_eo_currents_watchdog.avgCurrent, 0, s_eo_currents_watchdog.numberofmotors*sizeof(eoCurrentWD_averageData_t));
s_eo_currents_watchdog.accomulatorEp = eo_mempool_GetMemory(eo_mempool_GetHandle(), eo_mempool_align_auto, sizeof(float), s_eo_currents_watchdog.numberofmotors);
memset(s_eo_currents_watchdog.accomulatorEp, 0, s_eo_currents_watchdog.numberofmotors*sizeof(float));
s_eo_currents_watchdog.initted = eobool_true;
return(&s_eo_currents_watchdog);
}
void JointSet_calibrate(JointSet* o, uint8_t e, eOmc_calibrator_t *calibrator)
{
// for (int js=0; js<*(o->pN); ++js)
// {
// o->joint[o->joints_of_set[js]].control_mode = eomc_controlmode_calib;
// }
eOmc_controlmode_t joint_controlMode_old = o->joint[e].control_mode;
eOmc_controlmode_t jointSet_controlMode_old = o->control_mode;
o->joint[e].control_mode = eomc_controlmode_calib;
o->control_mode = eomc_controlmode_calib;
o->is_calibrated = FALSE;
switch (calibrator->type)
{
case eomc_calibration_type3_abs_sens_digital:
{
AbsEncoder_calibrate_absolute(o->absEncoder+e, calibrator->params.type3.offset, calibrator->params.type3.calibrationZero);
Motor_calibrate_withOffset(o->motor+e, 0);
o->calibration_in_progress = (eOmc_calibration_type_t)calibrator->type;
break;
}
case eomc_calibration_type5_hard_stops:
{
// //Debug code
// char message[150];
// snprintf(message, sizeof(message), "calib cmd rec: pwm%d cz%d", calibrator->params.type5.pwmlimit, calibrator->params.type5.calibrationZero);
// JointSet_send_debug_message(message, e);
o->calibration_timeout = 0;
BOOL ret = Motor_calibrate_moving2Hardstop(o->motor+e, calibrator->params.type5.pwmlimit, (calibrator->params.type5.final_pos - calibrator->params.type5.calibrationZero));
if(!ret)
{
o->joint[e].control_mode = joint_controlMode_old;
o->control_mode = jointSet_controlMode_old;
return;
}
Joint_set_hardware_limit(o->joint+e);
AbsEncoder_calibrate_fake(o->absEncoder+e);
o->calibration_in_progress = (eOmc_calibration_type_t)calibrator->type;
break;
}
case eomc_calibration_type6_mais:
{
// 1) check params are ok
CTRL_UNITS target_pos;
if (calibrator->params.type6.current == 1 )
{
target_pos = calibrator->params.type6.vmax;
}
else if (calibrator->params.type6.current == -1)
{
target_pos = calibrator->params.type6.vmin;
}
else
{
////debug code
char info[50];
snprintf(info, 50, "error type6.current=%d",calibrator->params.type6.current);
JointSet_send_debug_message(info, e);
////debug code ended
return;
}
//if I'm here I can perform calib type 6.
// 2) set state
o->calibration_timeout = 0;
o->calibration_in_progress = eomc_calibration_typeMixed;
o->joint[e].running_calibration.type = (eOmc_calibration_type_t)calibrator->type;
o->joint[e].running_calibration.data.type6.is_active = TRUE;
o->joint[e].running_calibration.data.type6.state = calibtype6_st_inited;
// o->joint[e].calib_type6_data.is_active = TRUE;
// o->joint[e].calib_type6_data.state = calibtype6_st_inited;
// o->joint[e].calibration_in_progress = (eOmc_calibration_type_t)calibrator->type;
// 3) calculate new joint encoder factor and param_zero
eOmc_joint_config_t *jconfig = &o->joint[e].eo_joint_ptr->config;
float computedJntEncoderResolution = (float)(calibrator->params.type6.vmax - calibrator->params.type6.vmin) / (float) (jconfig->userlimits.max - jconfig->userlimits.min);
eOresult_t res = eo_appEncReader_UpdatedMaisConversionFactors(eo_appEncReader_GetHandle(), e, computedJntEncoderResolution);
if(eores_OK != res)
{
////debug code
char info[70];
snprintf(info, 70, "calib6: error updating Mais conversion factor j%d", e);
JointSet_send_debug_message(info, e);
////debug code ended
return;
}
AbsEncoder_config_resolution(o->absEncoder+e, computedJntEncoderResolution);
//Now I need to re-init absEncoder because I chenged maisConversionFactor, therefore the values returned by EOappEncoreReder are changed.
o->absEncoder[e].state.bits.not_initialized = TRUE;
float computedJntEncoderZero = - (float)(jconfig->userlimits.min) + ((float)(calibrator->params.type6.vmin) / computedJntEncoderResolution);
o->joint[e].running_calibration.data.type6.computedZero = computedJntEncoderZero;
o->joint[e].running_calibration.data.type6.targetpos = target_pos / computedJntEncoderResolution - computedJntEncoderZero; //convert target pos from mais unit to icub deegre
o->joint[e].running_calibration.data.type6.velocity = calibrator->params.type6.velocity;
o->joint[e].running_calibration.data.type6.state = calibtype6_st_jntEncResComputed;
}
break;
case eomc_calibration_type7_hall_sensor:
{
//1) check params: nothinh to do
// 2) set state
o->calibration_timeout = 0;
o->calibration_in_progress = eomc_calibration_typeMixed;
o->joint[e].running_calibration.type = (eOmc_calibration_type_t)calibrator->type;
o->joint[e].running_calibration.data.type7.state = calibtype7_st_inited;
o->joint[e].running_calibration.data.type7.is_active = TRUE;
// o->joint[e].calib_type7_data.is_active = TRUE;
// o->joint[e].calib_type7_data.state = calibtype7_st_inited;
// o->joint[e].calibration_in_progress = (eOmc_calibration_type_t)calibrator->type;
// 2) calculate new joint encoder factor and param_zero
eOmc_joint_config_t *jconfig = &o->joint[e].eo_joint_ptr->config;
float computedJntEncoderResolution = (float)(calibrator->params.type7.vmax - calibrator->params.type7.vmin) / (float) (jconfig->userlimits.max - jconfig->userlimits.min);
int32_t offset = (((float)calibrator->params.type7.vmin)/computedJntEncoderResolution) - jconfig->userlimits.min;
eOresult_t res = eo_appEncReader_UpdatedHallAdcOffset(eo_appEncReader_GetHandle(), e, offset);
if(eores_OK != res)
{
////debug code
char info[70];
snprintf(info, 70, "calib7: error updating HallADC offset j%d", e);
JointSet_send_debug_message(info, e);
////debug code ended
return;
}
res = eo_appEncReader_UpdatedHallAdcConversionFactors(eo_appEncReader_GetHandle(), e, computedJntEncoderResolution);
if(eores_OK != res)
{
////debug code
char info[70];
snprintf(info, 70, "calib7: error updating HallADC conversion factor j%d", e);
JointSet_send_debug_message(info, e);
////debug code ended
return;
}
AbsEncoder_config_resolution(o->absEncoder+e, computedJntEncoderResolution);
//Now I need to re init absEncoder because I chenged hallADCConversionFactor, therefore the values returned by EOappEncoreReder are changed.
o->absEncoder[e].state.bits.not_initialized = TRUE;
float computedJntEncoderZero = (((float)calibrator->params.type7.vmin) / computedJntEncoderResolution) - ((float)(jconfig->userlimits.min)) - offset;
o->joint[e].running_calibration.data.type7.computedZero = computedJntEncoderZero;
o->joint[e].running_calibration.data.type7.state = calibtype7_st_jntEncResComputed;
}
break;
case eomc_calibration_type8_tripod_internal_hard_stop:
case eomc_calibration_type9_tripod_external_hard_stop:
{
if (o->calibration_in_progress == calibrator->type) return;
o->calibration_wait = 0;
o->calibration_timeout = 0;
o->calibration_in_progress = (eOmc_calibration_type_t)calibrator->type;
o->joint[o->joints_of_set[0]].control_mode = eomc_controlmode_calib;
o->joint[o->joints_of_set[1]].control_mode = eomc_controlmode_calib;
o->joint[o->joints_of_set[2]].control_mode = eomc_controlmode_calib;
AbsEncoder_calibrate_fake(o->absEncoder+o->encoders_of_set[0]);
AbsEncoder_calibrate_fake(o->absEncoder+o->encoders_of_set[1]);
AbsEncoder_calibrate_fake(o->absEncoder+o->encoders_of_set[2]);
o->tripod_calib.pwm = calibrator->params.type9.pwmlimit;
o->tripod_calib.max_delta = calibrator->params.type9.max_delta;
o->tripod_calib.zero = calibrator->params.type9.calibrationZero;
Motor_uncalibrate(o->motor+o->motors_of_set[0]);
Motor_uncalibrate(o->motor+o->motors_of_set[1]);
Motor_uncalibrate(o->motor+o->motors_of_set[2]);
Motor_set_run(o->motor+o->motors_of_set[0]);
Motor_set_run(o->motor+o->motors_of_set[1]);
Motor_set_run(o->motor+o->motors_of_set[2]);
o->tripod_calib.start_pos[0] = o->motor[o->motors_of_set[0]].pos_fbk;
o->tripod_calib.start_pos[1] = o->motor[o->motors_of_set[1]].pos_fbk;
o->tripod_calib.start_pos[2] = o->motor[o->motors_of_set[2]].pos_fbk;
break;
}
case eomc_calibration_type10_abs_hard_stop:
{
o->calibration_in_progress = (eOmc_calibration_type_t)calibrator->type;
o->calibration_timeout = 0;
Joint_set_hardware_limit(o->joint+e);
Motor_calibrate_withOffset(o->motor+e, 0);
Motor_set_run(o->motor+e);
o->motor[e].calib_pwm = calibrator->params.type10.pwmlimit;
AbsEncoder_still_check_reset(o->absEncoder+e);
AbsEncoder_start_hard_stop_calibrate(o->absEncoder+e, calibrator->params.type10.calibrationZero);
break;
}
case eomc_calibration_type11_cer_hands:
{
AbsEncoder* enc = o->absEncoder + 3*e;
AbsEncoder_calibrate_absolute(enc , calibrator->params.type11.offset0, enc[0].mul*32767);
AbsEncoder_calibrate_absolute(enc+1, calibrator->params.type11.offset1, enc[1].mul*32767);
AbsEncoder_calibrate_absolute(enc+2, calibrator->params.type11.offset2, enc[2].mul*32767);
JointSet_do_odometry(o);
Motor_calibrate_withOffset(o->motor+e, 0);
Motor_set_run(o->motor+e);
Motor_uncalibrate(o->motor+e);
o->joint[e].cable_constr.max_tension = SPRING_MAX_TENSION;
o->joint[e].cable_calib.pwm = calibrator->params.type11.pwm;
o->joint[e].cable_calib.cable_range = calibrator->params.type11.cable_range;
o->joint[e].cable_calib.delta = 900;//calibrator->params.type11.delta;
o->joint[e].cable_calib.target = o->joint[e].pos_fbk + o->joint[e].cable_calib.delta;
if (o->joint[e].cable_calib.target > o->joint[e].pos_max) o->joint[e].cable_calib.target = o->joint[e].pos_max;
if (o->joint[e].cable_calib.target < o->joint[e].pos_min) o->joint[e].cable_calib.target = o->joint[e].pos_min;
o->calibration_in_progress = (eOmc_calibration_type_t)calibrator->type;
o->calibration_wait = TRUE;
o->calibration_timeout = 0;
break;
}
case eomc_calibration_type12_absolute_sensor:
{
int32_t offset;
int32_t zero;
eOmc_joint_config_t *jointcfg = eo_entities_GetJointConfig(eo_entities_GetHandle(), e);
//1) Take absolute value of calibation parametr
int32_t abs_raw = (calibrator->params.type12.rawValueAtZeroPos > 0) ? calibrator->params.type12.rawValueAtZeroPos : -calibrator->params.type12.rawValueAtZeroPos;
// 1.1) update abs_raw with gearbox_E2J
abs_raw = abs_raw * jointcfg->gearbox_E2J;
// 2) calculate offset
if(abs_raw >= TICKS_PER_HALF_REVOLUTION)
offset = abs_raw - TICKS_PER_HALF_REVOLUTION;
else
offset = abs_raw + TICKS_PER_HALF_REVOLUTION;
// 3) find out sign of zero
if(jointcfg->jntEncoderResolution > 0)
zero = TICKS_PER_HALF_REVOLUTION / jointcfg->gearbox_E2J;
else
zero = -TICKS_PER_HALF_REVOLUTION / jointcfg->gearbox_E2J;
zero+=calibrator->params.type12.calibrationDelta; //this parameter should contain only the delta
// 4) call calibration function
////debug code
char info[80];
snprintf(info, sizeof(info), "CALIB 12 j %d: offset=%d zero=%d ", e, offset, zero);
JointSet_send_debug_message(info, e);
////debug code ended
AbsEncoder_calibrate_absolute(o->absEncoder+e, offset, zero);
Motor_calibrate_withOffset(o->motor+e, 0);
o->calibration_in_progress = (eOmc_calibration_type_t)calibrator->type;
break;
}
default:
break;
}
}
