Output is 0 when above the setpoint, but works as intended when below it.

[Zachary Elkins]

I'm trying to create a system that balances on 2 wheels. When it is tipped forward (input > setpoint) the output is always 0, despite the backwards motion correcting well.

Link to the code: https://www.codedump.xyz/cpp/Yx9jjI9rDx4HJrTL