Unity - Advanced Thrust Vectoring Rocket

#include <Arduino.h>
#include "PID.h"

PID::PID(float kp, float ki, float kd)
{
    this->kp = kp;
    this->ki = ki;
    this->kd = kd;
}

double PID::update(double input, double dt, double thrustSet, float momentArm, float setpoint)
{
    this->momentArm = momentArm;
    this->setpoint = setpoint;

    error = input - setpoint;

    integral += error * dt;
    integral = constrain(integral, -6, 6);

    derivative = (error - prevError) / dt;
    prevError = error;

    ut = (kp * error) + (ki * integral) + (kd * derivative);

    output = asin(ut / (momentArm * thrustSet));

    return output * (180 / PI);
}