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 a48b7087d9..10bc6af4ee 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 49 +#define EOMTHEEMSAPPLCFG_VERSION_MINOR 50 // version @@ -89,13 +89,13 @@ extern "C" { // year <2010-2030> #define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2022 // month <1-12> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 1 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 3 // day <1-31> -#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 28 +#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 2 // hour <0-23> -#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 16 +#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 21 // minute <0-59> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 50 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 13 // build date // Info @@ -495,7 +495,7 @@ extern "C" { // max size of output datagrams <64-1500> // default: 1024 -#define EOMTHEEMSAPPLCFG_SOCKET_OUTDGRAMSIZEOF 1408 +#define EOMTHEEMSAPPLCFG_SOCKET_OUTDGRAMSIZEOF 1416 // datagrams in socket @@ -554,7 +554,7 @@ extern "C" { // capacity of the ropframe of reply rops <16-1440:8> // default: 128 -#define EOMTHEEMSAPPLCFG_TRANSCEIVER_ROPFRAMEREPLIESCAPACITY 256 +#define EOMTHEEMSAPPLCFG_TRANSCEIVER_ROPFRAMEREPLIESCAPACITY 264 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 92933c0e83..b91cb25c94 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,20 +75,20 @@ extern "C" { #define EOMTHEEMSAPPLCFG_VERSION_MAJOR 3 // minor <0-255> // minor <0-255> -#define EOMTHEEMSAPPLCFG_VERSION_MINOR 33 +#define EOMTHEEMSAPPLCFG_VERSION_MINOR 34 // version // build date // year <2010-2030> #define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2022 // month <1-12> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 1 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 3 // day <1-31> -#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 28 +#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 2 // hour <0-23> -#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 16 +#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 21 // minute <0-59> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 50 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 00 // build date // Info @@ -489,7 +489,7 @@ extern "C" { // max size of output datagrams <64-1500> // default: 1024 -#define EOMTHEEMSAPPLCFG_SOCKET_OUTDGRAMSIZEOF 1408 +#define EOMTHEEMSAPPLCFG_SOCKET_OUTDGRAMSIZEOF 1416 // datagrams in socket @@ -548,7 +548,7 @@ extern "C" { // capacity of the ropframe of reply rops <16-1440:8> // default: 128 -#define EOMTHEEMSAPPLCFG_TRANSCEIVER_ROPFRAMEREPLIESCAPACITY 256 +#define EOMTHEEMSAPPLCFG_TRANSCEIVER_ROPFRAMEREPLIESCAPACITY 264 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 b300782c15..5189b94af6 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 @@ -87,7 +87,7 @@ extern "C" { // minor <0-255> -#define EOMTHEEMSAPPLCFG_VERSION_MINOR 47 +#define EOMTHEEMSAPPLCFG_VERSION_MINOR 48 // version @@ -96,13 +96,13 @@ extern "C" { // year <2010-2030> #define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2022 // month <1-12> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 1 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 3 // day <1-31> -#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 28 +#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 2 // hour <0-23> -#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 16 +#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 21 // minute <0-59> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 50 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 11 // build date @@ -504,7 +504,7 @@ extern "C" { // max size of output datagrams <64-1500> // default: 1024 -#define EOMTHEEMSAPPLCFG_SOCKET_OUTDGRAMSIZEOF 1408 +#define EOMTHEEMSAPPLCFG_SOCKET_OUTDGRAMSIZEOF 1416 // datagrams in socket @@ -555,15 +555,15 @@ extern "C" { // capacity of the ropframe of regular rops <16-1440:8> // default: 768 -#define EOMTHEEMSAPPLCFG_TRANSCEIVER_ROPFRAMEREGULARSCAPACITY 1024 +#define EOMTHEEMSAPPLCFG_TRANSCEIVER_ROPFRAMEREGULARSCAPACITY 1024 // capacity of the ropframe of occasional rops <16-1440:8> // default: 128 -#define EOMTHEEMSAPPLCFG_TRANSCEIVER_ROPFRAMEOCCASIONALSCAPACITY 128 +#define EOMTHEEMSAPPLCFG_TRANSCEIVER_ROPFRAMEOCCASIONALSCAPACITY 128 // capacity of the ropframe of reply rops <16-1440:8> // default: 128 -#define EOMTHEEMSAPPLCFG_TRANSCEIVER_ROPFRAMEREPLIESCAPACITY 256 +#define EOMTHEEMSAPPLCFG_TRANSCEIVER_ROPFRAMEREPLIESCAPACITY 264 diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c index f4a01f9bef..2500ad88e7 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c @@ -1501,7 +1501,7 @@ void MController_config_joint(int j, eOmc_joint_config_t* config) // MController_motor_config_torque_PID(j, &(config->pidtorque)); Motor_config_filter(o->motor+j, config->tcfiltertype); - Motor_config_friction(o->motor+j, config->motor_params.bemf_value, config->motor_params.ktau_value); + Motor_config_friction(o->motor+j, config->motor_params.bemf_value, config->motor_params.ktau_value, config->motor_params.friction); Joint_config(o->joint+j, j, config); @@ -1517,9 +1517,9 @@ void MController_config_motor(int m, eOmc_motor_config_t* config) // Motor_config(smc->motor+m, m, config); } -void MController_config_motor_friction(int m, eOmc_motor_params_t* friction) // +void MController_config_motor_friction(int m, eOmc_motor_params_t* params) // { - Motor_config_friction(smc->motor+m, friction->bemf_value, friction->ktau_value); + Motor_config_friction(smc->motor+m, params->bemf_value, params->ktau_value, params->friction); } void MController_config_joint_impedance(int j, eOmc_impedance_t* impedance) // diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.h index e8535059e2..02ff53f943 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.h +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.h @@ -114,7 +114,7 @@ extern void MController_get_motor_state(int m, eOmc_motor_status_t* motor_status extern void MController_update_joint_targets(int j); extern void MController_update_motor_pos_fbk(int m, int32_t position_raw); extern void MController_update_motor_current_fbk(int m, int16_t current); -extern void MController_config_motor_friction(int m, eOmc_motor_params_t* friction); // +extern void MController_config_motor_friction(int m, eOmc_motor_params_t* params); // extern void MController_config_joint_impedance(int j, eOmc_impedance_t* impedance); // extern void MController_config_minjerk_pid(int j, eOmc_PID_t *pid_conf); // diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c index c272f1d1f0..b806577d2a 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c @@ -437,9 +437,9 @@ void Motor_config_filter(Motor* o, uint8_t filter) // PID_config_filter(&o->trqPID, filter); } -void Motor_config_friction(Motor* o, float Bemf, float Ktau) // +void Motor_config_friction(Motor* o, float Bemf, float Ktau, eOmc_FrictionParams_t friction) // { - PID_config_friction(&o->trqPID, Bemf, Ktau); + PID_config_friction(&o->trqPID, Bemf, Ktau, friction); } void Motor_calibrate_withOffset(Motor* o, int32_t offset) // diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h index a9762f221f..aa3633cc38 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h @@ -250,7 +250,7 @@ extern void Motor_config_torque_PID(Motor* o, eOmc_PID_t* pid); extern void Motor_config_speed_PID(Motor* o, eOmc_PID_t* pid); extern void Motor_config_filter(Motor* o, uint8_t filter); // -extern void Motor_config_friction(Motor* o, float Bemf, float Ktau); // +extern void Motor_config_friction(Motor* o, float Bemf, float Ktau, eOmc_FrictionParams_t friction); // extern void Motor_calibrate_withOffset(Motor* o, int32_t offset); // extern BOOL Motor_calibrate_moving2Hardstop(Motor* o, int32_t pwm, int32_t zero); // extern void Motor_uncalibrate(Motor* o); diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Pid.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/Pid.c index 0823ff0adb..39ac1aac40 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Pid.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Pid.c @@ -86,10 +86,14 @@ void PID_config(PID* o, eOmc_PID_t* config) } -void PID_config_friction(PID *o, float Kbemf, float Ktau) +void PID_config_friction(PID *o, float Kbemf, float Ktau, eOmc_FrictionParams_t friction) { o->Kbemf = Kbemf; o->Ktau = Ktau; + o->viscous_pos_val = friction.viscous_pos_val; + o->viscous_neg_val = friction.viscous_neg_val; + o->coulomb_pos_val = friction.coulomb_pos_val; + o->coulomb_neg_val = friction.coulomb_neg_val; } void PID_config_filter(PID *o, uint8_t filter) @@ -175,7 +179,14 @@ float PID_do_out(PID* o, float En) return o->out_lpf; } +#include "EOtheErrorManager.h" + float PID_do_friction_comp(PID *o, float vel_fbk, float trq_ref) { +#ifdef USE_VISCOUS_COULOMB + //return o->Ktau*(o->coulomb_pos_val + o->viscous_pos_val*vel_fbk + o->Kff*trq_ref); + return o->Ktau*(o->Kbemf*vel_fbk+o->Kff*trq_ref); +#else return o->Ktau*(o->Kbemf*vel_fbk+o->Kff*trq_ref); +#endif // USE_VISCOUS_COULOMB } diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Pid.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/Pid.h index 50ce1e7524..dde5491c68 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Pid.h +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Pid.h @@ -48,7 +48,12 @@ typedef struct //PID float stiction_up; float stiction_down; - + + float viscous_pos_val; + float viscous_neg_val; + float coulomb_pos_val; + float coulomb_neg_val; + #ifdef FINGER_MK3 float out_max_open; float out_max_close; @@ -65,7 +70,7 @@ typedef struct //PID PID* PID_new(uint8_t n); extern void PID_init(PID* o); extern void PID_config(PID* o, eOmc_PID_t* config); -extern void PID_config_friction(PID *o, float Kbemf, float Ktau); +extern void PID_config_friction(PID *o, float Kbemf, float Ktau, eOmc_FrictionParams_t friction); extern void PID_config_filter(PID *o, uint8_t filter); extern void PID_reset(PID* o);