pidrocket.py

import matplotlib.pyplot as plt

# SPACECRAFT PARAMETERS #
m = 10.0 # kg


# SPACECRAFT PID PARAMETERS #
kp_thrust = 0.8
ki_thrust = 0.1
kp_vel = 0.5
ki_vel = 0.1
integrator_thrust = 0.0
int_min_thrust = 0
int_max_thrust = 250
min_resp_thrust = 0.0
max_resp_thrust = 250.0
integrator_vel = 0.0
int_min_vel = -50
int_max_vel = 50
min_resp_vel = -50
max_resp_vel = 50.0
response_vel = 0.0 # m/s initial
thrust_normaliser = 1.
vel_normaliser = 1.

# TRAJECTORY PARAMETERS #
v = 0.0 # m/s
h = 0.0 # m
h_array = []
v_array = []
t_array = []
target_h = 50.0 # m


# SIMULATION PARAMETERS #
t = 0.0 # s
dt = 0.01 # s
sim_time = 20.0 # s
g = 9.81 # m/s^2


def update_PID(sp, pv, kp, ki, integrator, int_min, int_max, min_resp_thrust, max_resp_thrust, normaliser):
    error = sp - pv
    PID_P = kp * error
    integrator += (error) # maybe * dt ?
    if integrator > int_max:
        integrator = int_max
    elif integrator < int_min:
        integrator = int_min
    PID_I = ki * integrator
    response = PID_P + PID_I
    if response > max_resp_thrust:
        response = max_resp_thrust
    elif response < min_resp_thrust:
        response = min_resp_thrust
    response /= normaliser
    return response, integrator


def trajectory(throttle, v, h0):
    a = ((throttle * max_thrust)/m)-g
    v += a * dt
    h0 += v * dt
    return v, h0


while t <= sim_time:
    response_thrust, integrator_thrust = update_PID(response_vel, v, kp_thrust, ki_thrust, integrator_thrust, int_min_thrust, int_max_thrust, min_resp_thrust, max_resp_thrust, thrust_normaliser)
    v, h = trajectory(response_thrust, v, h)
    response_vel, integrator_vel = update_PID(target_h, h, kp_vel, ki_vel, integrator_vel, int_min_vel, int_max_vel, min_resp_vel, max_resp_vel, vel_normaliser)
    h_array.append(h)
    v_array.append(v)
    t_array.append(t)
    t += dt


plt.plot(t_array, h_array)
plt.plot(t_array, v_array)
plt.show()