Added controller activated IMU

run_robot.py

@@ -16,10 +16,9 @@ def main(use_imu=False):
     config = Configuration()
     hardware_interface = HardwareInterface()
 
-    # Create imu handle
-    if use_imu:
-        imu = IMU(port="/dev/ttyACM0")
-        imu.flush_buffer()
+    imu_initialized = False
+    imu_activated = use_imu
+    imu = None
 
     # Create controller and user input handles
     controller = Controller(
@@ -63,10 +62,34 @@ def main(use_imu=False):
                 print("Deactivating Robot")
                 break
 
+            if command.imu_toggle_event == 1:
+                if imu_activated:
+                    print("Deactivating IMU")
+                    imu_activated = False
+                else:
+                    print("Activating IMU")
+                    imu_activated = True
+
+            if imu_activated and not imu_initialized:
+                try:
+                    # Create imu handle
+                    imu = IMU(port="/dev/ttyACM0")
+                    imu.flush_buffer()
+                    imu_initialized = True
+                except Exception:
+                    print('IMU not connected')
+                    imu_activated = False
+                    imu_initialized = False
+
             # Read imu data. Orientation will be None if no data was available
-            quat_orientation = (
-                imu.read_orientation() if use_imu else np.array([1, 0, 0, 0])
-            )
+            try:
+                quat_orientation = (
+                    imu.read_orientation() if imu_activated else np.array([1, 0, 0, 0])
+                )
+            except Exception:
+                print('IMU not connected')
+                imu_activated = False
+                imu_initialized = False
             state.quat_orientation = quat_orientation
 
             # Step the controller forward by dt

src/IMU.py

@@ -5,14 +5,17 @@
 
 class IMU:
     def __init__(self, port, baudrate=500000):
-        self.serial_handle = serial.Serial(
-            port=port,
-            baudrate=baudrate,
-            parity=serial.PARITY_NONE,
-            stopbits=serial.STOPBITS_ONE,
-            bytesize=serial.EIGHTBITS,
-            timeout=0,
-        )
+        try:
+            self.serial_handle = serial.Serial(
+                port=port,
+                baudrate=baudrate,
+                parity=serial.PARITY_NONE,
+                stopbits=serial.STOPBITS_ONE,
+                bytesize=serial.EIGHTBITS,
+                timeout=3,
+            )
+        except serial.serialutil.SerialException as err:
+            raise Exception('IMU not connected.')
         self.last_quat = np.array([1, 0, 0, 0])
         self.start_time = time.time()
 
@@ -21,25 +24,29 @@ def flush_buffer(self):
 
     def read_orientation(self):
         """Reads quaternion measurements from the Teensy until none are left. Returns the last read quaternion.
-        
+
         Parameters
         ----------
         serial_handle : Serial object
             Handle to the pyserial Serial object
-        
+
         Returns
         -------
         np array (4,)
             If there was quaternion data to read on the serial port returns the quaternion as a numpy array, otherwise returns the last read quaternion.
         """
 
         while True:
-            x = self.serial_handle.readline().decode("utf").strip()
-            if x is "" or x is None:
-                return self.last_quat
-            else:
+            try:
+                x = self.serial_handle.readline().decode("utf").strip()
+            except:
+                raise Exception("Error reading from IMU")
+            if x is not "" and x is not None:
                 parsed = x.split(",")
                 if len(parsed) == 4:
                     self.last_quat = np.array(parsed, dtype=np.float64)
                 else:
                     print("Did not receive 4-vector from imu")
+            else:
+                print("No data from IMU")
+            return self.last_quat

src/JoystickInterface.py

@@ -15,6 +15,7 @@ def __init__(
         self.previous_state = BehaviorState.REST
         self.previous_hop_toggle = 0
         self.previous_activate_toggle = 0
+        self.previous_imu_toggle = 0
 
         self.message_rate = 50
         self.udp_handle = UDPComms.Subscriber(udp_port, timeout=0.3)
@@ -25,23 +26,27 @@ def get_command(self, state, do_print=False):
         try:
             msg = self.udp_handle.get()
             command = Command()
-            
+
             ####### Handle discrete commands ########
             # Check if requesting a state transition to trotting, or from trotting to resting
             gait_toggle = msg["R1"]
             command.trot_event = (gait_toggle == 1 and self.previous_gait_toggle == 0)
 
             # Check if requesting a state transition to hopping, from trotting or resting
             hop_toggle = msg["x"]
-            command.hop_event = (hop_toggle == 1 and self.previous_hop_toggle == 0)            
-            
+            command.hop_event = (hop_toggle == 1 and self.previous_hop_toggle == 0)
+
             activate_toggle = msg["L1"]
             command.activate_event = (activate_toggle == 1 and self.previous_activate_toggle == 0)
 
+            imu_toggle = msg["triangle"]
+            command.imu_toggle_event = (imu_toggle == 1 and self.previous_imu_toggle == 0)
+
             # Update previous values for toggles and state
             self.previous_gait_toggle = gait_toggle
             self.previous_hop_toggle = hop_toggle
             self.previous_activate_toggle = activate_toggle
+            self.previous_imu_toggle = imu_toggle
 
             ####### Handle continuous commands ########
             x_vel = msg["ly"] * self.config.max_x_velocity
@@ -66,7 +71,7 @@ def get_command(self, state, do_print=False):
 
             height_movement = msg["dpady"]
             command.height = state.height - message_dt * self.config.z_speed * height_movement
-            
+
             roll_movement = - msg["dpadx"]
             command.roll = state.roll + message_dt * self.config.roll_speed * roll_movement
 
@@ -80,4 +85,4 @@ def get_command(self, state, do_print=False):
 
     def set_color(self, color):
         joystick_msg = {"ps4_color": color}
-        self.udp_publisher.send(joystick_msg)
+        self.udp_publisher.send(joystick_msg)