Added cable management to hand Mk3, activated by FINGER_MK3 macro

robotology · Feb 23, 2021 · 4c52093 · 4c52093
1 parent 50c655a
commit 4c52093
Show file tree

Hide file tree

Showing 5 changed files with 58 additions and 3 deletions.
diff --git a/emBODY/eBcode/arch-arm/board/mc4plus/appl/v2/proj/mc4plus.handv3.uvprojx b/emBODY/eBcode/arch-arm/board/mc4plus/appl/v2/proj/mc4plus.handv3.uvprojx
@@ -10,7 +10,7 @@
       <TargetName>mc4plus</TargetName>
       <ToolsetNumber>0x4</ToolsetNumber>
       <ToolsetName>ARM-ADS</ToolsetName>
-      <pCCUsed>6150000::V6.15::ARMCLANG</pCCUsed>
+      <pCCUsed>6140001::V6.14.1::ARMCLANG</pCCUsed>
       <uAC6>1</uAC6>
       <TargetOption>
         <TargetCommonOption>
@@ -337,7 +337,7 @@
             <v6Rtti>0</v6Rtti>
             <VariousControls>
               <MiscControls>-DEOTHESERVICES_customize_handV3 -Wno-pragma-pack -Wno-deprecated-register -DUSE_MC4PLUS -DUSE_OLD_BUGGY_MODE_TO_SEND_UP_FULLSCALE_WITH_INVERTED_BYTES</MiscControls>
-              <Define>DIAGNOSTIC2_enabled DIAGNOSTIC2_receive_from_daemon DIAGNOSTIC2_send_to_yarprobotinterface _noDIAGNOSTIC2_send_to_daemon</Define>
+              <Define>DIAGNOSTIC2_enabled DIAGNOSTIC2_receive_from_daemon DIAGNOSTIC2_send_to_yarprobotinterface _noDIAGNOSTIC2_send_to_daemon FINGER_MK3</Define>
               <Undefine></Undefine>
               <IncludePath>..\..\..\..\..\libs\highlevel\abslayer\ipal\api;..\..\..\..\..\libs\highlevel\abslayer\osal\api;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\core\core;..\..\..\..\..\libs\highlevel\services\embenv\api;..\cfg\boardcfg;..\cfg\eoemsappl;..\cfg\abslayer;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\plus\comm-v2\icub;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\plus\comm-v2\protocol\api;..\..\..\..\..\libs\midware\eventviewer\api;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\plus\comm-v2\protocol\src;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\plus\comm-v2\transport;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\plus\utils;.;..\src\eoappservices;..\cfg\eoappservices\icub-can-proto;..\cfg\eoappservices\icub-can-net;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\robotconfig\v1\backdoor;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\plus\comm-v2\opcprot;..\..\..\..\..\..\..\..\..\icub-firmware-shared\can\canProtocolLib;..\..\..\..\..\libs\highlevel\abslayer\hal2\api;..\..\..\..\ems004\env\cfg;..\..\..\..\..\libs\highlevel\services\embodyrobot;..\..\..\..\..\libs\midware\hl-plus\api;..\..\..\..\..\embobj\core\exec\multitask;..\..\..\..\..\embobj\plus\ctrloop;..\..\..\..\..\embobj\plus\embenv;..\..\..\..\..\embobj\plus\ipnet;..\..\..\..\..\embobj\plus\board;..\..\..\..\..\embobj\plus\can;..\..\..\..\..\embobj\plus\can\config-can-protocol;..\..\..\..\..\embobj\plus\mc;..\..\..\..\ems004\appl\v2\cfg\eoprot-callbacks;..\..\..\..\ems004\appl\v2\src\eoappservices;..\..\..\..\..\embot\cif;..\..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core;..\..\..\..\..\..\..\..\..\icub-firmware-shared\embot\prot\eth</IncludePath>
             </VariousControls>

diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c
@@ -119,6 +119,11 @@ void Joint_init(Joint* o)
 
     o->not_reversible = FALSE;
 
+#ifdef FINGER_MK3
+    o->ZTau = 560.0f*1.2f; // PWM/mNm * mNm;
+    o->Ke = 0.05f;
+#endif
+
     Joint_reset_calibration_data(o);
 }
 
@@ -640,6 +645,15 @@ CTRL_UNITS Joint_do_pwm_or_current_control(Joint* o)
                             o->output = PID_do_out(pid, o->pos_err);
                         }
                     }
+
+#ifdef FINGER_MK3
+                    CTRL_UNITS omega = o->vel_fbk;
+                    if (omega < ZERO) omega = -omega;
+
+                    CTRL_UNITS pwm_lim_trq = o->ZTau + o->Ke*omega;
+
+                    LIMIT(o->output, pwm_lim_trq);
+#endif
                 }
             }
             else

diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.h
@@ -188,6 +188,10 @@ typedef struct // Joint
 
     jointCalibrationData running_calibration;
 
+#ifdef FINGER_MK3
+    CTRL_UNITS ZTau;
+    CTRL_UNITS Ke;
+#endif 
 } Joint;
 
 extern Joint* Joint_new(uint8_t n);

diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Pid.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/Pid.c
@@ -56,8 +56,14 @@ void PID_config(PID* o, eOmc_PID_t* config)
 
     o->stiction_up   = rescaler*config->stiction_up_val;
     o->stiction_down = rescaler*config->stiction_down_val;
-
+
+#ifdef FINGER_MK3  
+    o->out_max_close = config->limitonoutput;
+    o->out_max_open = 0.2f*o->out_max_close;
+#else
     o->out_max = config->limitonoutput;
+#endif
+
     o->out_lpf = 0.0f;
     o->out = 0.0f;
 
@@ -127,7 +133,32 @@ float PID_do_out(PID* o, float En)
     // offset
     out += o->Ko;
 
+#ifdef FINGER_MK3
+    if (o->Kp > ZERO)
+    {
+        if (out > ZERO)
+        {
+            LIMIT(out, o->out_max_close);
+        }
+        else
+        {
+            LIMIT(out, o->out_max_open);
+        }
+    }
+    else
+    {
+        if (out < ZERO)
+        {
+            LIMIT(out, o->out_max_close);
+        }
+        else
+        {
+            LIMIT(out, o->out_max_open);
+        }
+    }
+#else
     LIMIT(out, o->out_max);
+#endif
 
     switch (o->filter)
     {

diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Pid.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/Pid.h
@@ -49,7 +49,13 @@ typedef struct //PID
     float stiction_up;
     float stiction_down;
 
+#ifdef FINGER_MK3
+    float out_max_open;
+    float out_max_close;
+#else
     float out_max;
+#endif
+
     float out_lpf;
     float out;