BicycleModelExploration/2b1_2.py at main · hepromark/BicycleModelExploration · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
import numpy as np
from BicycleSolver import BicycleSolver


def const_delta(t):
    return 0.1

def b1():
    u_values_km = np.array([20, 50, 75, 100, 200, 220,240,260,280,300])
    u_values = [u * 1000 / 3600 for u in u_values_km]

    init_vector = np.array([[0],[0]], dtype=np.float64)
    t_final = 20
    init_t = 0
    step_size = 0.2
    max_iter = int(t_final / step_size)

    histories = []
    labels = []

    y_accel_hists = []

    for u in u_values:
        y_accel_hist = []
        solver = BicycleSolver(
            m = 1400,
            a = 1.14,
            b = 1.33,
            C_alpha_f = 25000,
            C_alpha_r = 21000,
            I_z = 2420,
            u = u
        )

        def task_a_model(y, t) -> np.ndarray:
            return solver.bicycle_model(solver.A, solver.B, y, const_delta(t))

        res, hist = solver.solve(solver.rk4, task_a_model, init_vector, init_t, max_iter, step_size, y_accel_hist=y_accel_hist)

        histories.append(hist)
        stable = solver.check_stability()
        labels.append(f'{str(round(u *3.6))}km/h | stable: {stable}')
        y_accel_hists.append(y_accel_hist)

    solver.visualize_results(histories, labels, init_t, t_final, max_iter, title="Acceleration VS Time with Varied Speed", titleb="Yaw Rate VS Time with Varied Speed", y_accel_hists=y_accel_hists)

def b2():
    u_values_km = np.linspace(228, 230, 100)
    u_values = [u * 1000 / 3600 for u in u_values_km]

    init_vector = np.array([[0],[0]], dtype=np.float64)
    t_final = 20
    init_t = 0
    step_size = 0.2
    max_iter = int(t_final / step_size)

    histories = []
    labels = []

    y_accel_hists = []

    for u in u_values:
        y_accel_hist = []
        solver = BicycleSolver(
            m = 1400,
            a = 1.14,
            b = 1.33,
            C_alpha_f = 25000,
            C_alpha_r = 21000,
            I_z = 2420,
            u = u
        )

        def task_a_model(y, t) -> np.ndarray:
            return solver.bicycle_model(solver.A, solver.B, y, const_delta(t))

        if solver.check_stability():
            print(f'{u * 3600/1000:.4f}km/h is stable')
        else:
            print(f'{u * 3600/1000:.4f}km/h is unstable')
            break


if __name__=="__main__":
    b1()
    b2()