Robot body designed in Solidwork
How the robot works
Simscape and System identification in MATLAB was used to find the TF of the robot.
Block diagram of designed PID controller