Disable NXT UTZ data read when sensor is disconnected

lib/nxt/ultra.py

@@ -3,7 +3,7 @@ import utime
 import rp2
 
 from lib.hw_defs.pins import UTZ_I2C_SCK_PIN, UTZ_I2C_SDA_PIN
-from lib.robot_consts import I2C_NXT_UTZ_FREQ
+from lib.robot_consts import I2C_NXT_UTZ_FREQ, I2C_NXT_UTZ_BUS
 
 NXT_UTZ_I2C_ADDR = 0x01
 
@@ -85,18 +85,21 @@ class UltrasonicSensor:
             self.sm0.irq(self.handler)
             self.sm0.active(1)
         else:    
-            self.i2c = I2C(id=0,scl=Pin(UTZ_I2C_SCK_PIN), sda=Pin(UTZ_I2C_SDA_PIN), freq=(I2C_NXT_UTZ_FREQ))
+            self.i2c = I2C(id=I2C_NXT_UTZ_BUS,scl=Pin(UTZ_I2C_SCK_PIN), sda=Pin(UTZ_I2C_SDA_PIN), freq=(I2C_NXT_UTZ_FREQ))
         # Initialize return buffer    
         if only_first_target == True:
             self.read_buffer = [255]
         else:
             self.read_buffer = [255,255,255,255,255,255,255,255]
-        
+            
+        self.running = True
         # If periodic is on, create timer to call callback every 100ms
         if(periodic == True):
             self.timer = Timer()
             self.timer.init(period=100, mode=Timer.PERIODIC, callback=self.callback)
             self.read_data_from_sensor()
+
+        
         
     def deinit(self):
         if(self.use_nonblocking == True):
@@ -104,37 +107,41 @@ class UltrasonicSensor:
             self.sm0.active(0)
             self.sm1.irq(handler=None)
             self.sm1.active(0)
+        else:
+            self.i2c = None
         if self.periodic == True:
             self.timer.deinit()
 
     def read_data_from_sensor(self):
-        try:
-            # Write byte 0x42 to read data from sensor
-            buff = bytearray([0x42])
-            self.i2c.writeto(NXT_UTZ_I2C_ADDR, buff)
-            # Because HW glitch in ultrasonic we need another clock pulse
-            self.help_pin = Pin(17, mode=Pin.OPEN_DRAIN, value=1)
-            self.help_pin.toggle()
-            utime.sleep_us(self.clock_pulse)
-            self.help_pin.toggle()
-            
-            # Reinitialize I2C
-            self.i2c = I2C(id=0,scl=Pin(UTZ_I2C_SCK_PIN), sda=Pin(UTZ_I2C_SDA_PIN), freq=(I2C_NXT_UTZ_FREQ))
-            
-            # Read 8 targets
-            if(self.oft == False):
-                x = self.i2c.readfrom(NXT_UTZ_I2C_ADDR, 8)
-                self.read_buffer = list(x)
-            # Read first target
-            else:
-                x = self.i2c.readfrom(NXT_UTZ_I2C_ADDR, 1)
-                self.read_buffer = list(x)
-            
+        if self.running:
+            try:
+                # Write byte 0x42 to read data from sensor
+                buff = bytearray([0x42])
+                self.i2c.writeto(NXT_UTZ_I2C_ADDR, buff)
+                # Because HW glitch in ultrasonic we need another clock pulse
+                self.help_pin = Pin(UTZ_I2C_SCK_PIN, mode=Pin.OPEN_DRAIN, value=1)
+                self.help_pin.toggle()
+                utime.sleep_us(self.clock_pulse)
+                self.help_pin.toggle()
+                
+                # Reinitialize I2C
+                self.i2c = I2C(id=I2C_NXT_UTZ_BUS,scl=Pin(UTZ_I2C_SCK_PIN), sda=Pin(UTZ_I2C_SDA_PIN), freq=(I2C_NXT_UTZ_FREQ))
+                
+                # Read 8 targets
+                if(self.oft == False):
+                    x = self.i2c.readfrom(NXT_UTZ_I2C_ADDR, 8)
+                    self.read_buffer = list(x)
+                # Read first target
+                else:
+                    x = self.i2c.readfrom(NXT_UTZ_I2C_ADDR, 1)
+                    self.read_buffer = list(x)
                 
-        # If I2c throws an error use except
-        except Exception as e:
-            print(e)
-            print("Error during I2C communication with ultrasonic sensor")
+                    
+            # If I2c throws an error use except
+            except Exception as e:
+                self.running = False
+                print(e)
+                print("Error during I2C communication with ultrasonic sensor")
             
     # Return measured distance
     def distance(self):

lib/robot.py

@@ -220,7 +220,6 @@ class Sensors:
         if sensor_type is not None:
             if sensor_type == Sensor.NXT_ULTRASONIC:
                 if isinstance(self.ultra_nxt, EmptyObject):
-                    self.buttons.set_pin(I2C_STRONG_PULL_RPIN_BIT, 1)
                     self.ultra_nxt = nxt.UltrasonicSensor(periodic = True, use_nonblocking=False)
             elif sensor_type == Sensor.GYRO_ACC:
                 if isinstance(self.gyro_acc, EmptyObject):