My task is to develop a controller (I decided to do a PID) in Python.

This is the model I’ve been given: We consider a body in water with mainly two forces acting on it: gravity and buoyancy (friction is neglected here). The body-depth is controlled by ballast tanks within its hull, e.g. it changes mass to accelerate up or down. More precisely: The speed of flooding the ballast tanks is what I can control.

This is what I did so far:

import math
import matplotlib.pyplot as plt


WATER_DENSITY = 997  # [kg/m³]
GRAVITATIONAL_ACCELERATION = 9.81  # [m/s²]
BODY_VOLUME = 0.00238  # [m³]
BODY_VOLUME_GAGE = 0.00005  # [m³]

BODY_MASS = BODY_VOLUME * WATER_DENSITY  # [kg]
BODY_MASS_MAX = BODY_MASS + BODY_VOLUME_GAGE * WATER_DENSITY  # [kg]
BODY_MASS_MIN = BODY_MASS - BODY_VOLUME_GAGE * WATER_DENSITY  # [kg]

SIMULATION_TIME = 90  # [s]
SIMULATION_STEP = 0.001  # [s]
R0 = 0.0  # [m]
CONTROLLER_TARGET = -1  # [m]

KP = 2
KI = 0
KD = 0


def system_response(state, control, time_step):
    current_mass = state['m'] - control * time_step  # [kg]
    current_mass = max(BODY_MASS_MIN, current_mass)  # limit the current mass to min / max values
    current_mass = min(BODY_MASS_MAX, current_mass)
    f_gravity = current_mass * GRAVITATIONAL_ACCELERATION  # [N]
    f_buoyancy = WATER_DENSITY * BODY_VOLUME * GRAVITATIONAL_ACCELERATION  # [N]
    f_total = f_buoyancy - f_gravity  # [N]
    acceleration = f_total / current_mass  # [m/s²]
    velocity = state['v'] + time_step * acceleration  # [m/s]
    position = min(0, state['r'] + time_step * velocity)  # [m]
    return {
        'm': current_mass,
        'a': acceleration,
        'v': velocity,
        'r': position
    }


print("Running simulation...")
t = [0.0]  # [s]
m = [BODY_MASS]  # [kg]
a = [0.0]  # [m/s²]
v = [0.0]  # [m/s]
r = [R0]  # [m]
e = [CONTROLLER_TARGET - R0]
ie = 0.0
u = 0.0  # [kg/s]
i = 0
while True:
    new_state = system_response({
        'm': m[-1],
        'a': a[-1],
        'v': v[-1],
        'r': r[-1]
    }, u, SIMULATION_STEP)
    t.append(t[-1] + SIMULATION_STEP)
    m.append(new_state['m'])
    a.append(new_state['a'])
    v.append(new_state['v'])
    r.append(new_state['r'])
    e.append(CONTROLLER_TARGET - new_state['r'])
    de = e[-1] - e[-2]
    dt = SIMULATION_STEP
    ie += e[-1] * dt
    u = KP * e[-1] + KI * ie + KD * de / dt
    i += 1
    if t[-1] > SIMULATION_TIME:
        break

print("Simulation stopped after {0} steps".format(i))
plt.title('Simulation Results')
plt.plot(t, r, label='Position [m]')
plt.plot(t, [CONTROLLER_TARGET for j in range(len(t))], label='Target [m]')
plt.xlabel('Time [s]')
plt.legend()
plt.show()
plt.close()

showing me an oscillating output, no matter what I choose KP to be. I tried to do the Ziegler-Nichols method, but this seems undoable since I cant get a stable response. When I choose KD big, it shows a more stable response.

How do I proceed from here as a beginner in control loops?