diff --git a/bench/bench_main.py b/bench/bench_main.py index da0400d..bc71ae2 100644 --- a/bench/bench_main.py +++ b/bench/bench_main.py @@ -9,7 +9,10 @@ from myobrick import MyoBrick from safety_observer import SafetyObserver from control_listener import ControlListener -from bench.tick_loop import TickLoop +from tick_loop import TickLoop + + +TICK_LOOP_FREQ = 100 def run_bench_node(): print("Starting bench node") @@ -57,7 +60,8 @@ def run_bench_node(): myobrick_flex, myobrick_extend, safety_observer, - freq = 20 + control_listener, + freq = TICK_LOOP_FREQ ) print('Starting state broadcaster.') state_broadcaster.start() diff --git a/bench/control_listener.py b/bench/control_listener.py index 2447d2a..fbd655e 100644 --- a/bench/control_listener.py +++ b/bench/control_listener.py @@ -14,15 +14,15 @@ def __init__(self, self.safety_observer = safety_observer # Input storage - self.input = {} + self.input_list = [] # Setup message event callback self.subscriber = rospy.Subscriber('/test_bench/BenchMotorControl', BenchMotorControl, self._on_bench_motor_command_callback) def _on_bench_motor_command_callback(self, msg): - self.myobrick_flex.set_pwm(msg.flex_myobrick_pwm) - self.myobrick_extend.set_pwm(msg.extend_myobrick_pwm) + + self.input_list.append(msg) if msg.flex_myobrick_start : self.myobrick_flex.start() @@ -32,4 +32,19 @@ def _on_bench_motor_command_callback(self, msg): if msg.reset_kill_switch : self.safety_observer.reset_kill_switch() if msg.press_kill_switch : - self.safety_observer._kill_switch() \ No newline at end of file + self.safety_observer._kill_switch() + + def get_input(self, tick): + # Get input from queue and remove all older inputs + input = None + input_list = self.input_list.copy() + for i in input_list: + if i.tick == tick: + input = i + self.input_list.remove(i) + elif i.tick < tick: + self.input_list.remove(i) + + return input + + diff --git a/bench/data_recorder.py b/bench/data_recorder.py new file mode 100644 index 0000000..41e2c26 --- /dev/null +++ b/bench/data_recorder.py @@ -0,0 +1,53 @@ +import threading +import time +import csv + +class DataRecorder: + def __init__( + self, + freq, + path, + myobrick_flex, + myobrick_extend, + angle_sensor + ): + self.freq = freq + self.path = path + self.myobrick_flex = myobrick_flex + self.myobrick_extend = myobrick_extend + self.angle_sensor = angle_sensor + + + self.stop_event = threading.Event() + + def _run(self): + with open(self.path, 'a') as csvfile: + writer = csv.writer(csvfile) + while not self.stop_event.is_set(): + flex_state = self.myobrick_flex.get_state() + extend_state = self.myobrick_extend.get_state() + + writer.writerow([ + time.time(), + self.angle_sensor.get_value(), + flex_state['pv_pos_encoder'], + flex_state['pv_torque_encoder'], + flex_state['pv_current'], + self.myobrick_flex.sp_pwm, + extend_state['pv_pos_encoder'], + extend_state['pv_torque_encoder'], + extend_state['pv_current'], + self.myobrick_extend.sp_pwm, + + ]) + + time.sleep(1/self.freq) + + def start(self): + self.stop_event.clear() + self.thread = threading.Thread(target=self._run) + self.thread.start() + + def stop(self): + self.stop_event.set() + self.thread.join() \ No newline at end of file diff --git a/bench/tick_loop.py b/bench/tick_loop.py index c2638fa..3506002 100644 --- a/bench/tick_loop.py +++ b/bench/tick_loop.py @@ -11,12 +11,14 @@ def __init__(self, myobrick_flex, myobrick_extend, safety_observer, - freq = 20 + control_listener, + freq, ): self.angel_sensor = angel_sensor self.myobrick_flex = myobrick_flex self.myobrick_extend = myobrick_extend self.safety_observer = safety_observer + self.control_listener = control_listener # Setup message event callback self.pub = rospy.Publisher('/test_bench/BenchState', BenchState, queue_size=10) @@ -30,12 +32,15 @@ def __init__(self, self.tick = 0 def _loop(self): + time_to_sleep = 0 while not self.stop_event.is_set(): # Start loop timer - start_loop_time = time.time() + t_0 = time.time() msg = BenchState() + msg.tick = self.tick + # Get current tick internal and output variables msg.angle = self.angel_sensor.get_value() @@ -54,25 +59,49 @@ def _loop(self): msg.extend_myobrick_pos_encoder = extend_state_dict['pv_pos_encoder'] msg.extend_myobrick_torque_encoder = extend_state_dict['pv_torque_encoder'] msg.extend_myobrick_current = extend_state_dict['pv_current'] - msg.extend_myobrick_pwm = self.myobrick_extend.sp_pwm msg.extend_myobrick_in_running_state = self.myobrick_extend.is_running + t_1 = time.time() # Get current tick input + input_msg = self.control_listener.get_input(self.tick) - # Set current tick output - # self.myobrick_flex.set_pwm(msg.flex_myobrick_pwm) - # self.myobrick_extend.set_pwm(msg.extend_myobrick_pwm) - + t_2 = time.time() - # Broadcast current tick variables + # Set current tick output + if input_msg is not None: + self.myobrick_flex.set_pwm(input_msg.flex_myobrick_pwm) + self.myobrick_extend.set_pwm(input_msg.extend_myobrick_pwm) + msg.flex_myobrick_pwm = input_msg.flex_myobrick_pwm + msg.extend_myobrick_pwm = input_msg.extend_myobrick_pwm + else : + self.myobrick_flex.set_pwm(0) + self.myobrick_extend.set_pwm(0) + msg.flex_myobrick_pwm = 0 + msg.extend_myobrick_pwm = 0 + + t_3 = time.time() + + # Publish tick variables self.pub.publish(msg) + t_4 = time.time() + # Increment tick self.tick += 1 # Sleep until next tick - time.sleep(1/self.freq - (time.time() - start_loop_time)) + time_to_sleep = 1/self.freq - (time.time() - t_0) + if time_to_sleep < 0: + time_to_sleep = 0 + print('tick loop is too slow') + print('time to get state: ', t_1 - t_0) + print('time to get input: ', t_2 - t_1) + print('time to set output: ', t_3 - t_2) + print('time to publish: ', t_4 - t_3) + time.sleep(time_to_sleep) + print('last time to sleep: ', time_to_sleep) + def start(self): self.thread.start() diff --git a/catkin_ws/src/bench/msg/BenchMotorControl.msg b/catkin_ws/src/bench/msg/BenchMotorControl.msg index 04b90b3..cdfaeee 100644 --- a/catkin_ws/src/bench/msg/BenchMotorControl.msg +++ b/catkin_ws/src/bench/msg/BenchMotorControl.msg @@ -1,3 +1,5 @@ +uint64 tick + float32 flex_myobrick_pwm float32 extend_myobrick_pwm diff --git a/catkin_ws/src/bench/msg/BenchState.msg b/catkin_ws/src/bench/msg/BenchState.msg index de8f892..e2df941 100644 --- a/catkin_ws/src/bench/msg/BenchState.msg +++ b/catkin_ws/src/bench/msg/BenchState.msg @@ -1,3 +1,5 @@ +uint64 tick + float32 angle bool safety_switch_pressed