EigenScript/examples/pid.eigs at main · InauguralSystems/EigenScript · 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
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
# PID Controller — classic feedback control loop
# Demonstrates: error tracking, integral/derivative terms, observer convergence

# PID state: [kp, ki, kd, integral, prev_error]
define pid_new as:
    kp is n[0]
    ki is n[1]
    kd is n[2]
    return [kp, ki, kd, 0.0, 0.0]

define pid_update as:
    state is n[0]
    error is n[1]
    dt is n[2]
    kp is state[0]
    ki is state[1]
    kd is state[2]
    integral is state[3] + error * dt
    derivative is (error - state[4]) / dt
    output is kp * error + ki * integral + kd * derivative
    new_state is [kp, ki, kd, integral, error]
    return [output, new_state]

# ---- Plant: simple mass with friction ----
# position, velocity, mass, friction
define plant_step as:
    pos is n[0]
    vel is n[1]
    force is n[2]
    mass is n[3]
    friction is n[4]
    dt is n[5]
    accel is (force - friction * vel) / mass
    vel is vel + accel * dt
    pos is pos + vel * dt
    return [pos, vel]

# ============================================================
# Scenario 1: Position control — move to target
# ============================================================
print of "--- PID Position Control ---"

target is 10.0
pos is 0.0
vel is 0.0
mass is 1.0
friction is 0.3
dt is 0.05

pid is pid_new of [2.0, 0.5, 1.0]

for step in range of 100:
    error is target - pos
    result is pid_update of [pid, error, dt]
    force is result[0]
    pid is result[1]

    plant is plant_step of [pos, vel, force, mass, friction, dt]
    pos is plant[0]
    vel is plant[1]

    if (step % 10) == 0:
        print of ("t=" + (str of (step * dt)) + "  pos=" + (str of pos) + "  vel=" + (str of vel) + "  err=" + (str of error))

print of ("Final: pos=" + (str of pos) + " target=" + (str of target))

# ============================================================
# Scenario 2: Temperature control — track setpoint with disturbance
# ============================================================
print of ""
print of "--- PID Temperature Control ---"

setpoint is 72.0
temp is 60.0
pid2 is pid_new of [1.5, 0.3, 0.8]
dt is 0.1

for step in range of 80:
    error is setpoint - temp
    result is pid_update of [pid2, error, dt]
    heater is result[0]
    pid2 is result[1]

    # Clamp heater to [0, 100]
    if heater < 0:
        heater is 0
    if heater > 100:
        heater is 100

    # Thermal model: temp drifts toward ambient (50) and heater adds energy
    ambient is 50.0
    temp is temp + (heater * 0.05 - (temp - ambient) * 0.02) * dt

    # Add disturbance at step 40 (window opens)
    if step == 40:
        temp is temp - 5.0
        print of "  ** disturbance: -5 degrees **"

    if (step % 10) == 0:
        print of ("t=" + (str of (step * dt)) + "  temp=" + (str of temp) + "  heater=" + (str of heater) + "  err=" + (str of error))

print of ("Final temp: " + (str of temp))

# ============================================================
# Scenario 3: Tuning comparison — overdamped vs underdamped
# ============================================================
print of ""
print of "--- Tuning Comparison ---"

configs is [[5.0, 0.1, 0.5], [1.0, 0.1, 3.0], [2.0, 0.5, 1.0]]
labels is ["aggressive", "overdamped", "balanced"]

for c in range of (len of configs):
    pid3 is pid_new of configs[c]
    pos is 0.0
    vel is 0.0
    max_overshoot is 0.0
    settle_step is 0

    for step in range of 100:
        error is 10.0 - pos
        result is pid_update of [pid3, error, 0.05]
        force is result[0]
        pid3 is result[1]
        plant is plant_step of [pos, vel, force, 1.0, 0.3, 0.05]
        pos is plant[0]
        vel is plant[1]
        overshoot is pos - 10.0
        if overshoot > max_overshoot:
            max_overshoot is overshoot
        diff is pos - 10.0
        if diff < 0:
            diff is 0 - diff
        if diff > 0.1:
            settle_step is step

    print of (labels[c] + ": final=" + (str of pos) + " overshoot=" + (str of max_overshoot) + " settle=" + (str of (settle_step * 0.05)) + "s")