Consider a tank like robot with a motor driver channel for each side of the robot (two motors on the left and two motors on the right) and an IMU. I'm interested in driving the robot in a straight line using the yaw data from the gyro and magnetometer of the IMU, removing the noise caused by slightly different behaving motors, and the possibility to change the desired direction angle. For example some event comes and I want the car to switch the desired direction to +120 degrees and turn while driving.

I'm using Arduino Uno, MinIMU-9 v3 and two DRV8838 Single Brushed DC Motor Driver from Pololu.

Can you please give me some hints and a short pseudo-code example? Thanks!


4 Answers 4


Assume that your motors can drive both forward and back. (which is not a must, but if the vehicle moves only forward, maneuvering will require a lot of space compared to a two-directional drive)

Assume that you want to manually control speed, by commanding to all motors.

variables: r = gyro yaw rate (can be positive or negative, assume (+) for yaw to left; assume we are interested in +/-10deg per second magnitude) H = heading from magnetometer (0 to 360) H_com = commanded (desired) heading M_com = speed term (motor power, controlled by the user)

M_l = left motor power (can be positive or negative) M_r = right motor power (can be positive or negative)

M_l = M_com + Kr * r + Kh * (H_com - H) M_r = M_com - Kr * r - Kh * (H_com - H)

Kp and Kh are to be found by analysis or trial and error. (H_com-H) is the error term that the controller tries to zeroize, by moving the vehicle(H) to H_com.


  1. you should add some modulus logic, as you wouldn't want to go 300 degrees when you could only go 60 degrees to the desired heading.
  2. at max speeds (max forward, or max rearward), the yaw authority is limited (as motor speeds hit their max), so Kr and Kh may need to be altered to be functions of speed.
  3. for controller tuning strategies, refer to this question.

As you are using the Arduino, you should leverage the most from the community libraries. My advice would be to use the PID library.

There is a detailed explanation about how the library works in that page. It is a link to the creators's blog. Also with explanations about how to tune up the Kp, Ki and Kd.

I have a similar project and it turned out to be too imprecise for us to use only a magnetometer. The worst scenario for a robot is to be next to a source of interference ( like a TV or a big metallic structure ) and then move several meters based on the decision taken when turning.

To solve these issues, we used a gyroscope ( L3G4200D in our case ). This is the code that we ended up using:

  unsigned long amount_off_target ;
  L3G4200D gyroscope;
  while (!gyroscope.begin(L3G4200D_SCALE_250DPS, L3G4200D_DATARATE_400HZ_50))  // Set scale 250 dps and 400HZ Output data rate (cut-off 50)
    Serial.println("Could not find a valid L3G4200D sensor, check wiring!");

  // here it goes some code referred to our encoders

  double Setpoint = 0, Input = 0, Output = 0;
  const double scale_factor = 7000000.0f;
  PID myPID(&Input, &Output, &Setpoint, 9.5 / scale_factor, 4 / scale_factor, 3.5 / scale_factor, DIRECT);


  myPID.SetOutputLimits(-50, 50);   // these parameters are related to your particular motor PWM values
  int min_power = MN_POWER+35;

  long z_total = 0;
  Input = 0;
  const long target_z = long(float(degs) * (90000000.0f / 90.0f));
  unsigned long last_off_target = millis();
  unsigned long last_read_sensor = micros();
  const unsigned long started_turn = millis();

  Setpoint = target_z;
  // Grace period to start moving
  long last_not_moving = millis() + 50;

  int direction;

  while ((millis() - last_off_target) < 100L && (millis() - started_turn) < 2000L)
   // more code about our encoders goes here
    const long now_millis = millis();
    const long bump_min_speed_frequency = 10;

    amount_off_target = abs(z_total - target_z);
    if (amount_off_target > 2506516L) {   // here you decide that you are on target
      last_off_target = millis();

    const unsigned long read_sensor_time = micros();
    Vector norm = gyroscope.readNormalize();
    const long time_since_read = (read_sensor_time - last_read_sensor);
    last_read_sensor = read_sensor_time;

    long yaw = (long)norm.ZAxis;
    z_total += yaw * time_since_read;

    Input = double(z_total);

    if (Output > 0) {
      direction = 1;
    else {
      direction = -1;

    int mtr_pwr = min_power + abs(Output);

      motorRight( mtr_pwr, -direction);
      motorLeft( mtr_pwr, direction);


I hope that this works as a general principle of implementation. The main code structure can be easily understood if you look at this PDF explaining it.

The code was tuned up by a real rockstar coder that worked with our team and it works surprisingly well in many diverse surfaces. You might notice that it overshoots a bit when it's on glass and undershoots when it's on a thick rug.


You define the powers for motors as: p_left = p_average + p_differential and p_right = p_average - p_differential

Your PID regulator should take the filtered angle as input and provide you with p_differential. As such, it will control the "turning" of the robot. When the mentioned event occurs, just change the referential angle.


Do you have, or can you add encoders to the motors? If you do, I would advise to try to use them as a first step. Encoders allow you to control speed of the motors in terms of their actual rotation speed not PWM. If robot still doesn't drive straight, you can try to apply UMBmark method to correct it. For me it yields very satisfying results, although I have not tried it with tank-link platforms.

If encoders are not the option, you can try IMU. Remember, that it is also subject to errors, so filtering its readings will be crucial here. You can get heading either reading magnetometer directly, or by integrating gyro readings. To use both, you should use Kalman Filter - some reading here, or here.

Then, just insert heading calculated by KF to regular PID controller, so that the bigger is the error between desired and actual heading, the bigger is speed difference between left and right wheels.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.