Algorithm 1 Non-Linear Controller Execution

Result: Torque signals to be applied by GYEMS motors Each loop is timed per interruption at 10 ms while do     if “writing” is equal to 0 then         Generate position and velocity profiles         Compute control law and obtain torques in N/m         Scale the torque signals from N/m to Amp         Transform torques to communication CAN BUS signal         Set “writing” to 1     else         Request position and speed information by CAN BUS         Perform scaling of position and velocity signals         Apply filter to the speed reference         Set “writing” to 0     end if      end while