pure5.1.py

@@ -173,7 +173,7 @@ def main():
         # logger.add('file.log')
         # logger.debug('角度 {},y {},x {},北斗角度 {}，北斗x {}，北斗y {}', str(yaw1), str(y1), str(x1),str(tcp4.heading),str(tcp4.east),str(tcp4.north))
         v.append(state.v)
-        # print('v', v[-1])
+        print('v', v[-1])
 
         t.append(time)
         # plt.cla()

pure_back_1.py

@@ -309,246 +309,9 @@ def main():
         # plt.pause(0.001)
 
 
-def car_back_right(direction):
-    print('进入右倒库')
-    a = 0
-    g1_trace = abs(math.sqrt((float(tcp4.east) - 67.97) ** 2 + (float(tcp4.north) - 45.75) ** 2))
-    g2_trace = abs(math.sqrt((float(tcp4.east) - 68.35) ** 2 + (float(tcp4.north) - 43.96) ** 2))
-    if g1_trace< a <g2_trace:
-        direction == "line"
-    else:
-        direction == "rightback"
-    if direction == "line":
-        def pure_pursuit_control(state, cx, cy, pind):
-            global fangxp
-            ind = calc_target_index(state, cx, cy)
-            # if pind >= ind:
-            #     ind = pind
-            if ind < len(cx):
-                tx = cx[ind]
-                print('tx', tx)
-                ty = cy[ind]
-                print('ty', ty)
-            else:
-                tx = cx[-1]
-                # print('tx1',tx)
-
-                ty = cy[-1]
-
-                # print('ty1',ty)
-
-                ind = len(cx) - 1
-
-            alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw
-            # print('alpha',alpha)
-            if state.v < 0:  # back
-                alpha = math.pi - alpha
-            Lf = k * state.v + Lfc
-            delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0)
-            if delta > np.pi / 6.0:
-                delta = np.pi / 6.0
-                # print('delta1', delta)
-
-            elif delta < - np.pi / 6.0:
-                delta = - np.pi / 6.0
-                # print('delta2', delta)
-            angle = delta * 57.3
-            fangxp = int(angle * 18)
-            print('fxp', fangxp)
-
-            db_file = "turndb.db"
-            conn = sqlite3.connect(db_file)
-            cur = conn.cursor()
-            # sql='INSERT INTO turndb(id) VALUES(1);'
-            sql = "update turndb set angle='%s' WHERE id = 1" % fangxp
-            cur.execute(sql)
-            conn.commit()
-            cur.close()
-            conn.close()
-            time.sleep(0.04)
-            # print(delta)
-            return delta, ind
-
-        def calc_target_index(state, cx, cy):
-            # 搜索最临近的路点
-            dx = [state.x - icx for icx in cx]
-            dy = [state.y - icy for icy in cy]
-            d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
+def car_back_right():
 
-            ind = d.index(min(d))
-            # print('ind',ind)
-            L = 0.0
-            Lf = k * state.v + Lfc
-            while Lf > L and (ind + 1) < len(cx):
-                dx = cx[ind + 1] - cx[ind]
-                dy = cx[ind + 1] - cx[ind]
-                L += math.sqrt(dx ** 2 + dy ** 2)
-                ind += 1
-                # print('ind1', ind)
-
-            return ind
-
-        def main():
-            #  设置目标路点
-            cx = []
-            cy = []
-            target_speed = 5 / 3.6  # [m/s]
-            T = 500.0  # 最大模拟时间
-            # 设置车辆的出事状态
-            state = VehicleState(x=tcp4.x_car, y=tcp4.y_car, yaw=tcp4.yaw, v=sped.speed)
-            # state = VehicleState(x=89, y=20, yaw=0.0, v=0.0)
-            lastIndex = len(cx) - 1
-            time = 0.0
-            x = [state.x]
-            # print('x',x)
-            y = [state.y]
-            # print('y',y)
-            yaw = [state.yaw]
-            v = [state.v]
-            t = [0.0]
-            target_ind = calc_target_index(state, cx, cy)
-            while True:
-                # print('heading',tcp4.heading)
-                ai = PControl(target_speed, state.v)
-                di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
-                state = update(state, ai, di)
-                time = time + dt
-                x.append(state.x)
-                x1 = x[-1]
-                print('x1', x[-1])
-                y.append(state.y)
-                y1 = y[-1]
-                print('y1', y[-1])
-                yaw.append(state.yaw)
-                yaw1 = y[-1] * 57.3
-                print('yaw', yaw[-1])
-                t.append(time)
-                # logger.add('file.log')
-                # logger.debug('角度 {},y {},x {},北斗角度 {}，北斗x {}，北斗y {}', str(yaw1), str(y1), str(x1),str(tcp4.heading),str(tcp4.east),str(tcp4.north))
-                v.append(state.v)
-                # print('v', v[-1])
-
-                t.append(time)
-                # plt.cla()
-                # plt.plot(cx, cy, ".r", label="course")
-                # plt.plot(x, y, "-b", label="trajectory")
-                # plt.plot(cx[target_ind], cy[target_ind], "go", label="target")
-                # plt.axis("equal")
-                # plt.grid(True)
-                # plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4])
-                # plt.pause(0.001)
-
-        def pure_pursuit_control(state, cx, cy, pind):
-            global fangxp
-            ind = calc_target_index(state, cx, cy)
-            # if pind >= ind:
-            #     ind = pind
-            if ind < len(cx):
-                tx = cx[ind]
-                print('tx', tx)
-                ty = cy[ind]
-                print('ty', ty)
-            else:
-                tx = cx[-1]
-                # print('tx1',tx)
-
-                ty = cy[-1]
-
-                # print('ty1',ty)
-
-                ind = len(cx) - 1
-
-            alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw
-            # print('alpha',alpha)
-            if state.v < 0:  # back
-                alpha = math.pi - alpha
-            Lf = k * state.v + Lfc
-            delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0)
-            if delta > np.pi / 6.0:
-                delta = np.pi / 6.0
-                # print('delta1', delta)
-
-            elif delta < - np.pi / 6.0:
-                delta = - np.pi / 6.0
-                # print('delta2', delta)
-            angle = delta * 57.3
-            fangxp = int(angle * 18)
-            print('fxp', fangxp)
-
-            db_file = "turndb.db"
-            conn = sqlite3.connect(db_file)
-            cur = conn.cursor()
-            # sql='INSERT INTO turndb(id) VALUES(1);'
-            sql = "update turndb set angle='%s' WHERE id = 1" % fangxp
-            cur.execute(sql)
-            conn.commit()
-            cur.close()
-            conn.close()
-            time.sleep(0.04)
-            # print(delta)
-            return delta, ind
-
-        def calc_target_index(state, cx, cy):
-            # 搜索最临近的路点
-            dx = [state.x - icx for icx in cx]
-            dy = [state.y - icy for icy in cy]
-            d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
-
-            ind = d.index(min(d))
-            # print('ind',ind)
-            L = 0.0
-            Lf = k * state.v + Lfc
-            while Lf > L and (ind + 1) < len(cx):
-                dx = cx[ind + 1] - cx[ind]
-                dy = cx[ind + 1] - cx[ind]
-                L += math.sqrt(dx ** 2 + dy ** 2)
-                ind += 1
-                # print('ind1', ind)
-
-            return ind
-
-        def main():
-            #  设置目标路点
-            cx = [67.982, 68.007, 68.028, 68.055, 68.077, 68.089, 68.114, 68.138, 68.158, 68.189, 68.208, 68.223, 68.24, 68.265, 68.29, 68.312, 68.338, 68.365]
-            cy = [45.775, 45.671, 45.569, 45.466, 45.368, 45.256, 45.156, 45.056, 44.945, 44.838, 44.739, 44.638, 44.533, 44.419, 44.314, 44.214, 44.101, 43.99]
-            target_speed = 5 / 3.6  # [m/s]
-            T = 500.0  # 最大模拟时间
-            # 设置车辆的出事状态
-            state = VehicleState(x=tcp4.x_car, y=tcp4.y_car, yaw=tcp4.yaw, v=sped.speed)
-            # state = VehicleState(x=89, y=20, yaw=0.0, v=0.0)
-            lastIndex = len(cx) - 1
-            time = 0.0
-            x = [state.x]
-            # print('x',x)
-            y = [state.y]
-            # print('y',y)
-            yaw = [state.yaw]
-            v = [state.v]
-            t = [0.0]
-            target_ind = calc_target_index(state, cx, cy)
-            while True:
-                # print('heading',tcp4.heading)
-                ai = PControl(target_speed, state.v)
-                di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
-                state = update(state, ai, di)
-                time = time + dt
-                x.append(state.x)
-                x1 = x[-1]
-                print('x1', x[-1])
-                y.append(state.y)
-                y1 = y[-1]
-                print('y1', y[-1])
-                yaw.append(state.yaw)
-                yaw1 = y[-1] * 57.3
-                print('yaw', yaw[-1])
-                t.append(time)
-                # logger.add('file.log')
-                # logger.debug('角度 {},y {},x {},北斗角度 {}，北斗x {}，北斗y {}', str(yaw1), str(y1), str(x1),str(tcp4.heading),str(tcp4.east),str(tcp4.north))
-                v.append(state.v)
-                # print('v', v[-1])
-
-                t.append(time)
-    if direction == "rightback":
+    print('进入右倒库')
     while True:
         global fangxp
         db_file = "turndb.db"
@@ -583,7 +346,7 @@ def car_back_right(direction):
 
             print('回正',fangxp)
 
-            elif 70 < float(tcp4.fang_xiang) < 110 and rc_trace<1.25:
+        elif 70 < float(tcp4.fang_xiang) < 110 and rc_trace<1.2:
             fangxp = -540
             print('右打死',fangxp)
 
@@ -736,8 +499,8 @@ if __name__ == '__main__':
     t9 = threading.Thread(target=car_back_lelf)
     t10 = threading.Thread(target=car_back_lelf_fly)
     # 启动线程
-    t5.start()
-    # t8.start()
+    # t5.start()
+    t8.start()
     # t9.start()
     # t10.start()
     t6.start()