Biped Coding Issues

So use the table to store what the servo angles would be on the flat, but before writing them to the servos, adjust them according to the incline.

Or don't use table lookup at all, but instead calculate the servo motion required, knowing where you are in the cycle, the current servo angles, and the incline.