Added calibrate to forearm. Still need to add separate file for shoul…

…der, it's in wrist for now. offsets are still not working completely.
VikramBhat00 · Jul 17, 2017 · f3f69ac · f3f69ac
1 parent 12e4291
commit f3f69ac
Show file tree

Hide file tree

Showing 2 changed files with 42 additions and 19 deletions.
diff --git a/IMU_Forearm_Control.py b/IMU_Forearm_Control.py
@@ -1,7 +1,8 @@
 import argparse
 import numpy
 import rospy
-
+import time
+
 import baxter_interface
 import baxter_external_devices
 
@@ -10,24 +11,25 @@
 
 
 
-ser = serial.Serial('/dev/ttyACM1', 115200)
+ser = serial.Serial('/dev/ttyACM0', 115200)
 
 
 
 
 def forearm_control():
     right = baxter_interface.Limb('right')
     rj = right.joint_names()
+    offsetp = calibrate(1)
     while True:
         sensorValues = ser.readline()
         if(is_float(sensorValues)): 
             ypr = sensorValues.split()
             yaw = numpy.radians(float(ypr[0]))
-            pitch = numpy.radians(float(ypr[1]))
+            pitch = numpy.radians(float(ypr[1])) - offsetp
             roll = numpy.radians(float(ypr[2]))
-            print pitch
+            print roll
             #print(str(yaw) + ", " + str(pitch) + ", " + str(roll))
-            right.set_joint_positions({rj[5]:pitch}, {rj[6]:roll})
+            right.set_joint_positions({rj[4]:roll})
             #right.set_joint_positions({rj[6]:roll})
 
 def is_float(s):
@@ -41,6 +43,12 @@ def is_float(s):
     except IndexError:
         return False
 
+def calibrate(s):
+
+    time.sleep(5)
+    sensorValues = ser.readline()
+    return numpy.radians(float(sensorValues.split()[s]))
+
 
 
 

diff --git a/IMU_Wrist_Control.py b/IMU_Wrist_Control.py
@@ -11,27 +11,38 @@
 
 
 
-ser = serial.Serial('/dev/ttyACM0', 115200)
-
+ser = serial.Serial('/dev/ttyACM1', 115200)
+y = 0
+p = 1
+r = 2
 
 
 
 def forearm_control():
     right = baxter_interface.Limb('right')
     rj = right.joint_names()
-    offset = calibrate()
+
+    sensorReadings = ser.readline()
+    if(is_float(sensorReadings)):
+        offsety = calibrate(y)
+        offsetp = calibrate(p)
+        offsetr = calibrate(r)
     right.set_joint_positions({rj[0]: 0.0})
+
     while True:
         sensorValues = ser.readline()
         if(is_float(sensorValues)): 
             ypr = sensorValues.split()
-            yaw = -numpy.radians(float(ypr[0]))
-            pitch = numpy.radians(float(ypr[1])) - offset
-            roll = numpy.radians(float(ypr[2]))
+            yaw = numpy.radians(float(ypr[0])) - offsety
+            pitch = numpy.radians(float(ypr[1])) - offsetp
+            roll = numpy.radians(float(ypr[2])) - offsetr
             print yaw
             #print(str(yaw) + ", " + str(pitch) + ", " + str(roll))
-            set_j(right, rj[3], pitch)
-            #set_j(right, rj[4], roll)
+            set_j(right, yaw, pitch)
+            #right.set_joint_positions({rj[0]:yaw})
+            #right.set_joint_positions({rj[1]:pitch})
+            #right.set_joint_positions({rj[2]:roll})
+
 
 
 def is_float(s):
@@ -46,16 +57,20 @@ def is_float(s):
         return False
 
 
-def calibrate():
+def calibrate(s):
 
-    time.sleep(3)
+    time.sleep(7)
     sensorValues = ser.readline()
-    return numpy.radians(float(sensorValues.split()[1]))
+    return numpy.radians(float(sensorValues.split()[s]))
 
-def set_j(limb, joint_name, value):
+def set_j(joint_name, yaw, pitch):
+    right = baxter_interface.Limb('right')
+    rj = right.joint_names()
 
-        joint_command = {joint_name: value}
-        limb.set_joint_positions(joint_command)
+    joint_command_y = {rj[0]: yaw}
+    joint_commany_p = {rj[1]: pitch}
+    #joint_command_r = {rj[2]: roll}
+    joint_name.set_joint_positions(joint_command_y)