23年电赛小车，准备做控制类赛题，还有点问题，改改再说

import math
import time 
import serial
from location import Location as mt

from threading import Thread

left_num = 1.1
right_num = 1.1

left_location = 1
right_location = 2

start_flag = 1

left_port = serial.Serial(
                            port="/dev/left_roll",
                            baudrate=115200,
                            bytesize=serial.EIGHTBITS,
                            parity=serial.PARITY_NONE,
                            stopbits=serial.STOPBITS_ONE,
                            )

right_port = serial.Serial(
                            port="/dev/right_roll",
                            baudrate=115200,
                            bytesize=serial.EIGHTBITS,
                            parity=serial.PARITY_NONE,
                            stopbits=serial.STOPBITS_ONE,
                            )

def left_speed_control(speed):
    str0 = "T"+str(speed)+"\n"
    left_port.write(str0.encode("utf-8"))

def right_speed_control(speed):
    str1 = "T"+str(speed)+"\n"
    right_port.write(str1.encode("utf-8"))
    
def get_right():
    global right_num,right_port

    strr = ""
    while True:
        if right_port.inWaiting() > 0:
            data = (right_port.read()).decode("utf-8")
            if data == '\n':
                #print(strr)
                #print("ll:"+strr)
                try:

                    strr = float(strr[:-1])
                    right_num = strr
                except:
                    pass
                strr = ""
            else:
                strr = strr + data
        
            #self.queue_get.get()

def get_left():

    global left_num,left_port

    strl = ""
    while True:
        if left_port.inWaiting() > 0:
            data = (left_port.read()).decode("utf-8")
            if data == '\n':
                #print(strl)                
                try:
                        #print(1)
                    # print("strl:"+strl)
                    strl = float(strl[:-1])
                    left_num = strl
                    # print("puted"+strl)
                except:
                    #print("g")
                    pass

                    #print("g")
                strl = ""
            else:
                strl = strl + data



left_proc = Thread(target=get_left)
right_proc = Thread(target=get_right)

left_proc.start()
right_proc.start()
run = mt(0,0)
time.sleep(1)
while left_num == -1.0:
    continue
start_left = left_num
while right_num == -1.0:
    continue
start_right = right_num

while True:
    time.sleep(0.01)   
    

   # right_location = right_queue.get()
    while left_num == -1.0:
        continue
    left_location = left_num - start_left
    while right_num == -1.0:
        continue
    right_location = right_num - start_right
    #print(left_location)

    location =run.count_location(left_location,right_location)
    print(location)
 

# This is a sample Python script.
import time
import numpy as np
import random
import math

class Location:
    def __init__(self,left_location,right_location):
        # 单位mm
        self.wheelbase = 140
        self.radius = (71.52)/2
        self.last_left_location = left_location
        self.last_right_location = right_location
        self.now_location = [0,0]
        self.now_dig = 0

    def dig_config(self):
        if self.now_dig > math.pi:
            self.now_dig = self.now_dig - 2 * math.pi
        elif self.now_dig < -math.pi:
            self.now_dig = self.now_dig + 2 * math.pi
        else:
            pass
        # 旨在确保角度的区间不会起飞


    def count_location(self,left_now,right_now):
        delta_left = left_now - self.last_left_location
        delta_right = right_now - self.last_right_location
        self.last_left_location = left_now
        self.last_right_location = right_now
        # 计算和上次的差值
        dist_right = delta_right * self.radius
        dist_left = delta_left * self.radius
        dis = (dist_left + dist_right)/2
        # 计算大概的走的距离
        dig = math.atan((dist_right - dist_left)/self.wheelbase)
        return (self.flash_location(dig,dis))

    def flash_location(self,dig_add,dis_add):
        self.now_dig += dig_add
        self.dig_config()
        #print(self.now_dig)

        x_delta = math.cos(self.now_dig)*dis_add
        y_delta = math.sin(self.now_dig)*dis_add
        #print(dig_add)
        self.now_location[0] = self.now_location[0] + x_delta
        self.now_location[1] = self.now_location[1] + y_delta

        return (self.now_location)
if __name__ == '__main__':
    loc = Location(0,0)
    a = 1
    b = 1
    js = 1
    x_list = []
    y_list = []
    loc_list = []
    while js <= 1000000:
        #time.sleep(0.001)
        a += 0.001*random.random()
        b += 0.001*random.random()
        loc_list = loc.count_location(a,b)
        x_list.append( loc_list[0])
        y_list.append(loc_list[1])
        js += 1
# now it`s finally working