def convert_adc_to_force(adc_value, a, b):
return (adc_value - b) / a
if __name__ == '__main__':
# create node
rospy.init_node('force_feedback', anonymous=True)
# Create a Python proxy for PSM2, name must match ros namespace
p = dvrk.psm('PSM1')
rate = rospy.Rate(500)
# instantiating classes
x_adc = utilities.XYdataFromADC(1)
y_adc = utilities.XYdataFromADC(0)
z_adc = utilities.ZLCdataFromADC(1)
z_joint = p.get_current_joint_effort()[2]
# # load linear equations parameters
load_lin_eq_param = np.load("calibration_equation.npz")
a_x_eq_param = load_lin_eq_param['a_x']
a_y_eq_param = load_lin_eq_param['a_y']
# create publishers
pub_fx = rospy.Publisher('/force_feedback/force_x', Float32, queue_size=1)
pub_fy = rospy.Publisher('/force_feedback/force_y', Float32, queue_size=1)
pub_fz = rospy.Publisher('/force_feedback/force_z', Float32, queue_size=1)
# get reading when force is 0
import dvrk
import utilities
from std_msgs.msg import Float32
import rospy
import numpy as np
if __name__ == '__main__':
# create node
rospy.init_node('force_data', anonymous=True)
# create publishers
pub_lc = rospy.Publisher('/force_data/load_cell', Float32, queue_size=1)
lc = utilities.XYdataFromADC(0)
rate = rospy.Rate(500)
npz_lc_data = np.load("load_cell_linear_equation_parameters.npz")
lc_lin_eq_param = npz_lc_data['lc_equation_parameters']
print 'a=', lc_lin_eq_param[0]
print 'b=', lc_lin_eq_param[1]
while not rospy.is_shutdown():
force_m = lc_lin_eq_param[0] * lc.get_value() + lc_lin_eq_param[1]
# publish data
pub_lc.publish(Float32(force_m))
rate.sleep()
from std_msgs.msg import Float32
size = 1000
# create arrays to save data
arr_data_1 = []
arr_data_2 = []
if __name__ == '__main__':
# create node
rospy.init_node('adc_listener', anonymous=True)
pub_data_1 = rospy.Publisher('/adc_listener/data1', Float32, queue_size=1)
pub_data_2 = rospy.Publisher('/adc_listener/data2', Float32, queue_size=1)
# create classes
ch_1 = utilities.XYdataFromADC(0)
ch_2 = utilities.XYdataFromADC(1)
answer = 0
while not rospy.is_shutdown():
for i in range(0, 1000):
arr_data_1.append(ch_1.get_value())
arr_data_2.append(ch_2.get_value())
pub_data_1.publish(Float32(ch_1.get_value()))
pub_data_2.publish(Float32(ch_2.get_value()))
answer = raw_input("Write data? (y/n) ")
if answer == 'y':
fname_str = 'noise_data' + str(datetime.now())
thefile = open('{0}.txt'.format(fname_str), 'a')
thefile.write('1st_channel 2nd_channel\n')