added tick system to bench

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()

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()
+            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
+
+

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()

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()

catkin_ws/src/bench/msg/BenchMotorControl.msg

@@ -1,3 +1,5 @@
+uint64 tick
+
 float32 flex_myobrick_pwm
 float32 extend_myobrick_pwm
 

catkin_ws/src/bench/msg/BenchState.msg

@@ -1,3 +1,5 @@
+uint64 tick
+
 float32 angle
 
 bool safety_switch_pressed