Fixed low level tilt limit restart problem.

robotology · Oct 29, 2015 · 1eb755b · 1eb755b
1 parent 43e1ecf
commit 1eb755b
Show file tree

Hide file tree

Showing 4 changed files with 18 additions and 37 deletions.
diff --git a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/proj/ems4rd-v02.uvoptx b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/proj/ems4rd-v02.uvoptx
@@ -1977,7 +1977,7 @@
 
   <Group>
     <GroupName>eo-emsappl</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -2141,7 +2141,7 @@
 
   <Group>
     <GroupName>eo-emsappl-cfg</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -2253,7 +2253,7 @@
 
   <Group>
     <GroupName>opc-protocol</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -2274,7 +2274,7 @@
 
   <Group>
     <GroupName>appl-services</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -2412,7 +2412,7 @@
 
   <Group>
     <GroupName>board-cfg</GroupName>
-    <tvExp>1</tvExp>
+    <tvExp>0</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -2602,7 +2602,7 @@
 
   <Group>
     <GroupName>eo-motor-controller</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -2891,7 +2891,7 @@
 
   <Group>
     <GroupName>eo-protocol</GroupName>
-    <tvExp>1</tvExp>
+    <tvExp>0</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -3042,7 +3042,7 @@
 
   <Group>
     <GroupName>eo-protocol-cfg-callbacks</GroupName>
-    <tvExp>1</tvExp>
+    <tvExp>0</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>

diff --git a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheEMSapplBody.c b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheEMSapplBody.c
@@ -649,10 +649,10 @@ static void s_eo_emsapplBody_encodersReader_init(EOtheEMSapplBody *p)
     cfg.SPI_callbackOnLastRead = NULL;
     cfg.SPI_callback_arg = NULL;
 
-    p->appEncReader = NULL; //eo_appEncReader_New(&cfg);
+    p->appEncReader = eo_appEncReader_New(&cfg);
 
-//    eo_errman_Assert(eo_errman_GetHandle(), (NULL != p->appEncReader), 
-//                     "error in appEncReader_New", s_eobj_ownname, &eo_errman_DescrTobedecided);
+    eo_errman_Assert(eo_errman_GetHandle(), (NULL != p->appEncReader), 
+                     "error in appEncReader_New", s_eobj_ownname, &eo_errman_DescrTobedecided);
 
 }
 

diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/EOemsController.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/EOemsController.c
@@ -537,39 +537,21 @@ extern void eo_emsController_PWM(int16_t* pwm_motor_16)
         int32_t rho0= ems->axis_controller[0]->position;
         int32_t rho1= ems->axis_controller[1]->position;
         int32_t rho2= ems->axis_controller[2]->position;
-
+        
         if (rho0*rho0+rho1*rho1+rho2*rho2-rho0*rho1-rho1*rho2-rho2*rho0>400000) // 25 deg limit
         {
-            if ((rho0 <= rho1 || rho0 <= rho2) && pwm_joint[0]<0.f)
-            {
-                eo_axisController_Stop(ems->axis_controller[0]);
-                pwm_joint[0] = 0.f;
-            }
-            else if ((rho0 >= rho1 || rho0 >= rho2) && pwm_joint[0]>0.f)
+            if ( !(rho0 <= rho1 && rho0 <= rho2 && pwm_joint[0]>0.f) && !(rho0 >= rho1 && rho0 >= rho2 && pwm_joint[0]<0.f))
             {
-                eo_axisController_Stop(ems->axis_controller[0]);
                 pwm_joint[0] = 0.f;
             }
-
-            if ((rho1 <= rho2 || rho1 <= rho0) && pwm_joint[1]<0.f)
-            {
-                eo_axisController_Stop(ems->axis_controller[1]);
-                pwm_joint[1] = 0.f;
-            }
-            else if ((rho1 >= rho2 || rho1 >= rho0) && pwm_joint[1]>0.f)
+
+            if ( !(rho1 <= rho2 && rho1 <= rho0 && pwm_joint[1]>0.f) && !(rho1 >= rho2 && rho1 >= rho0 && pwm_joint[1]<0.f))
             {
-                eo_axisController_Stop(ems->axis_controller[1]);
                 pwm_joint[1] = 0.f;
             }
-
-            if ((rho2 <= rho0 || rho2 <= rho1) && pwm_joint[2]<0.f)
-            {
-                eo_axisController_Stop(ems->axis_controller[2]);
-                pwm_joint[2] = 0.f;
-            }
-            else if ((rho2 >= rho0 || rho2 >= rho1) && pwm_joint[2]>0.f)
+
+            if ( !(rho2 <= rho0 && rho2 <= rho1 && pwm_joint[2]>0.f) && !(rho2 >= rho0 && rho2 >= rho1 && pwm_joint[2]<0.f))
             {
-                eo_axisController_Stop(ems->axis_controller[2]);
                 pwm_joint[2] = 0.f;
             }
         }

diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/EOemsControllerCfg.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/EOemsControllerCfg.h
@@ -26,7 +26,7 @@ extern "C" {
 //#define EXPERIMENTAL_SPEED_CONTROL
 
 #define USE_ONLY_QE
-    
+
 //#define SHOULDER_3rd_JOINT_COUPLING
 //#define USE_JACOBIAN
 
@@ -43,7 +43,6 @@ extern "C" {
 #if defined(HEAD_V3)
     #undef USE_2FOC_FAST_ENCODER
 #endif
-
 //#warning in v3 fare in modo che NON venga definito USE_2FOC_FAST_ENCODER 
 
 #define EMS_PERIOD           0.001f