Always face North behavior

We’ll build a behavior with a heading as a set point for a PID and the IMU Euler heading as feedback. The error value between these will be how far, in degrees, the robot is facing away from the North heading. For example, a heading of 0 should be North – note that you could pick another heading as needed. We will use the PID output to control the motor movements, with the output adding to the speed of one motor and subtracting from the other, producing a turn.

Let’s see how this looks as a block diagram:

Figure 12.10 – Face North behavior block diagram

The preceding diagram shows the flow of data. The expected heading (or target) with the actual heading from the IMU are used to calculate the error. This error and dt (delta time) are the inputs to the PID. The output from the PID, the control signal, is added for one motor and subtracted for the other. The motors then result in robot movement, which...