Just finsted the main controller fot motors using multtreading, but need to set setpoints and write equations for power to the motors