Tracking

import cv2
import numpy as np
import math
import time
#import serial

uart_baud = 115200
#ser = serial.Serial("/dev/ttyUSB0", uart_baud)

black_min=(0,43,46)
black_max=(10,255,255)
angle_exp=0
distance_exp=0

#get the mid line of the track
def liner(img,result):
    # print(img)
    n=[len(img[:,0]),len(img[0,:])]
    # print(n,'n')
    (x_point,y_point)=np.nonzero(img)
    if len(x_point)<2:
        return 0
    f1 = np.polyfit(x_point, y_point,1)
    p1 = np.poly1d(f1)
    point1=(int(p1(0)),0)
    point2=(int(p1(n[0])),n[0])
    #print(point2,point1)
    #print((int(n[1]/2),0) , (int(n[1]/2),n[0]))
    cv2.line(result, (int(n[1]/2),0) , (int(n[1]/2),n[0]), (0, 0, 255),2)
    cv2.line(result,point1,point2,(0,255,0),2)
    
    
    cv2.imshow('inside',result)
    #print(math.atan(f1[0])*180/math.pi)
    #print([n[1]/2-p1(n[0]),f1[1]],'ret')
    angle_now=math.atan(f1[0])*180/math.pi # i'm not sure, maybe i should add a -...
    distance_now = n[1]/2-p1(n[0]) # the same as upper
    return [distance_now,angle_now]

#PID control
class Pid():

    def __init__(self, exp_val, now_val,kp,kd):
        self.KP = kp
        self.KD = kd
        self.exp_val = exp_val
        self.now_val = now_val
        #self.sum_err = 0
        self.now_err = 0
        self.last_err = 0

    def cmd_pid(self):
        self.last_err = self.now_err
        self.now_err = self.exp_val - self.now_val
        #self.sum_err += self.now_err

        self.now_val = self.KP * (self.exp_val - self.now_val) + self.KD * (self.now_err - self.last_err)
        return self.now_val

def int_transform_4hex(str_list_16nums):
    str_list_16nums=list(str_list_16nums)
    
    if len(str_list_16nums) == 4:
        
        str_list_16nums.insert(2, ' ')
        
    elif len(str_list_16nums) == 3:
        
        str_list_16nums.insert(0, '0')
        str_list_16nums.insert(2, ' ')
    elif len(str_list_16nums) == 2:
        
        str_list_16nums.insert(0, '0')
        str_list_16nums.insert(0, '0')
        str_list_16nums.insert(2, ' ')  
    elif len(str_list_16nums) == 1:
        str_list_16nums.insert(0, '0')        
        str_list_16nums.insert(0, '0')
        str_list_16nums.insert(0, '0')
        str_list_16nums.insert(2, ' ')   
    else:
        pass
    str_list_16nums = ''.join(str_list_16nums)
    
    return str_list_16nums.upper()

def trans(adjust_speed,rotate_speed):
    upper=65535
    direction_adjust='00'
    direction_rotate='00'
    adjust_speed=int(adjust_speed/0.001)
    rotate_speed=int(rotate_speed/0.01)
    if adjust_speed>0:
       direction_adjust = '01'  # to judge the direction , if not right please add a -
    if rotate_speed>0:
       direction_rotate ='01'
    adjust_speed=abs(adjust_speed)
    rotate_speed=abs(rotate_speed)
    if adjust_speed>upper:
       adjust_speed=upper
    if rotate_speed>upper:
       rotate_speed=upper
    adjust_trans=hex(adjust_speed).replace('0x','')
    rotate_trans=hex(rotate_speed).replace('0x','')
    adjust_trans=int_transform_4hex(adjust_trans)
    rotate_trans=int_transform_4hex(rotate_trans)
    #print(adjust_trans,rotate_trans)
    control_cmd='CD 0A 01 00 03 20'+' '+direction_adjust+' '+adjust_trans+' '+direction_rotate+' '+rotate_trans+' 3A'
    #print(control_cmd)
    return control_cmd
    

#main func

if __name__ == '__main__':
    cap = cv2.VideoCapture(0)  
    while True:  
        ret, frame = cap.read()  
        grid_RGB = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
 
        grid_HSV = cv2.cvtColor(grid_RGB, cv2.COLOR_RGB2HSV)
 
        lower1 = np.array([0,43,46])
        upper1 = np.array([10,255,255])
        mask1 = cv2.inRange(grid_HSV, lower1, upper1)      
        res1 = cv2.bitwise_and(grid_RGB, grid_RGB, mask=mask1)

        lower2 = np.array([156,43,46])
        upper2 = np.array([180,255,255])
        mask2 = cv2.inRange(grid_HSV, lower2, upper2)
        res2 = cv2.bitwise_and(grid_RGB,grid_RGB, mask=mask2)
 
        mask = mask1 + mask2
        #mask=cv2.inRange(frame,black_min,black_max)
        kernel=np.ones((3,3),np.uint8)
        erode=cv2.morphologyEx(mask,cv2.MORPH_ERODE,kernel)
        #cv2.imshow('mask', mask)
        #cv2.imshow('erode', erode)
        
        cv2.imshow('frame', frame)
        c = cv2.waitKey(1)
        control_val=liner(mask,frame)#calculate angle and distance
        #no.1 is dis while no.2 is angle

        adjust_speed=Pid(distance_exp,control_val[0],0.1,0.15)# expected transverse v to adjust distance , here to adjust
        rotate_speed=Pid(angle_exp,control_val[1],0.1,0.15)# expected rotate to adjust angle , here to adjust
        control_cmd=trans(adjust_speed.now_val,rotate_speed.now_val)
        d=bytes.fromhex(control_cmd)
        print(d)
        ser.flushInput()
        ############serial has not sent control_cmd yet

        time.sleep(0.05)#control period 50ms  
        if c == ord('q'):
            break
    cap.release()
    cv2.destroyAllWindows()