Great to see what is actually going on…when I see it running it seems that your response-time is way to slow… this can be cause by your control loop running at a wrong rate or the controller’s repose is way to slow. So you need to tune some of the parameters.
Another option could be that the set-point you are trying to reach is way off – the sensor input could be wrong.
In general getting a robot to balance upright is difficult. You are trying to make a highly unstable system – stable. I would say that your approach of trying to copy a Lab-View program that is running with a different sample rate and has a lot of magic numbers is doomed to go wrong. If I where you I would start over and implement a PID control loop so you know what is going on.
But before that I would start by verifying that the sensor input is correct – if this isn’t the case you will never get your robot working. Make a program where you manually move the robot to a upright position and make it fall – record those data with a fixed sample-rate and plot the curve – what are you seeing? Does the sensor input look alright?
Hope to hear from you soon.