Slowly making progress, I have been kept busy with some stuff lately.
Struggled with timer interrupts for a while because it kept interrupting serial communication, making the robot twitch every 3~5 seconds. (although it was sort of entertaining to watch). I am trying various approaches in terms of programming.
It is slightly disappointing that the servos I have do not provide direct feedbacks from its integrated potentiometers. I have to query their position using a command in software and there is a 50uS to 5ms delay before its position is reported back. Maybe digital ones will have direct position feedback? But they are very costly though.
Still needs a lot of work.