Switch NXT UTZ from pio to normal I2C communication

lib/nxt/ultra.py

@@ -3,8 +3,10 @@ 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
+
+NXT_UTZ_I2C_ADDR = 0x01
 
-# I2C Nonblocking via PIO
 @rp2.asm_pio(autopush=False, autopull=False, out_shiftdir=rp2.PIO.SHIFT_RIGHT, sideset_init=rp2.PIO.OUT_HIGH, out_init=rp2.PIO.OUT_HIGH)
 def I2C_ReadSensor():
     wrap_target()           # Acting as while loop
@@ -69,20 +71,21 @@ class UltrasonicSensor:
         # Define if data should be read periodicaly or only when data is needed
         self.periodic = periodic
         self.use_nonblocking = use_nonblocking
-        
+        self.clock_pulse = int(1/I2C_NXT_UTZ_FREQ*2*1000000)
+
         # Inicialization of I2C, recommended baud rate is 9600, capable of 30000
         if(self.use_nonblocking == True):
             SDA = Pin(UTZ_I2C_SDA_PIN,mode=Pin.OPEN_DRAIN, pull=Pin.PULL_UP)
             SCL = Pin(UTZ_I2C_SCK_PIN,mode=Pin.OPEN_DRAIN, pull=Pin.PULL_UP)
-            self.sm0 = rp2.StateMachine(0, I2C_ReadSensor, out_base=SDA, sideset_base=SCL, freq=30000*8)
-            self.sm1 = rp2.StateMachine(1, I2C_ReadSensor2, in_base=SDA, out_base=SDA, sideset_base=SCL, freq=30000*8)
+            self.sm0 = rp2.StateMachine(0, I2C_ReadSensor, out_base=SDA, sideset_base=SCL, freq=I2C_NXT_UTZ_FREQ*8)
+            self.sm1 = rp2.StateMachine(1, I2C_ReadSensor2, in_base=SDA, out_base=SDA, sideset_base=SCL, freq=I2C_NXT_UTZ_FREQ*8)
             
             self.sm1.irq(self.handler2)
             self.sm1.active(1)
             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=(30000))
+            self.i2c = I2C(id=0,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]
@@ -91,7 +94,8 @@ class UltrasonicSensor:
         
         # If periodic is on, create timer to call callback every 100ms
         if(periodic == True):
-            Timer().init(freq = freq, mode=Timer.PERIODIC, callback=self.callback)
+            self.timer = Timer()
+            self.timer.init(period=100, mode=Timer.PERIODIC, callback=self.callback)
         
     def deinit(self):
         if(self.use_nonblocking == True):
@@ -100,34 +104,35 @@ class UltrasonicSensor:
             self.sm1.irq(handler=None)
             self.sm1.active(0)
         if self.periodic == True:
-            Timer().deinit()
+            self.timer.deinit()
 
     def read_data_from_sensor(self):
         try:
             # Write byte 0x42 to read data from sensor
             buff = bytearray([0x42])
-            self.i2c.writeto(0x02,buff)
+            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(20)
+            utime.sleep_us(self.clock_pulse)
             self.help_pin.toggle()
             
             # Reinitialize I2C
-            self.i2c = I2C(id=0,scl=Pin(17), sda=Pin(16), freq=(30000))
+            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(0x02,8)
+                x = self.i2c.readfrom(NXT_UTZ_I2C_ADDR, 8)
                 self.read_buffer = list(x)
             # Read first target
             else:
-                x = self.i2c.readfrom(0x02,1)
-                print('here', x)
+                x = self.i2c.readfrom(NXT_UTZ_I2C_ADDR, 1)
                 self.read_buffer = list(x)
+            
                 
         # If I2c throws an error use except
-        except:
+        except Exception as e:
+            print(e)
             print("Error during I2C communication with ultrasonic sensor")
             
     # Return measured distance

lib/robot.py

@@ -187,7 +187,7 @@ class Sensors:
         if sensor_type is not None:
             if sensor_type == Sensor.NXT_ULTRASONIC:
                 if isinstance(self.ultra_nxt, EmptyObject):
-                    self.ultra_nxt = nxt.UltrasonicSensor(periodic = False)
+                    self.ultra_nxt = nxt.UltrasonicSensor(periodic = True, use_nonblocking=False)
             elif sensor_type == Sensor.GYRO_ACC:
                 if isinstance(self.gyro_acc, EmptyObject):
                     self.gyro_acc = brick_ll.ICM()

lib/robot_consts.py

@@ -82,9 +82,10 @@ PCF_CHECK_PERIOD = const(50)		# ms
 I2C_SLAVE_READ_PERIOD = const(1)	# ms
 
 I2C_FREQ = const(400000)			# Hz
+I2C_NXT_UTZ_FREQ = const(30000)             # Hz
 I2C_MULTICUBE_FREQ = const(100000)
 
 ESP_BAUDRATE = const(115200)		# bps
 ESP32_COMMAND_PIN = const(8)
 
-FW_VERSION = "2.11.2024"		# Firmware version
\ No newline at end of file
+FW_VERSION = "3.11.2024"		# Firmware version
\ No newline at end of file