The first step in the design of a controller for a robot manipulator is to solve for its kinematics, inverse kinematics, dynamics, and the feedback control equation that will be used. Also the type of input and the user interface should be determined at this stage.
For trajectory generation, we used the cubic polynomials method which generates a cubic function that describes the motion from a starting point to a goal point in a certain time. The error in position and velocity is calculated using the readings from the sensors. Our control module simulates a PD controller to minimize that error. The error depends on several factors such as: frequency of update, frequency of sensors reading, and the desired trajectory (for example, if we want to move with a big angle in a very small time interval, the error will be large).