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

num = 5  #Set the maximum number of detections to 5
x = 0
y = 0
i = 0

def result_output():
    global num, x, y, i
    # 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.kVisionColor, sentry_obj_info_e.kStatus)
    if obj_num:
        print("Totally %d points: "%( obj_num ))
        print("|",end='')
        for i in range(1, (obj_num + 1), 1):
            #Obtain the label (color ID) of the i-th object
            label = sengo2.GetValue(sengo2_vision_e.kVisionColor,sentry_obj_info_e.kLabel,i)
            print(label,end='|')
            time.sleep(0.2)
        print("\n")
        

# Wait for Sengo2 to  initialize 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)

# Select UART or I2C communication mode. Sengo2 is I2C mode by default. You can change it by just pressing the button.
# 4 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)
i2c = I2C(0,scl=Pin(21),sda=Pin(20),freq=400000)

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

err = sengo2.begin(i2c)
print("sengo2.begin: 0x%x"% err)


# Set the colors of the two LEDs of Sengo2. Available: Close,White,Red,Green,Yellow,Blue,Purple,Cyan
# The three parameters in sequence: the color when the target is detected and recognized, the color when the target is not detected, the brightness
# When the ambient light is insufficient, all LEDs can be set to White, and the brightness value can be set to a maximum of 15 (range: 1-15).
# Two LEDs cannot be set separately
sengo2.LedSetColor(sentry_led_color_e.kLedBlue,sentry_led_color_e.kLedBlue,1)


# The default parameters of the algorithm. If they do not need to be changed, this line of code can be deleted
sengo2.SetParamNum(sengo2_vision_e.kVisionColor,1)
# The default parameters of the algorithm. If they do not need to be changed, this line of code can be deleted
sengo2.SetParam(sengo2_vision_e.kVisionColor,[50, 50, 3, 4, 0],1)
 
 
err = sengo2.VisionBegin(sengo2_vision_e.kVisionColor)
print("sengo2.VisionBegin(sengo2_vision_e.kVisionColor):0x%x"% err)

#Set the num group parameters of the color recognition algorithm, control the number of the num group parameters by setting the num
sengo2.SetParamNum(sengo2_vision_e.kVisionColor,num)

while True:
    # num = 5 : 5 loops correspond to 5 recognition boxes respectively
    for i in range(1, (num + 1), 1):
        #Set the X-coordinate of the recognition box displayed on the screen
        x = (round(100 * i / (num + 1)))
        #Set the Y-coordinate of the recognition box displayed on the screen
        y = 50
        #Set the width at which the recognition box is displayed on the screen
        w = (round(i * 2 + 1))
        #Set the height at which the recognition box is displayed on the screen
        h = (round(i * 2 + 1))
        #Write the identification box parameters
        sengo2.SetParam(sengo2_vision_e.kVisionColor,[x, y, w, h, 0],i)
        #Delay 0.1S
        time.sleep(0.1)
    result_output()
    time.sleep(0.3)