admin

Δραστηριότητα “Ακολούθησε τη γραμμή” με Raspberry Pi Pico και AI κάμερα Huskylens

Σε αυτή τη δραστηριότητα θα συνδέσουμε το Huskylens με το Raspberry Pi Pico και θα προγραμματίσουμε το Pico να ακολουθεί μία μαύρη γραμμή.

Η Huskylens είναι μια κάμερα AI από την dfrobot.com . Έχει πολλά χαρακτηριστικά όπως αναγνώριση προσώπου, αναγνώριση αντικειμένων, παρακολούθηση αντικειμένων, αναγνώριση χρωμάτων, παρακολούθηση γραμμών. Μπορεί να εκπαιδευτεί μόνο με ένα κλικ στο κουμπί ή μερικές γραμμές κωδικών που υποστηρίζονται σε γλώσσα python, micropython και Arduino. Περισσότερα για το Huskylens.

Κατάλογος υλικών

  • 1Huskylens Σύνδεσμος
  • 1 Raspberry Pi Pic, Σύνδεσμος
  • 1 Πλακέτα επέκτασης Wukong2040 Breakout Board For Raspberry Pi Pico (ή άλλη, )
  • 2 Κινητήρες DC, σύνδεσμός
  • 2 Προσαρμογείς για εξαρτήματα lego (R) πανω στους κινητήρες , σύνδεσμος για 3Δ εκτύπωση
  • εξαρτήματα και τροχοί LEGO (r)

Σημείωση: Πριν ξεκινήσετε, πρέπει να γνωρίζετε για το Raspberry Pi Pico και το Thonny IDE καθώς πρόκειται να χρησιμοποιήσουμε το Thonny IDE για τον προγραμματισμό του Pico. Δείτε άλλα άρθρα σε αυτό το ιστολόγιο.

Σύνδεση κυκλώματος

  • Πραγματοποιήστε όλες τις συνδέσεις όπως φαίνεται στο διάγραμμα κυκλώματος.

Ρυθμίστε στην κάμερα Huskylens το πρωτόκολλο σε I2C. Ακολουθήστε για λεπτομερείς οδηγίες: https://wiki.dfrobot.com/HUSKYLENS_V1.0_SKU_SEN0305_SEN0336.

Πιέστε δεξιά – αριστερά το αριστερό πλήκτρο και πιέστε το στην επιλογή “Line Tracking”, κατόπιν πατήστε το δεξιό πλήκτρο για να ξεκινήσετε την λειτουργία εκμάθησης γραμμής

Κώδικας

ΒΗΜΑ 1ο. Δημιουργία βιβλιοθήκης Huskylens στο Pico
*Αντιγράψτε τον παρακάτω κώδικα και αποθηκεύστε ως huskylensPythonLibrary.py στον κατάλογο Raspberry Pi Pico.

  #Library credits: RRoy  https://community.dfrobot.com/makelog-310469.html
 
import ubinascii
import time
from machine import UART,I2C,Pin
 
commandHeaderAndAddress = "55AA11"
algorthimsByteID = {
    "ALGORITHM_OBJECT_TRACKING": "0100",
    "ALGORITHM_FACE_RECOGNITION": "0000",
    "ALGORITHM_OBJECT_RECOGNITION": "0200",
    "ALGORITHM_LINE_TRACKING": "0300",
    "ALGORITHM_COLOR_RECOGNITION": "0400",
    "ALGORITHM_TAG_RECOGNITION": "0500",
    "ALGORITHM_OBJECT_CLASSIFICATION": "0600"
}
 
COMMAND_REQUEST_CUSTOMNAMES= 0x2f
COMMAND_REQUEST_TAKE_PHOTO_TO_SD_CARD = 0x30
COMMAND_REQUEST_SAVE_MODEL_TO_SD_CARD = 0x32
COMMAND_REQUEST_LOAD_MODEL_FROM_SD_CARD = 0x33
COMMAND_REQUEST_CUSTOM_TEXT = 0x34
COMMAND_REQUEST_CLEAR_TEXT = 0x35
COMMAND_REQUEST_LEARN_ONECE = 0x36
COMMAND_REQUEST_FORGET = 0x37
COMMAND_REQUEST_SCREENSHOT_TO_SD_CARD = 0x39
COMMAND_REQUEST_FIRMWARE_VERSION = 0x3C
 
class HuskyLensLibrary:
    def __init__(self, proto):
        self.proto=proto
        self.address=0x32
        if(self.proto=="SERIAL"):
            self.huskylensSer = UART(2,baudrate=9600,rx=33,tx=32,timeout=100)
        else :
            self.huskylensSer = I2C(0, scl=Pin(5), sda=Pin(4), freq=100000)
        self.lastCmdSent = ""
 
    def writeToHuskyLens(self, cmd):
        self.lastCmdSent = cmd
        if(self.proto=="SERIAL"):
            self.huskylensSer.write(cmd)
        else :
            self.huskylensSer.writeto(self.address, cmd)
 
    def calculateChecksum(self, hexStr):
        total = 0
        for i in range(0, len(hexStr), 2):
            total += int(hexStr[i:i+2], 16)
        hexStr = hex(total)[-2:]
        return hexStr
 
    def cmdToBytes(self, cmd):
        return ubinascii.unhexlify(cmd)
 
    def splitCommandToParts(self, str):
        headers = str[0:4]
        address = str[4:6]
        data_length = int(str[6:8], 16)
        command = str[8:10]
        if(data_length > 0):
            data = str[10:10+data_length*2]
        else:
            data = []
        checkSum = str[2*(6+data_length-1):2*(6+data_length-1)+2]
        return [headers, address, data_length, command, data, checkSum]
 
    def getBlockOrArrowCommand(self):
        if(self.proto=="SERIAL"):
                    byteString = self.huskylensSer.read(5)
                    byteString += self.huskylensSer.read(int(byteString[3]))
                    byteString += self.huskylensSer.read(1)
        else:
                    byteString  = self.huskylensSer.readfrom(self.address,5)
                    #print("byteString= ", byteString)
                    #print(type(byteString))
                    #for i in range (len(byteString)):
                    #    print(byteString[i])
                    byteString += self.huskylensSer.readfrom(self.address,byteString[3]+1)
                    #print("byteString 2= ", byteString)
                    #print(type(byteString))
                    #for i in range (len(byteString)):
                    #   print(byteString[i])
        commandSplit = self.splitCommandToParts(''.join(['%02x' % b for b in byteString]))
        print("commandSplit 2= ", commandSplit)
        print(type(commandSplit))
        for i in range (len(commandSplit)):
            print(commandSplit[i])
        return commandSplit[4]
 
    def processReturnData(self):
        inProduction = True
        if(inProduction):
            try:
                if(self.proto=="SERIAL"):
                    byteString = self.huskylensSer.read(5)
                    byteString += self.huskylensSer.read(int(byteString[3]))
                    byteString += self.huskylensSer.read(1)
                else:
                    byteString  = self.huskylensSer.readfrom(self.address,5)
                    ##print("______")
                    ##print(byteString)
                    ##print(byteString[3])
                    byteString += self.huskylensSer.readfrom(self.address,byteString[3]+1)
                    ##print("=======")
                    ##print(byteString)
                commandSplit = self.splitCommandToParts(''.join(['%02x' % b for b in byteString]))
                print("before finalData, ", commandSplit)
                if(commandSplit[3] == "2e"):
                    return "Knock Recieved"
                else:
                    returnData = []
                    numberOfBlocksOrArrow = int(
                        commandSplit[4][2:4]+commandSplit[4][0:2], 16)
                    numberOfIDLearned = int(
                        commandSplit[4][6:8]+commandSplit[4][4:6], 16)
                    frameNumber = int(
                        commandSplit[4][10:12]+commandSplit[4][8:10], 16)
                    for i in range(numberOfBlocksOrArrow):
                        returnData.append(self.getBlockOrArrowCommand())
                    finalData=[]
                    tmp=[]
                    #change this code to mesume xtail and xhead from 256 to 320. sanagn@uoc.gr
                    for i in returnData:
                        tmp = []
                        for q in range(0, len(i), 4):
                            low=int(i[q:q+2], 16)
                            high=int(i[q+2:q+4], 16)
                            if(high>0):
                                val=low+255+high
                            else:
                                val=low
                            tmp.append(val)
                        finalData.append(tmp)
                        tmp = []
                        print(type(finalData))
                        for i in range (len(finalData)):
                           print(finalData[i])                        
                    '''
                    for i in returnData:
                        tmp=[]
                        for q in range(0,len(i),4):
                            tmp.append(int(i[q:q+2],16)+int(i[q+2:q+4],16))
                        finalData.append(tmp)
                        tmp=[]
                        print("finalData= ", finalData)
                    '''
                    return finalData
            except:
                 print("Read error")
                 return []
                
    def command_request_knock(self):
        cmd = self.cmdToBytes(commandHeaderAndAddress+"002c3c")
        self.writeToHuskyLens(cmd)
        return self.processReturnData()
 
    def command_request(self):
        cmd = self.cmdToBytes(commandHeaderAndAddress+"002030")
        self.writeToHuskyLens(cmd)
        return self.processReturnData()
 
    def command_request_blocks(self):
        cmd = self.cmdToBytes(commandHeaderAndAddress+"002131")
        self.writeToHuskyLens(cmd)
        return self.processReturnData()
 
    def command_request_arrows(self):
        cmd = self.cmdToBytes(commandHeaderAndAddress+"002232")
        self.writeToHuskyLens(cmd)
        return self.processReturnData()
 
    def command_request_learned(self):
        cmd = self.cmdToBytes(commandHeaderAndAddress+"002333")
        self.writeToHuskyLens(cmd)
        return self.processReturnData()
 
    def command_request_blocks_learned(self):
        cmd = self.cmdToBytes(commandHeaderAndAddress+"002434")
        self.writeToHuskyLens(cmd)
        return self.processReturnData()
 
    def command_request_arrows_learned(self):
        cmd = self.cmdToBytes(commandHeaderAndAddress+"002535")
        self.writeToHuskyLens(cmd)
        return self.processReturnData()
 
    def line_tracking_mode(self):
        cmd = self.cmdToBytes(commandHeaderAndAddress+"022d030042")
        self.writeToHuskyLens(cmd)
        return self.processReturnData()
 
    def face_recognition_mode(self):
        cmd = self.cmdToBytes(commandHeaderAndAddress+"022d00003f")
        self.writeToHuskyLens(cmd)
        return self.processReturnData()
 
    def object_tracking_mode(self):
        cmd = self.cmdToBytes(commandHeaderAndAddress+"022d010040")
        self.writeToHuskyLens(cmd)
        return self.processReturnData()
 
    def object_recognition_mode(self):
        cmd = self.cmdToBytes(commandHeaderAndAddress+"022d020041")
        self.writeToHuskyLens(cmd)
        return self.processReturnData()
 
    def color_recognition_mode(self):
        cmd = self.cmdToBytes(commandHeaderAndAddress+"022d040043")
        self.writeToHuskyLens(cmd)
        return self.processReturnData()
 
    def tag_recognition_mode(self):
        cmd = self.cmdToBytes(commandHeaderAndAddress+"022d050044")
        self.writeToHuskyLens(cmd)
        return self.processReturnData()
 
    def command_request_by_id(self, idVal):
        idVal = "{:04x}".format(idVal)
        idVal = idVal[2:]+idVal[0:2]
        cmd = commandHeaderAndAddress+"0226"+idVal
        cmd += self.calculateChecksum(cmd)
        cmd = self.cmdToBytes(cmd)
        self.writeToHuskyLens(cmd)
        return self.processReturnData()
 
    def command_request_blocks_by_id(self, idVal):
        idVal = "{:04x}".format(idVal)
        idVal = idVal[2:]+idVal[0:2]
        cmd = commandHeaderAndAddress+"0227"+idVal
        cmd += self.calculateChecksum(cmd)
        cmd = self.cmdToBytes(cmd)
        self.writeToHuskyLens(cmd)
        return self.processReturnData()
 
    def command_request_arrows_by_id(self, idVal):
        idVal = "{:04x}".format(idVal)
        idVal = idVal[2:]+idVal[0:2]
        cmd = commandHeaderAndAddress+"0228"+idVal
        cmd += self.calculateChecksum(cmd)
        cmd = self.cmdToBytes(cmd)
        self.writeToHuskyLens(cmd)
        return self.processReturnData()
 
    def command_request_algorthim(self, alg):
        if alg in algorthimsByteID:
            cmd = commandHeaderAndAddress+"022d"+algorthimsByteID[alg]
            cmd += self.calculateChecksum(cmd)
            cmd = self.cmdToBytes(cmd)
            self.writeToHuskyLens(cmd)
            return self.processReturnData()
        else:
            print("INCORRECT ALGORITHIM NAME")
            
    #---------------------  8.5 update new features           
    #display text on husky's screen    
    def command_request_custom_text(self, text,x,y):
 
        textLength = len(text)
        dataLength = textLength+4
        cmd = commandHeaderAndAddress #[0x55,0xAA,0x11] [85, 170, 17]
        cmd += "{:02x}".format(dataLength)   #length of data [4+len(msg)=6]
        #cmd += str(34)#COMMAND_REQUEST_CUSTOM_TEXT = 0x34,[52]
        cmd += "{:02x}".format(COMMAND_REQUEST_CUSTOM_TEXT) 
        #first 4 digits, len,cor_x1,cor_x2,cor_y 
        #len
        cmd += "{:02x}".format(dataLength)  
        #cor_x1,cor_x2    
        if x > 255:
            data_1 = 0xff
            cmd += "{:02x}".format(data_1)      
            data_2 = x % 256        
            cmd += "{:02x}".format(data_2)   
        else:
            cmd += "{:02x}".format(0)
            cmd += "{:02x}".format(x)
        #cor_y 
        cmd += "{:02x}".format(y)
        
        
        for char in text:
            cmd += "{:02x}".format(ord(char))
            
        cmd += self.calculateChecksum(cmd)
        cmd = self.cmdToBytes(cmd)
        self.writeToHuskyLens(cmd)
        #print("cmd:",cmd)
        return self.processReturnData()
        
     #clearscreen
    def command_request_clear_text(self):
        cmd = commandHeaderAndAddress#[0x55,0xAA,0x11] [85, 170, 17]
        dataLength = 0
        cmd += "{:02x}".format(dataLength)
        cmd += "{:02x}".format(COMMAND_REQUEST_CLEAR_TEXT)
        cmd += self.calculateChecksum(cmd)
        cmd = self.cmdToBytes(cmd)
        self.writeToHuskyLens(cmd)
        return self.processReturnData()
 
    
    #Save photos to SD card
    def command_request_photo(self):
        cmd = commandHeaderAndAddress
        dataLength = 0
        cmd += "{:02x}".format(dataLength)
        cmd += "{:02x}".format(COMMAND_REQUEST_TAKE_PHOTO_TO_SD_CARD)
        cmd += self.calculateChecksum(cmd)
        cmd = self.cmdToBytes(cmd)
        self.writeToHuskyLens(cmd)
        return self.processReturnData()
        
        
    #forget algorithm
    def command_request_forget(self):
        cmd = commandHeaderAndAddress
        dataLength = 0
        cmd += "{:02x}".format(dataLength)
        cmd += "{:02x}".format(COMMAND_REQUEST_FORGET)       
        cmd += self.calculateChecksum(cmd)
        cmd = self.cmdToBytes(cmd)
        self.writeToHuskyLens(cmd)
        return self.processReturnData()
 
   
    #Save screenshot to SD card
    def command_request_screenshot(self):
        cmd = commandHeaderAndAddress
        dataLength = 0
        cmd += "{:02x}".format(dataLength)
        cmd += "{:02x}".format(COMMAND_REQUEST_SCREENSHOT_TO_SD_CARD)      
        cmd += self.calculateChecksum(cmd)
        cmd = self.cmdToBytes(cmd)
        self.writeToHuskyLens(cmd)
        return self.processReturnData()
       
    #Learn id once
    def command_request_learn_once(self,id):
        cmd = commandHeaderAndAddress        
        dataLength = 2
        cmd += "{:02x}".format(dataLength)
        cmd += "{:02x}".format(COMMAND_REQUEST_LEARN_ONECE)   
        id = [id & 0xff, (id >> 8) & 0xff]   
        cmd += "{:02x}".format(id[0])
        cmd += "{:02x}".format(id[1])        
        cmd += self.calculateChecksum(cmd)
        cmd = self.cmdToBytes(cmd)
        self.writeToHuskyLens(cmd)
        return self.processReturnData()
        
    # Custom name id name
    def command_request_customnames(self, id, name):
        nameLength = len(name)
        dataLength = nameLength+3
        cmd = commandHeaderAndAddress        
        cmd += "{:02x}".format(dataLength)
        cmd += "{:02x}".format(COMMAND_REQUEST_CUSTOMNAMES)   
        
        cmd += "{:02x}".format(id)
        cmd += "{:02x}".format(nameLength+1)
        for char in name:
            cmd += "{:02x}".format(ord(char))           
        cmd += "{:02x}".format(0)
        cmd += self.calculateChecksum(cmd)
        cmd = self.cmdToBytes(cmd)
        self.writeToHuskyLens(cmd)
        return self.processReturnData()
        
   #Save model to SD       
    def command_request_save_model_to_SD_card(self,index):
        cmd = commandHeaderAndAddress        
        dataLength = 2
        cmd += "{:02x}".format(dataLength)
        cmd += "{:02x}".format(COMMAND_REQUEST_SAVE_MODEL_TO_SD_CARD)   
        index = [index & 0xff, (index >> 8) & 0xff]   
        cmd += "{:02x}".format(index[0])
        cmd += "{:02x}".format(index[1])        
        cmd += self.calculateChecksum(cmd)
        cmd = self.cmdToBytes(cmd)
        self.writeToHuskyLens(cmd)
        return self.processReturnData()
     
    #Read model to SD card 
    def command_request_load_model_from_SD_card(self, index):
        cmd = commandHeaderAndAddress        
        dataLength = 2
        cmd += "{:02x}".format(dataLength)
        cmd += "{:02x}".format(COMMAND_REQUEST_LOAD_MODEL_FROM_SD_CARD)   
        index = [index & 0xff, (index >> 8) & 0xff]   
        cmd += "{:02x}".format(index[0])
        cmd += "{:02x}".format(index[1])        
        cmd += self.calculateChecksum(cmd)
        cmd = self.cmdToBytes(cmd)
        self.writeToHuskyLens(cmd)
        return self.processReturnData()

ΒΗΜΑ 2 Κωδικός αναγνώρισης γραμμής
*Αντιγράψτε τον δεδομένο κώδικα και αποθηκεύστε στον κατάλογο Raspberry Pi Pico ως main.py

import machine
from huskylensPythonLibrary import HuskyLensLibrary
import utime
import time
import uos

husky = HuskyLensLibrary("I2C") ## SERIAL OR I2C
#φροντιζουμε η καμερα να είναι σε κατασταση  line tracking

# Ορίζουμε τους ακροδέκτες GPIO για τους κινητήρες του ρομπότ
M1A = machine.PWM(machine.Pin(10)) 
M1B = machine.PWM(machine.Pin(11)) 
M1A.freq(50) 
M1B.freq(50) 
M2A = machine.PWM(machine.Pin(20)) 
M2B = machine.PWM(machine.Pin(21)) 
M2A.freq(50) 
M2B.freq(50)

# Ορίζουμε τη λειτουργία των κινητήρων του ρομπότ
def move__forword(speed): # Ο κύκλος λειτουργίας πρέπει να είναι μεταξύ 0 και 65535
    #print("Fspeed"+str(speed))
    M1A.duty_u16(0) 
    M1B.duty_u16(speed)
    M2A.duty_u16(0)
    M2B.duty_u16(speed)  
def move__backword(speed): # Ο κύκλος λειτουργίας πρέπει να είναι μεταξύ 0 και 65535
    M1A.duty_u16(speed)
    M1B.duty_u16(0)
    M2A.duty_u16(speed)
    M2B.duty_u16(0)      
def move_left(speed): # Ο κύκλος λειτουργίας πρέπει να είναι μεταξύ 0 και 65535
    #print("Lspeed"+str(speed))
    M1A.duty_u16(0) 
    M1B.duty_u16(0)
    M2A.duty_u16(0)
    M2B.duty_u16(speed)
def move_right(speed):# Ο κύκλος λειτουργίας πρέπει να είναι μεταξύ 0 και 65535
    #print("Rspeed"+str(speed))
    M1A.duty_u16(0) 
    M1B.duty_u16(speed)
    M2A.duty_u16(0)
    M2B.duty_u16(0)
def stop_robot(speed):
    M1A.duty_u16(0) 
    M1B.duty_u16(speed)
    M2A.duty_u16(0)
    M2B.duty_u16(0)


# Main loop
utime.sleep_ms(1000)
while True:
    data = husky.command_request_arrows()
    #print(type(data))
    #print(data)
    #r = data[0] stack if data is empty!
    #print(f"[ Head:(x:{r[0]}, y:{r[1]}), Tail:(x:{r[2]}, y:{r[3]})")
    #print ("δεδομένα:")
    for i in data:
        XTail = int(i[0])
        YTail= i[1]
        XHead = int(i[2])
        YHead  = i[3]
        #print ("Χ-αρχής="+ str(XTail) +", Υ-αρχής="+ str(YTail)+",Χ-τελους="+str(XHead)+", Υ-τέλους="+str(YHead))
    Rten = XHead - 160 #160 είναι το μέσο Χ της κάμερας
    Rdir = XHead
    #print ("Rten="+str(Rten)+", Rdir="+ str(Rdir)) 
    #error = (int32_t)result.xTarget;
    # Analyze data and determine the position of the line
    # Example: Parse data to extract line position information

    # Control motors based on line position
    # Example: If line is more to the left, turn left. If more to the right, turn right.
    if Rdir > 180:
        move_right(abs(Rten)*400)
        utime.sleep_ms(abs(Rten))
    elif Rdir < 130:
        move_left(abs(Rten)*400)
        utime.sleep_ms(abs(Rten))
    elif (Rdir > 130) &(Rdir < 180):
        move__forword(40000)
    else:
        stop_robot(0)
    utime.sleep_ms(10)  # Adjust as needed for control frequency