From 85c3a3dde09273549900dc3e907a83366d2adf64 Mon Sep 17 00:00:00 2001
From: Jacopo Losi <jacopo.losi@iit.it>
Date: Thu, 23 Mar 2023 14:22:01 +0100
Subject: [PATCH] Use calib 6 for open-close finger joints right arm of
 ergoCubSN000

Adding calibration configuration files for right arm of ergoCubSN000
Setting calibration 6 for fingers adding parameters
Setting rotor position limits min and max after first completed calibration
Check correctness of calibration procedure on ergoCubSN000
---
 ergoCubSN000/calibrators/left_arm-calib.xml   | 32 ++++++++---------
 ergoCubSN000/calibrators/right_arm-calib.xml  | 34 +++++++++----------
 .../mechanicals/left_arm-eb23-j7_10-mec.xml   |  6 ++--
 .../mechanicals/right_arm-eb22-j7_10-mec.xml  |  7 ++--
 .../motorControl/right_arm-eb22-j7_10-mc.xml  |  2 +-
 5 files changed, 41 insertions(+), 40 deletions(-)

diff --git a/ergoCubSN000/calibrators/left_arm-calib.xml b/ergoCubSN000/calibrators/left_arm-calib.xml
index 5e2c34374..f03c6cf45 100644
--- a/ergoCubSN000/calibrators/left_arm-calib.xml
+++ b/ergoCubSN000/calibrators/left_arm-calib.xml
@@ -11,27 +11,27 @@
 	</group>
 
 	<group name="HOME">
-		<param name="positionHome">            5         30         0          10         0        0       0     30.00      30.00   30.00   30.00   30.00   30.00  </param>
-		<param name="velocityHome">           10         10         10         10         10       10      10    40.00      40.00   40.00   40.00   40.00   40.00  </param>
+	<!--                                shoulder-pitch    shoulder-roll    shoulder-yaw    elbow    wrist-yaw    wrist-roll    wrist-pitch    thumb_add    thumb_oc    index_add    index_oc    middle_oc    ring_pinky_oc -->
+		<param name="positionHome">            5                 30              0           10         0            0             0             30.00        30.00       30.00        30.00       30.00          30.00    </param>
+		<param name="velocityHome">            10                10              10          10         10           10            10            40.00        40.00       40.00        40.00       40.00          40.00    </param>
 	</group>
 	<group name="CALIBRATION">
-		<param name="calibrationType">        10          10          10         10          12      12      12     12      6      12       6      6      6     </param>
-		<param name="calibration1">           4000       -3000       -3000       4000        10816   7369    21632  0       0       0       0       0       0      </param>
-		<param name="calibration2">	      0           0           0          0           0       0       0      0       9102    0       9102    9102    9102      </param>
-		<param name="calibration3">           0           0           0          0           0       0       0      0       1       0       1       1       1      </param>
-		<param name="calibration4">           0           0           0          0           0       0       0      0       0       0       0       0       0      </param>
-		<param name="calibration5">           0           0           0          0           0       0       0      0       10923   0       14564   14564   14564      </param>
-		<param name="calibrationZero">        35         -15         -52         -5          0       0       0      0       0       0       0       0       0      </param>
-		<param name="calibrationDelta">       0           0           0          0           0       0       0      0       0       0       0       0       0      </param>
-		<param name="startupPosition">        34          50          0          10          0.0     0.0     0.0      0.0     0.0     0.0     0.0     0.0     0.0    </param>
-		<param name="startupVelocity">        10.0        10.0        10.0       10.0        10.0    10.0    10.0      100.0   100.0   0.0   100     100.0   100.0   </param>
-		<param name="startupMaxPwm">          8000        8000        8000       8000        16000   16000   16000    0       0       0       0       0       0      </param>
-		<param name="startupPosThreshold">    2           2           2          2           2       2       2        90      90      90      90      90      90      </param>
+		<param name="calibrationType">        10                 10              10          10         12           12            12            12           6           12           6           6              6        </param>
+		<param name="calibration1">           4000               -3000           -3000       4000       17565        8161          21461         0            0           0            0           0              0        </param>
+		<param name="calibration2">	          0                  0               0           0          0            0             0             0            9102        0            9102        9102           9102     </param>
+		<param name="calibration3">           0                  0               0           0          0            0             0             0            1           0            1           1              1        </param>
+		<param name="calibration4">           0                  0               0           0          0            0             0             0            0           0            0           0              0        </param>
+		<param name="calibration5">           0                  0               0           0          0            0             0             0            10923       0            14564       13653          12743    </param>
+		<param name="calibrationZero">        35                -15             -52          -5         0            0             0             0            0           0            0           0              0        </param>
+		<param name="calibrationDelta">       0                  0               0           0          0            0             0             0            0           0            0           0              0        </param>
+		<param name="startupPosition">        34                 50              0           10         0.0          0.0           0.0           0.0          0.0         0.0          0.0         0.0            0.0      </param>
+		<param name="startupVelocity">        10.0               10.0            10.0        10.0       10.0         10.0          10.0          100.0        100.0       0.0          100         100.0          100.0    </param>
+		<param name="startupMaxPwm">          8000               8000            8000        8000       16000        16000         16000         0            0           0            0           0              0        </param>
+		<param name="startupPosThreshold">    2                  2               2           2          2            2             2             90           90          90           90          90             90       </param>
 	</group>
 
 	<!-- motor's encoder of joint 7 can't read -->
-
-	<param name="CALIB_ORDER"> (4 5 6) (3) (2) (0) (1) (8) (7 9 10) (11 12) </param> 
+	<param name="CALIB_ORDER"> (4 5 6)  (2) (0) (1) (3) (7 8) (10) (11 12) </param>
 
 	<action phase="startup" level="10" type="calibrate">
 		<param name="target">left_arm-mc_remapper</param>
diff --git a/ergoCubSN000/calibrators/right_arm-calib.xml b/ergoCubSN000/calibrators/right_arm-calib.xml
index 4e97a381d..021ad5de4 100644
--- a/ergoCubSN000/calibrators/right_arm-calib.xml
+++ b/ergoCubSN000/calibrators/right_arm-calib.xml
@@ -11,27 +11,27 @@
 	</group>
 
 	<group name="HOME">
-    <!--                                shoulder-pitch    shoulder-roll    shoulder-yaw    elbow    wrist-yaw    wrist-roll    wrist-pitch  -->
-		<param name="positionHome">            5                30              0           10         0             0              0               30.00      30.00   30.00   30.00   30.00   30.00  </param>
-		<param name="velocityHome">            10               10              10          10         10            10             10              40.00      40.00   40.00   40.00   40.00   40.00  </param>
+	<!-- For calib6 to set calibration5, i.e. target just multiply desidered pos in deg by 182,044444 (2^(16)/360) -->
+    <!--                                shoulder-pitch    shoulder-roll    shoulder-yaw    elbow    wrist-yaw    wrist-roll    wrist-pitch    thumb_add    thumb_oc    index_add    index_oc    middle_oc    ring_pinky_oc -->
+		<param name="positionHome">            5                30              0           10         0             0              0            30.00        30.00       30.00        30.00       30.00       30.00       </param>
+		<param name="velocityHome">            10               10              10          10         10            10             10           40.00        40.00       40.00        40.00       40.00       40.00       </param>
 	</group>
 	<group name="CALIBRATION">
-		<param name="calibrationType">         10                10             10          10         12            12             12              12      12      12      12      12      12     </param>
-		<param name="calibration1">           -4000              3000           3000       -4000       19023          28993          36030           0       0       0       0       0       0      </param>
-		<param name="calibration2">	           0                 0              0           0          0             0              0               0       0       0       0       0       0      </param>
-		<param name="calibration3">            0                 0              0           0          0             0              0               0       0       0       0       0       0      </param>
-		<param name="calibration4">            0                 0              0           0          0             0              0               0       0       0       0       0       0      </param>
-		<param name="calibration5">            0                 0              0           0          0             0              0               0       0       0       0       0       0      </param>
-		<param name="calibrationZero">         35               -15            -52         -5          0             0              0               0       0       0       0       0       0      </param>
-		<param name="calibrationDelta">        0                 0              0           0          0             0              0               0       0       0       0       0       0      </param>
-		<param name="startupPosition">         34                50             0           10         0.0           0.0            0.0             0.0     0.0     0.0     0.0     0.0     0      </param>
-		<param name="startupVelocity">         10.0              10.0           10.0        10.0       10.0          10.0           10.0            100.0   100.0   0.0   100     100.0   100.0    </param>
-		<param name="startupMaxPwm">           8000              8000           8000        8000       16000         16000          16000           0       0       0       0       0       0      </param>
-		<param name="startupPosThreshold">     2                 2              2           2          2             2              2               90      90      90      90      90      90     </param>
+		<param name="calibrationType">         10                10             10          10         12            12             12           12           6           12           6           6           6           </param>
+		<param name="calibration1">           -4000              3000           3000       -4000       19023         28993          36030        0            0           0            0           0           0           </param>
+		<param name="calibration2">	           0                 0              0           0          0             0              0            0            9102        0            9102        9102        9102        </param>
+		<param name="calibration3">            0                 0              0           0          0             0              0            0            1           0            1           1           1           </param>
+		<param name="calibration4">            0                 0              0           0          0             0              0            0            0           0            0           0           0           </param>
+		<param name="calibration5">            0                 0              0           0          0             0              0            0            10923       0            12743       13653       12743       </param>
+		<param name="calibrationZero">         35               -15            -52         -5          0             0              0            0            0           0            0           0           0           </param>
+		<param name="calibrationDelta">        0                 0              0           0          0             0              0            0            0           0            0           0           0           </param>
+		<param name="startupPosition">         34                50             0           10         0.0           0.0            0.0          0.0          0.0         0.0          0.0         0.0         0           </param>
+		<param name="startupVelocity">         10.0              10.0           10.0        10.0       10.0          10.0           10.0         30.0         30.0        30.0         30.0        30.0        30.0        </param>
+		<param name="startupMaxPwm">           8000              8000           8000        8000       16000         16000          16000        0            0           0            0           0           0           </param>
+		<param name="startupPosThreshold">     2                 2              2           2          2             2              2            90           90          90           90          90          90          </param>
 	</group>
 
-<!--	<param name="CALIB_ORDER"> (4 5 6)  (2) (0) (1) (3) (7 8) (11 12) </param> --> 
-	<param name="CALIB_ORDER"> (3) (2) (0) (1) (7 8) (11 12) </param>
+	<param name="CALIB_ORDER"> (4 5 6)  (2) (0) (1) (3) (7 8) (9 10) (11 12) </param>
 
 	<action phase="startup" level="10" type="calibrate">
 		<param name="target">right_arm-mc_remapper</param>
diff --git a/ergoCubSN000/hardware/mechanicals/left_arm-eb23-j7_10-mec.xml b/ergoCubSN000/hardware/mechanicals/left_arm-eb23-j7_10-mec.xml
index 850c70ce6..6ad99944c 100644
--- a/ergoCubSN000/hardware/mechanicals/left_arm-eb23-j7_10-mec.xml
+++ b/ergoCubSN000/hardware/mechanicals/left_arm-eb23-j7_10-mec.xml
@@ -9,7 +9,7 @@
         <!-- joint number in sub-part       0                       1                       2                           3                       -->
         <!-- joint name                                                                                                                         -->
         <param name="AxisMap">              0                       1                       2                           3                       </param>
-        <param name="AxisName">             "l_thumb_oc"           "l_index_oc"            "l_middle_oc"               "l_ring_pinky_oc" </param>
+        <param name="AxisName">             "l_thumb_oc"           "l_index_oc"            "l_middle_oc"               "l_ring_pinky_oc"        </param>
         <param name="AxisType">             "revolute"              "revolute"              "revolute"                  "revolute"              </param>
         <param name="Encoder">              182.044                 182.044                 182.044                     182.044                 </param>
         <param name="fullscalePWM">         3360                    3360                    3360                        3360                    </param>
@@ -25,8 +25,8 @@
     <group name="LIMITS">
         <param name="hardwareJntPosMax">    90              90              90              90         </param>
         <param name="hardwareJntPosMin">    0               0               0               0          </param>
-        <param name="rotorPosMin">          0               -1000          -1000           -1000          </param>
-        <param name="rotorPosMax">          0               32000               37000           24000          </param>
+        <param name="rotorPosMin">          -5000           -8000          -15000           -5000          </param>
+        <param name="rotorPosMax">          20000           27000           25000           24000          </param>
     </group>
 
     <group name="COUPLINGS">
diff --git a/ergoCubSN000/hardware/mechanicals/right_arm-eb22-j7_10-mec.xml b/ergoCubSN000/hardware/mechanicals/right_arm-eb22-j7_10-mec.xml
index 8d0ce120f..8b26e3889 100644
--- a/ergoCubSN000/hardware/mechanicals/right_arm-eb22-j7_10-mec.xml
+++ b/ergoCubSN000/hardware/mechanicals/right_arm-eb22-j7_10-mec.xml
@@ -9,7 +9,7 @@
         <!-- joint number in sub-part       0                       1                       2                           3                       -->
         <!-- joint name                                                                                                                         -->
         <param name="AxisMap">              0                       1                       2                           3                       </param>
-        <param name="AxisName">             "r_thumb_oc" "r_index_oc" "r_middle_oc"    "r_ring_pinky_oc" </param>
+        <param name="AxisName">             "r_thumb_oc"            "r_index_oc"            "r_middle_oc"               "r_ring_pinky_oc"       </param>
         <param name="AxisType">             "revolute"              "revolute"              "revolute"                  "revolute"              </param>
         <param name="Encoder">              182.044                 182.044                 182.044                     182.044                 </param>
         <param name="fullscalePWM">         3360                    3360                    3360                        3360                    </param>
@@ -23,10 +23,11 @@
     </group>
 
     <group name="LIMITS">
+        <!-- After run 1st round of calibration check best rotorPosMin and rotorPosMin on motor position in the yarpmotorgui and set them (just do that one time)-->
         <param name="hardwareJntPosMax">    90              90              90              90         </param>
         <param name="hardwareJntPosMin">    0               0               0               0          </param>
-        <param name="rotorPosMin">          0               0               0               0          </param>
-        <param name="rotorPosMax">          0               0               0               0          </param>
+        <param name="rotorPosMin">          -25000          -3000           -5000           -5000      </param>
+        <param name="rotorPosMax">          3000            32000           22000           27000      </param>
     </group>
 
     <group name="COUPLINGS">
diff --git a/ergoCubSN000/hardware/motorControl/right_arm-eb22-j7_10-mc.xml b/ergoCubSN000/hardware/motorControl/right_arm-eb22-j7_10-mc.xml
index 1ef11f40a..c7f1af030 100644
--- a/ergoCubSN000/hardware/motorControl/right_arm-eb22-j7_10-mc.xml
+++ b/ergoCubSN000/hardware/motorControl/right_arm-eb22-j7_10-mc.xml
@@ -14,7 +14,7 @@
     <!-- joint name                             thumb               index               medium              pinky               -->    
     <group name="LIMITS">
         <param name="jntPosMin">                0                   0                   0                   0                  </param>
-        <param name="jntPosMax">                85                  85                  85                  85                 </param>
+        <param name="jntPosMax">                85                  75                  85                  85                 </param>
         <param name="jntVelMax">                1000                1000                1000                1000               </param>
         <param name="motorOverloadCurrents">    2000                2000                2000                2000               </param>
         <param name="motorNominalCurrents">     700                 700                 700                 700                </param>