Put wrist decoupler code under macro WRIST_MK2. EMS ver 1.76 AMC ver 1.1

robotology · Oct 10, 2023 · e349115 · e349115
1 parent 448f63c
commit e349115
Show file tree

Hide file tree

Showing 8 changed files with 103 additions and 200 deletions.
diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theApplication_config.h b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theApplication_config.h
@@ -45,9 +45,9 @@ namespace embot { namespace app { namespace eth {
 //            {102, 5},
 //    #endif            
 //#else            
-            {1, 0},  
+            {1, 1},  
 //#endif            
-            {2023, Month::Oct, Day::six, 10, 00}
+            {2023, Month::Oct, Day::ten, 14, 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
@@ -81,21 +81,21 @@ extern "C" {
 #define EOMTHEEMSAPPLCFG_VERSION_MAJOR          (VERSION_MAJOR_OFFSET+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         2023
 //  <o> month           <1-12>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH        9
+#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH        10
 //  <o> day             <1-31>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY          14
+#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY          10
 //  <o> hour            <0-23>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR         11
+#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR         14
 //  <o> minute          <0-59>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN          21
+#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN          00
 //  </h>build date
 // </h>Info 
 

diff --git a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/proj/ems4rd.diagnostic2ready.uvoptx b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/proj/ems4rd.diagnostic2ready.uvoptx
@@ -3010,7 +3010,7 @@
 
   <Group>
     <GroupName>eo-emsappl-cfg</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>

diff --git a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/proj/ems4rd.diagnostic2ready.uvprojx b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/proj/ems4rd.diagnostic2ready.uvprojx
@@ -1417,8 +1417,8 @@
         <TargetCommonOption>
           <Device>STM32F407IG</Device>
           <Vendor>STMicroelectronics</Vendor>
-          <PackID>Keil.STM32F4xx_DFP.2.16.0</PackID>
-          <PackURL>http://www.keil.com/pack/</PackURL>
+          <PackID>Keil.STM32F4xx_DFP.2.17.0</PackID>
+          <PackURL>https://www.keil.com/pack/</PackURL>
           <Cpu>IRAM(0x20000000-0x2001FFFF) IRAM2(0x10000000-0x1000FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(25000000) CPUTYPE("Cortex-M4") FPU2</Cpu>
           <FlashUtilSpec></FlashUtilSpec>
           <StartupFile>"Startup\ST\STM32F4xx\startup_stm32f4xx.s" ("STM32F4xx Startup Code")</StartupFile>
@@ -2909,8 +2909,8 @@
         <TargetCommonOption>
           <Device>STM32F407IG</Device>
           <Vendor>STMicroelectronics</Vendor>
-          <PackID>Keil.STM32F4xx_DFP.2.16.0</PackID>
-          <PackURL>http://www.keil.com/pack/</PackURL>
+          <PackID>Keil.STM32F4xx_DFP.2.17.0</PackID>
+          <PackURL>https://www.keil.com/pack/</PackURL>
           <Cpu>IRAM(0x20000000-0x2001FFFF) IRAM2(0x10000000-0x1000FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(25000000) CPUTYPE("Cortex-M4") FPU2</Cpu>
           <FlashUtilSpec></FlashUtilSpec>
           <StartupFile>"Startup\ST\STM32F4xx\startup_stm32f4xx.s" ("STM32F4xx Startup Code")</StartupFile>
@@ -4503,8 +4503,8 @@
         <TargetCommonOption>
           <Device>STM32F407IG</Device>
           <Vendor>STMicroelectronics</Vendor>
-          <PackID>Keil.STM32F4xx_DFP.2.16.0</PackID>
-          <PackURL>http://www.keil.com/pack/</PackURL>
+          <PackID>Keil.STM32F4xx_DFP.2.17.0</PackID>
+          <PackURL>https://www.keil.com/pack/</PackURL>
           <Cpu>IRAM(0x20000000-0x2001FFFF) IRAM2(0x10000000-0x1000FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(25000000) CPUTYPE("Cortex-M4") FPU2</Cpu>
           <FlashUtilSpec></FlashUtilSpec>
           <StartupFile>"Startup\ST\STM32F4xx\startup_stm32f4xx.s" ("STM32F4xx Startup Code")</StartupFile>

diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c
@@ -513,6 +513,7 @@ static void update_jointAndMotor_withJointset_constraints(void)
                 o->joint[j].not_reversible = TRUE;
             }
         }
+        #ifdef WRIST_MK2
         if(o->jointSet[s].special_constraint == eomc_jsetconstraint_ergocubwrist)
         {
             JointSet_init_wrist_decoupler(&(o->jointSet[s]));
@@ -525,6 +526,7 @@ static void update_jointAndMotor_withJointset_constraints(void)
                 o->joint[j].belong2WristMK2 = TRUE;
             }
         }
+        #endif
 
 //#ifdef WRIST_MK2 moved to JointSet_init_wrist_decoupler function
 //        if(o->jointSet[s].special_constraint == eomc_jsetconstraint_ergocubwrist)
@@ -2021,21 +2023,21 @@ void MController_go_idle(void)
 
 void MController_get_joint_state(int j, eOmc_joint_status_t* joint_state)
 {
-//    static uint32_t count =0;
-//    
-//    count++;
+
     Joint *j_ptr= smc->joint+j;
-    //#ifndef WRIST_MK2
+#ifdef WRIST_MK2
     if((!j_ptr->belong2WristMK2) || ((j_ptr->belong2WristMK2) && (j_ptr->control_mode == eomc_controlmode_notConfigured)))
     {
         Joint_get_state(j_ptr, joint_state);
     }
-    //#else
     else
     {
         JointSet_get_state(&(smc->jointSet[0]), j, joint_state);
     }
-    //#endif
+
+#else
+     Joint_get_state(j_ptr, joint_state);
+#endif
 
     AbsEncoder* enc_ptr = smc->absEncoder + j*smc->multi_encs;
 
@@ -2045,8 +2047,7 @@ void MController_get_joint_state(int j, eOmc_joint_status_t* joint_state)
         //joint_state->addinfo.multienc[k] = count;
     }
 
-//    if(count>10000)
-//        count = 0;
+
 }
 
 
@@ -2110,14 +2111,15 @@ void MController_config_joint_vel_ref_timeout(int j, int32_t timeout_ms)
 BOOL MController_set_joint_pos_ref(int j, CTRL_UNITS pos_ref, CTRL_UNITS vel_ref)
 {
     Joint *j_ptr= smc->joint+j;
-//#if !defined(WRIST_MK2)
+
+#ifdef WRIST_MK2
     if(!j_ptr->belong2WristMK2)
         return Joint_set_pos_ref(j_ptr, pos_ref, vel_ref);
-//#else
     else
-
         return JointSet_set_pos_ref(&(smc->jointSet[0]), j, pos_ref, vel_ref);
-//#endif
+#else
+        return Joint_set_pos_ref(j_ptr, pos_ref, vel_ref);
+#endif
 }
 
 BOOL MController_set_joint_vel_ref(int j, CTRL_UNITS vel_ref, CTRL_UNITS acc_ref)
@@ -2128,13 +2130,16 @@ BOOL MController_set_joint_vel_ref(int j, CTRL_UNITS vel_ref, CTRL_UNITS acc_ref
 BOOL MController_set_joint_pos_raw(int j, CTRL_UNITS pos_ref)
 {
     Joint *j_ptr= smc->joint+j;
-//#if !defined(WRIST_MK2)
+#ifdef WRIST_MK2
     if(!j_ptr->belong2WristMK2)
         return Joint_set_pos_raw(j_ptr, pos_ref);
-//#else
+
     else
         return JointSet_set_pos_ref(&(smc->jointSet[0]), j, pos_ref, 0.0f);
-//#endif
+#else
+
+     return Joint_set_pos_raw(j_ptr, pos_ref);
+#endif
 }
 
 BOOL MController_set_joint_vel_raw(int j, CTRL_UNITS vel_ref)
@@ -2160,13 +2165,14 @@ BOOL MController_set_joint_cur_ref(int j, CTRL_UNITS cur_ref)
 void MController_stop_joint(int j)
 {
     Joint *j_ptr= smc->joint+j;
-//#ifdef WRIST_MK2
+#ifdef WRIST_MK2
     if(j_ptr->belong2WristMK2)
         JointSet_stop(&(smc->jointSet[0]), j);
-//#else
     else
         Joint_stop(j_ptr);
-//#endif
+#else
+    Joint_stop(j_ptr);
+#endif
 }
 
 void MController_config_motor_gearbox_M2J(int m, float32_t gearbox_M2J)

diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c
@@ -156,20 +156,20 @@ void Joint_reset_calibration_data(Joint* o)
 void Joint_config(Joint* o, uint8_t ID, eOmc_joint_config_t* config)
 {
     o->ID = ID;
-//#ifndef WRIST_MK2        
-    if(!o->belong2WristMK2)
-    {
-        o->pos_min_soft = config->userlimits.min;
-        o->pos_max_soft = config->userlimits.max;    
-        o->pos_min_hard = config->hardwarelimits.min;
-        o->pos_max_hard = config->hardwarelimits.max;
-
-        o->pos_min = config->userlimits.min;
-        o->pos_max = config->userlimits.max;
 
-        o->vel_max = config->maxvelocityofjoint;
-    }
-    else
+    o->pos_min_soft = config->userlimits.min;
+    o->pos_max_soft = config->userlimits.max;    
+    o->pos_min_hard = config->hardwarelimits.min;
+    o->pos_max_hard = config->hardwarelimits.max;
+
+    o->pos_min = config->userlimits.min;
+    o->pos_max = config->userlimits.max;
+
+    o->vel_max = config->maxvelocityofjoint;
+
+
+#ifdef WRIST_MK2
+    if(o->belong2WristMK2)
     {
         const CTRL_UNITS lim = (150.0f/360.0f)*65536.0f;
 
@@ -180,8 +180,8 @@ void Joint_config(Joint* o, uint8_t ID, eOmc_joint_config_t* config)
         o->pos_max_hard =  lim+400.0f;
 
         o->vel_max = (90.0f/360.0f)*65536.0f;
-    }
-//#endif
+     }
+#endif    
 
     o->acc_max = 10000000.0f;
 
@@ -489,19 +489,17 @@ int8_t Joint_pushing_limit(Joint* o)
 
 void Joint_set_limits(Joint* o, CTRL_UNITS pos_min, CTRL_UNITS pos_max)
 {
-//#ifndef WRIST_MK2
-    if(!o->belong2WristMK2)
-    {
-        o->pos_min = pos_min;
-        o->pos_max = pos_max;
-    }
-//#else
-    else
+#ifdef WRIST_MK2
+    if(o->belong2WristMK2)
     {
         o->pos_min = -(150.0f/360.0f)*65536.0f;
         o->pos_max = -o->pos_min;
     }
-//#endif
+#else    
+    o->pos_min = pos_min;
+    o->pos_max = pos_max;
+#endif    
+
 
     Trajectory_config_limits(&o->trajectory, pos_min, pos_max, 0.0f, 0.0f);
 }
@@ -662,13 +660,13 @@ CTRL_UNITS Joint_do_pwm_or_current_control(Joint* o)
 
             //CTRL_UNITS pos_err_old = o->pos_err;
 
-//#ifdef WRIST_MK2
+#ifdef WRIST_MK2
             if(o->belong2WristMK2)
                 o->pos_err = wrap180(o->pos_ref - o->pos_fbk);
-//#else
-            else
-                o->pos_err = o->pos_ref - o->pos_fbk;
-//#endif
+#else
+
+            o->pos_err = o->pos_ref - o->pos_fbk;
+#endif
             o->vel_err = o->vel_ref - o->vel_fbk;
 
             if (o->interaction_mode == eOmc_interactionmode_stiff)