Adding the heading control P controller to the upper control system

In the control system applied on RPi3 (upper control loop) which sends the set variable to the Tiva boards, a P regulator was implemented to keep the heading constant on the basis of readings of the IMU. The conclusions after the tuning can be seen in the video. There is a bit of drift in the heading even though.

Next step would be to give it a finesse touch and add positioning encoders to have x and y coordinate feedbacl. The loaded hysteresis curve will also need to be looked at,