Added graphs, tested. NOTE: if setpoint is below starting point, system breaks