From a28c1021fe4260231e92305b1166e8ae22085808 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ozan=20Aky=C4=B1ld=C4=B1z?= Date: Thu, 18 Jan 2018 14:27:40 -0500 Subject: [PATCH 01/10] Update README.md --- README.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/README.md b/README.md index 9435065..af048fa 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,8 @@ +# OAkyildiz/reflex-ros-pkg +fork of RightHandRobotics/reflex-ros-pkg +This fork aims to fix the catkin build order problems and namespace issues + + reflex-ros-pkg ====== From 2b7486d7de46378d7beba95f74ed3a1163ad2164 Mon Sep 17 00:00:00 2001 From: Ozan Akyildiz Date: Thu, 18 Jan 2018 16:42:05 -0500 Subject: [PATCH 02/10] Fixed the issues with namespaces, now you can also put everything in a namespace --- reflex/launch/reflex_sf.launch | 2 +- reflex/src/reflex/motor.py | 10 +++++++++- reflex/src/reflex/reflex_usb_hand.py | 17 +++++++++++------ reflex/src/reflex/reflex_usb_motor.py | 2 +- reflex/yaml/reflex_sf_motor_zero_points.yaml | 8 ++++---- 5 files changed, 26 insertions(+), 13 deletions(-) diff --git a/reflex/launch/reflex_sf.launch b/reflex/launch/reflex_sf.launch index c5763b4..0a7c573 100644 --- a/reflex/launch/reflex_sf.launch +++ b/reflex/launch/reflex_sf.launch @@ -22,5 +22,5 @@ --port reflex_sf_port reflex_sf_f1 reflex_sf_f2 reflex_sf_f3 reflex_sf_preshape" output="screen"/> - + diff --git a/reflex/src/reflex/motor.py b/reflex/src/reflex/motor.py index 2a0e876..3f48cf2 100644 --- a/reflex/src/reflex/motor.py +++ b/reflex/src/reflex/motor.py @@ -33,7 +33,15 @@ def __init__(self, name): Assumes that "name" is the name of the controller with a preceding slash, e.g. /reflex_takktile_f1 or /reflex_sf_f1 ''' - self.name = name[1:] + # self.name = name[1:] # prone-tofail + # this is better if RHR wants to keep it: + # self.name=name[1:] if name[0] ='/' else name + + ''' + But why do that, and then manually add slash? Instead: + ''' + self.name=name + self._DEFAULT_MOTOR_SPEED = rospy.get_param(name + '/default_motor_speed') self._MAX_MOTOR_SPEED = rospy.get_param(name + '/max_motor_speed') self._MAX_MOTOR_TRAVEL = rospy.get_param(name + '/max_motor_travel') diff --git a/reflex/src/reflex/reflex_usb_hand.py b/reflex/src/reflex/reflex_usb_hand.py index 8b2160e..e4374a7 100755 --- a/reflex/src/reflex/reflex_usb_hand.py +++ b/reflex/src/reflex/reflex_usb_hand.py @@ -40,10 +40,12 @@ motor_names = ['_f1', '_f2', '_f3', '_preshape'] class ReflexUSBHand(ReflexHand): - def __init__(self): + def __init__(self,name): self.usb_hand_type = rospy.get_param('usb_hand_type') - self.init_namespace = '/' + self.usb_hand_type + self.init_namespace = name + super(ReflexUSBHand, self).__init__(self.init_namespace, ReflexUSBMotor) + #print("after super:" + self.namespace) self.hand_state_pub = rospy.Publisher(self.namespace + '/hand_state', reflex_msgs.msg.Hand, queue_size=10) self.encoder_last_value = [0, 0, 0] #This will be updated constantly in _receive_enc_state_cb() @@ -57,7 +59,7 @@ def __init__(self): if (self.usb_hand_type == "reflex_plus"): self.enc_subscriber = rospy.Subscriber('/encoder_states', Encoder, self._receive_enc_state_cb) rospy.Service(self.namespace + '/calibrate_fingers', Empty, self.calibrate_auto) - self.encoder_zero_point = rospy.get_param('/enc_zero_points') + self.encoder_zero_point = rospy.get_param('/enc_zero_points') #remove slash? else: rospy.Service(self.namespace + '/calibrate_fingers', Empty, self.calibrate_manual) @@ -230,9 +232,9 @@ def calc_distal_angle(self, joint_angle, proximal): else: return diff -def main(): +def main(name): rospy.sleep(4.0) # To allow services and parameters to load - hand = ReflexUSBHand() + hand = ReflexUSBHand(name) rospy.on_shutdown(hand.disable_torque) r = rospy.Rate(20) while not rospy.is_shutdown(): @@ -241,4 +243,7 @@ def main(): if __name__ == '__main__': - main() + import sys + #this needs to be slightly revised as and empty 'args' parameter in roslaunch causes problems + nm='reflex_sf' if len(sys.argv)<=1 else sys.argv[1] + main(nm) diff --git a/reflex/src/reflex/reflex_usb_motor.py b/reflex/src/reflex/reflex_usb_motor.py index 80c9f3f..107c4d1 100644 --- a/reflex/src/reflex/reflex_usb_motor.py +++ b/reflex/src/reflex/reflex_usb_motor.py @@ -33,7 +33,7 @@ class ReflexUSBMotor(Motor): def __init__(self, name): super(ReflexUSBMotor, self).__init__(name) - self.zero_point = rospy.get_param(self.name + '/zero_point') + self.zero_point = rospy.get_param(self.name + "/zero_point") self.MOTOR_TO_JOINT_GEAR_RATIO = rospy.get_param(self.name + '/motor_to_joint_gear_ratio') self.MOTOR_TO_JOINT_INVERTED = rospy.get_param(self.name + '/motor_to_joint_inverted') self.motor_cmd_pub = rospy.Publisher(name + '/command', Float64, queue_size=10) diff --git a/reflex/yaml/reflex_sf_motor_zero_points.yaml b/reflex/yaml/reflex_sf_motor_zero_points.yaml index b7bf7e3..800f3d8 100644 --- a/reflex/yaml/reflex_sf_motor_zero_points.yaml +++ b/reflex/yaml/reflex_sf_motor_zero_points.yaml @@ -1,4 +1,4 @@ -reflex_sf_f1: {zero_point: 23.337983706892146} -reflex_sf_f2: {zero_point: 20.15037162966578} -reflex_sf_f3: {zero_point: 24.664877088413224} -reflex_sf_preshape: {zero_point: 24.907246052899154} +reflex_sf_f1: {zero_point: 25.171090748415487} +reflex_sf_f2: {zero_point: 21.078430006336596} +reflex_sf_f3: {zero_point: 20.961847466457286} +reflex_sf_preshape: {zero_point: 21.715032033309136} From 3a6063b77e94e77852868475265032bab507d9f1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ozan=20Aky=C4=B1ld=C4=B1z?= Date: Thu, 18 Jan 2018 16:58:56 -0500 Subject: [PATCH 03/10] Update reflex_plus.launch also added the args required for reflex_plus, since the py file is modified --- reflex/launch/reflex_plus.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/reflex/launch/reflex_plus.launch b/reflex/launch/reflex_plus.launch index a57bc06..3f5935b 100644 --- a/reflex/launch/reflex_plus.launch +++ b/reflex/launch/reflex_plus.launch @@ -23,5 +23,5 @@ --port reflex_plus_port reflex_plus_f1 reflex_plus_f2 reflex_plus_f3 reflex_plus_preshape" output="screen"/> - + From 6bb0fbd81f1cd288fca67251521e68627b365b28 Mon Sep 17 00:00:00 2001 From: Ozan Akyildiz Date: Thu, 1 Feb 2018 12:10:38 -0500 Subject: [PATCH 04/10] launch files? i don'T remeber what I changed --- reflex/src/reflex/reflex_usb_hand.py | 2 +- reflex/yaml/reflex_sf_motor_zero_points.yaml | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/reflex/src/reflex/reflex_usb_hand.py b/reflex/src/reflex/reflex_usb_hand.py index e4374a7..af03b0f 100755 --- a/reflex/src/reflex/reflex_usb_hand.py +++ b/reflex/src/reflex/reflex_usb_hand.py @@ -45,7 +45,7 @@ def __init__(self,name): self.init_namespace = name super(ReflexUSBHand, self).__init__(self.init_namespace, ReflexUSBMotor) - #print("after super:" + self.namespace) + self.hand_state_pub = rospy.Publisher(self.namespace + '/hand_state', reflex_msgs.msg.Hand, queue_size=10) self.encoder_last_value = [0, 0, 0] #This will be updated constantly in _receive_enc_state_cb() diff --git a/reflex/yaml/reflex_sf_motor_zero_points.yaml b/reflex/yaml/reflex_sf_motor_zero_points.yaml index 800f3d8..bef063e 100644 --- a/reflex/yaml/reflex_sf_motor_zero_points.yaml +++ b/reflex/yaml/reflex_sf_motor_zero_points.yaml @@ -1,4 +1,4 @@ -reflex_sf_f1: {zero_point: 25.171090748415487} -reflex_sf_f2: {zero_point: 21.078430006336596} -reflex_sf_f3: {zero_point: 20.961847466457286} -reflex_sf_preshape: {zero_point: 21.715032033309136} +reflex_sf_f1: {zero_point: 25.07444995877869} +reflex_sf_f2: {zero_point: 20.894352311790318} +reflex_sf_f3: {zero_point: 20.86674065760838} +reflex_sf_preshape: {zero_point: 21.572371820035773} From d59a80f132c426a9a0e9344cb31e0b48e6526a9a Mon Sep 17 00:00:00 2001 From: Ozan Akyildiz Date: Sat, 19 May 2018 19:43:50 -0400 Subject: [PATCH 05/10] macroified the hand model to support multiple girppers --- .../urdf/full_reflex_model.urdf.xacro | 1207 +++++++++-------- 1 file changed, 604 insertions(+), 603 deletions(-) diff --git a/reflex_visualizer/urdf/full_reflex_model.urdf.xacro b/reflex_visualizer/urdf/full_reflex_model.urdf.xacro index a1fb1c5..257524a 100644 --- a/reflex_visualizer/urdf/full_reflex_model.urdf.xacro +++ b/reflex_visualizer/urdf/full_reflex_model.urdf.xacro @@ -1,7 +1,9 @@ - + + + @@ -34,12 +36,12 @@ - + + name="${tf_prefix}finger[${finger_id}]/flex_link[${flex_id}]"> + name="${tf_prefix}"> @@ -58,9 +60,9 @@ - - - + + + @@ -68,81 +70,89 @@ - - - + + + - - - + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -150,534 +160,525 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 44b62fe67d460d83dc29182e1e2973ab9a2a492b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ozan=20Aky=C4=B1ld=C4=B1z?= Date: Fri, 15 Jun 2018 16:15:29 -0400 Subject: [PATCH 06/10] added default calibration values added default calibration values to avoid a merge conflict --- reflex/yaml/reflex_sf_motor_zero_points.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/reflex/yaml/reflex_sf_motor_zero_points.yaml b/reflex/yaml/reflex_sf_motor_zero_points.yaml index bef063e..b7bf7e3 100644 --- a/reflex/yaml/reflex_sf_motor_zero_points.yaml +++ b/reflex/yaml/reflex_sf_motor_zero_points.yaml @@ -1,4 +1,4 @@ -reflex_sf_f1: {zero_point: 25.07444995877869} -reflex_sf_f2: {zero_point: 20.894352311790318} -reflex_sf_f3: {zero_point: 20.86674065760838} -reflex_sf_preshape: {zero_point: 21.572371820035773} +reflex_sf_f1: {zero_point: 23.337983706892146} +reflex_sf_f2: {zero_point: 20.15037162966578} +reflex_sf_f3: {zero_point: 24.664877088413224} +reflex_sf_preshape: {zero_point: 24.907246052899154} From 45bda96279c03c472300e6380e9ff4a43086529b Mon Sep 17 00:00:00 2001 From: Ozan Akyildiz Date: Fri, 15 Jun 2018 20:53:28 -0400 Subject: [PATCH 07/10] added xml hint to visualizer.launch, extra calib files --- reflex/yaml/left_hand_motor_zero_points.yaml | 4 ++++ reflex/yaml/right_hand_motor_zero_points.yaml | 4 ++++ reflex/yaml/this_is_left_hand.yaml | 4 ++++ reflex_visualizer/launch/reflex_visualizer.launch | 1 + 4 files changed, 13 insertions(+) create mode 100644 reflex/yaml/left_hand_motor_zero_points.yaml create mode 100644 reflex/yaml/right_hand_motor_zero_points.yaml create mode 100644 reflex/yaml/this_is_left_hand.yaml diff --git a/reflex/yaml/left_hand_motor_zero_points.yaml b/reflex/yaml/left_hand_motor_zero_points.yaml new file mode 100644 index 0000000..98dc326 --- /dev/null +++ b/reflex/yaml/left_hand_motor_zero_points.yaml @@ -0,0 +1,4 @@ +reflex_sf_f1: {zero_point: 20.840662984214323} +reflex_sf_f2: {zero_point: 20.377400786272858} +reflex_sf_f3: {zero_point: 25.471750982841073} +reflex_sf_preshape: {zero_point: 21.573905800823656} diff --git a/reflex/yaml/right_hand_motor_zero_points.yaml b/reflex/yaml/right_hand_motor_zero_points.yaml new file mode 100644 index 0000000..315ebff --- /dev/null +++ b/reflex/yaml/right_hand_motor_zero_points.yaml @@ -0,0 +1,4 @@ +reflex_sf_f1: {zero_point: 20.627439654698218} +reflex_sf_f2: {zero_point: 20.363594959181885} +reflex_sf_f3: {zero_point: 22.788818584829084} +reflex_sf_preshape: {zero_point: 21.71043009094548} diff --git a/reflex/yaml/this_is_left_hand.yaml b/reflex/yaml/this_is_left_hand.yaml new file mode 100644 index 0000000..2dae907 --- /dev/null +++ b/reflex/yaml/this_is_left_hand.yaml @@ -0,0 +1,4 @@ +reflex_sf_f1: {zero_point: 20.773167829547354} +reflex_sf_f2: {zero_point: 20.49244934536428} +reflex_sf_f3: {zero_point: 25.615945176902322} +reflex_sf_preshape: {zero_point: 21.567769877672117} diff --git a/reflex_visualizer/launch/reflex_visualizer.launch b/reflex_visualizer/launch/reflex_visualizer.launch index 48edb56..59accc9 100644 --- a/reflex_visualizer/launch/reflex_visualizer.launch +++ b/reflex_visualizer/launch/reflex_visualizer.launch @@ -1,3 +1,4 @@ + From 7071eca199c685cffdb7a80f820f951d77559282 Mon Sep 17 00:00:00 2001 From: Ozan Akyildiz Date: Sat, 16 Jun 2018 01:18:20 -0400 Subject: [PATCH 08/10] small change of parent params --- reflex_visualizer/urdf/full_reflex_model.urdf.xacro | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/reflex_visualizer/urdf/full_reflex_model.urdf.xacro b/reflex_visualizer/urdf/full_reflex_model.urdf.xacro index 257524a..add3928 100644 --- a/reflex_visualizer/urdf/full_reflex_model.urdf.xacro +++ b/reflex_visualizer/urdf/full_reflex_model.urdf.xacro @@ -3,6 +3,7 @@ + @@ -85,7 +86,7 @@ - + @@ -147,8 +148,8 @@ - - + + From b6afc5aae9a46d57c40fa6d353ad3761dcd3f8c1 Mon Sep 17 00:00:00 2001 From: HiRo Lab Date: Fri, 5 Oct 2018 08:02:52 -0400 Subject: [PATCH 09/10] calibration file names --- reflex/yaml/left_hand_motor_zero_points.yaml | 8 ++++---- reflex/yaml/right_hand_motor_zero_points.yaml | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/reflex/yaml/left_hand_motor_zero_points.yaml b/reflex/yaml/left_hand_motor_zero_points.yaml index 98dc326..6b2dca0 100644 --- a/reflex/yaml/left_hand_motor_zero_points.yaml +++ b/reflex/yaml/left_hand_motor_zero_points.yaml @@ -1,4 +1,4 @@ -reflex_sf_f1: {zero_point: 20.840662984214323} -reflex_sf_f2: {zero_point: 20.377400786272858} -reflex_sf_f3: {zero_point: 25.471750982841073} -reflex_sf_preshape: {zero_point: 21.573905800823656} +reflex_sf_f1: {zero_point: 20.95724552409363} +reflex_sf_f2: {zero_point: 20.28075999663606} +reflex_sf_f3: {zero_point: 24.543692606170257} +reflex_sf_preshape: {zero_point: 21.764119418521478} diff --git a/reflex/yaml/right_hand_motor_zero_points.yaml b/reflex/yaml/right_hand_motor_zero_points.yaml index 315ebff..67f6d1b 100644 --- a/reflex/yaml/right_hand_motor_zero_points.yaml +++ b/reflex/yaml/right_hand_motor_zero_points.yaml @@ -1,4 +1,4 @@ -reflex_sf_f1: {zero_point: 20.627439654698218} -reflex_sf_f2: {zero_point: 20.363594959181885} -reflex_sf_f3: {zero_point: 22.788818584829084} -reflex_sf_preshape: {zero_point: 21.71043009094548} +reflex_sf_f1: {zero_point: 22.929944817314563} +reflex_sf_f2: {zero_point: 20.469439633545996} +reflex_sf_f3: {zero_point: 22.88545937446588} +reflex_sf_preshape: {zero_point: 23.58955655610539} From 4b5b858717ade04783b539e899d7c233797a1e61 Mon Sep 17 00:00:00 2001 From: HiRo Lab Date: Tue, 23 Oct 2018 03:14:23 -0400 Subject: [PATCH 10/10] fixed the param names for zero points and calibration process --- reflex/src/reflex/reflex_usb_hand.py | 38 ++++++++++++++-------------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/reflex/src/reflex/reflex_usb_hand.py b/reflex/src/reflex/reflex_usb_hand.py index af03b0f..f55ca32 100755 --- a/reflex/src/reflex/reflex_usb_hand.py +++ b/reflex/src/reflex/reflex_usb_hand.py @@ -41,9 +41,9 @@ class ReflexUSBHand(ReflexHand): def __init__(self,name): - self.usb_hand_type = rospy.get_param('usb_hand_type') + self.usb_hand_type = rospy.get_param('usb_hand_type') self.init_namespace = name - + super(ReflexUSBHand, self).__init__(self.init_namespace, ReflexUSBMotor) self.hand_state_pub = rospy.Publisher(self.namespace + '/hand_state', @@ -62,7 +62,7 @@ def __init__(self,name): self.encoder_zero_point = rospy.get_param('/enc_zero_points') #remove slash? else: rospy.Service(self.namespace + '/calibrate_fingers', Empty, self.calibrate_manual) - + def _receive_enc_state_cb(self, data): #Receives and processes the encoder state #print("encoder 1: " + str(data.encoders[0]) + " encoder 2: " + str(data.encoders[1]) + " encoder 3: " + str(data.encoders[2])) @@ -76,7 +76,7 @@ def _receive_enc_state_cb(self, data): #motor_angles[i] = raw_motor_angle self.distal_approx[i] = self.calc_distal_angle(motor_joint_angle, self.proximal_angle[i]) #print motor_angles - + def _receive_cmd_cb(self, data): self.disable_force_control() self.set_speeds(data.velocity) @@ -145,7 +145,7 @@ def calibrate_auto(self, data=None): j=0 while(1): enc_pos = self.encoder_last_value[i] - if (j==0): + if (j==0): j=1 last = enc_pos if ((abs(enc_pos-last) < self.calibration_error)): @@ -174,20 +174,20 @@ def _write_zero_point_data_to_file(self, filename, data): outfile.write(yaml.dump(data)) def _zero_current_pose(self): - data = dict( - reflex_sf_f1=dict(zero_point=self.motors[self.namespace + '_f1'].get_current_raw_motor_angle()), - reflex_sf_f2=dict(zero_point=self.motors[self.namespace + '_f2'].get_current_raw_motor_angle()), - reflex_sf_f3=dict(zero_point=self.motors[self.namespace + '_f3'].get_current_raw_motor_angle()), - reflex_sf_preshape=dict(zero_point=self.motors[self.namespace + '_preshape'].get_current_raw_motor_angle()) - ) + data = dict() + data[self.namespace+"_f1"]=dict(zero_point=self.motors[self.namespace + "_f1"].get_current_raw_motor_angle()) + data[self.namespace+"_f2"]=dict(zero_point=self.motors[self.namespace + "_f2"].get_current_raw_motor_angle()) + data[self.namespace+"_f3"]=dict(zero_point=self.motors[self.namespace + "_f3"].get_current_raw_motor_angle()) + data[self.namespace+"_preshape"]=dict(zero_point=self.motors[self.namespace + "_preshape"].get_current_raw_motor_angle()) + if (self.usb_hand_type == 'reflex_plus'): data = dict( - reflex_plus_f1 = data['reflex_sf_f1'], - reflex_plus_f2 = data['reflex_sf_f2'], - reflex_plus_f3 = data['reflex_sf_f3'], - reflex_plus_preshape = data['reflex_sf_preshape'] + reflex_plus_f1 = data[self.namespace+'_f1'], + reflex_plus_f2 = data[self.namespace+'_f2'], + reflex_plus_f3 = data[self.namespace+'_f3'], + reflex_plus_preshape = data[self.namespace+'_preshape'] ) - self._write_zero_point_data_to_file(self.usb_hand_type + '_motor_zero_points.yaml', data) + self._write_zero_point_data_to_file(self.namespace + '_motor_zero_points.yaml', data) #Encoder data processing functions are based off encoder functions used #For the reflex_takktile hand as in reflex_driver_node.cpp @@ -197,12 +197,12 @@ def calibrate_encoders_locally(self, data): self.encoder_zero_point[i] = data[i]*self.enc_scale self.encoder_offset[i] = 0; data = dict(enc_zero_points = self.encoder_zero_point) - self._write_zero_point_data_to_file('reflex_plus_encoder_zero_points.yaml', data) + self._write_zero_point_data_to_file('reflex_plus_encoder_zero_points.yaml', data) def update_encoder_offset(self, raw_value): #Given a raw and past (self.encoder_last_value) value, track encoder wrapes (self.enc_offset) offset = self.encoder_offset[:] - + for i in range(0,3): if (offset[i]==-1): #This happens at start up @@ -213,7 +213,7 @@ def update_encoder_offset(self, raw_value): offset[i] = offset[i] + 16383 elif (self.encoder_last_value[i] - raw_value[i] < -5000): offset[i] = offset[i] - 16383 - + self.encoder_offset = offset[:] def calc_proximal_angle(self, raw_value, zero, offset):