from machine import I2C,UART,Pin
from  Sengo2  import *
import time

# Wait for Sengo2 to complete the initialization of the operating system. This waiting time cannot be removed to prevent the situation where the controller has already developed and sent instructions before Sengo2 has been fully initialized
time.sleep(3)

# Choose UART or I2C communication mode. Sengo2 is I2C mode by default. You can switch between the two by short pressing the mode button.
# Four UART communication modes: UART9600 (Standard Protocol Instruction), UART57600 (Standard Protocol Instruction), UART115200 (Standard Protocol Instruction), Simple9600 (Simple Protocol Instruction)
#########################################################################################################
# port = UART(2,rx=Pin(16),tx=Pin(17),baudrate=9600)
port = I2C(0,scl=Pin(21),sda=Pin(20),freq=400000)

# The communication address of Sengo2 is 0x60. If multiple devices are connected to the I2C bus, please avoid address conflicts.
sengo2 = Sengo2(0x60)

err = sengo2.begin(port)
print("sengo2.begin: 0x%x"% err)
 
# 1. sengo2 can learn 15 objects;
# 2. For better recognition results, the learning scenarios (including but not limited to background, lighting, angle, etc.) should be consistent with or relatively similar to the actual usage ones.
# 3. If it is necessary to detect an object from multiple angles within a large range, the object needs to be learned from different angles at different distances, and they will be stored as ID1...IDn respectively. When in use, the output label value only needs to be one of the above IDs.
err = sengo2.VisionBegin(sengo2_vision_e.kVisionLearning)
print("sengo2.VisionBegin(sengo2_vision_e.kVisionLearning):0x%x"% err)


while True:
# Sengo2 does not actively return the detection and recognition results; it requires the main control board to send instructions for reading.
# The reading process: 1.read the number of recognition results. 2.After receiving the instruction, Sengo2 will refresh the result data. 3.If the number of results is not zero, the board will then send instructions to read the relevant information. 
# (Please be sure to build the program according to this process.)
    obj_num = sengo2.GetValue(sengo2_vision_e.kVisionLearning, sentry_obj_info_e.kStatus)
    if obj_num:
        print("Totally %d objects: "%obj_num)
        for i in range(1,obj_num+1):
            #The x, y, w, h values returned by the deep learning algorithm are fixed so do not require reading processing.
            l = sengo2.GetValue(sengo2_vision_e.kVisionLearning, sentry_obj_info_e.kLabel, i)
            print("object%d: label=%d"%(i,l))
            time.sleep(0.2)
            print("\n")
