On algorithm, you have two ways to do it. Either in direct kinematics or in inverse kinematics.
The first is easier to implement based on PID but will not allow you to make it move forward correctly.
The IK is more complex at the beginning but I advise you to start directly on that. You have to understand the concepts of plane change and matrices. I advise you to look for courses on the subject. This is the direction I will go in myself.
i make same spot micro project i need the program for self balancing quadraped I wish you help me