trying to combine encoders and compass to move the car straight

Projects Programming
1

Hi all,

These are the two seperate functions that I want to combine so that my car can travell as straight as possible. I've tried to combine them together but haven't succeeded yet.

Code for the encoders:

void driveStraight(float drive_distance, int motor_power)
{
  Serial.print("\n");
  Serial.println("num_tick_l \t num_tick_r \t diff_l \t diff_r \t enc_1_prev \t enc_r_prev \t Power_l \t Power_r \t Power_a \t Power_b ");
 
  unsigned long num_ticks_l;
  unsigned long num_ticks_r;

  // Set initial motor power
  int power_l = motor_power;
  int power_r = motor_power;

  // Used to determine which way to turn to adjust
  unsigned long diff_l;
  unsigned long diff_r;

  // Reset encoder counts
  enc_l = 0;
  enc_r = 0;

  // Remember previous encoder counts
  unsigned long enc_l_prev = enc_l;
  unsigned long enc_r_prev = enc_r;

  // Calculate targer number of ticks
  float num_rev = (drive_distance * 10) / wheel_c;   // Convert to mm
  unsigned long target_count =  29; //num_rev * counts_per_rev;


  /*
    // Debug
    Serial.print("Driving for ");
    Serial.print(drive_distance);
    Serial.print(" cm(");
    Serial.print(target_count);
    Serial.print(" ticks) at ");
    Serial.print(motor_power);
    Serial.println(" Motor Power");
  */

  //Drive until on of the encoders reaches desired count
  while ((enc_l < target_count) && (enc_r < target_count))
  {

    bluetooth();                                // if new bluetooth value received break from loop
    if (blueToothVal == 5) {
      break;                                    // If a 'Stop' Bluetooth command is received then break from the Loop
    }

    // Sample number of encoder ticks
    num_ticks_l = enc_l;
    num_ticks_r = enc_r;

    // Sample out current number of ticks
    Serial.print(num_ticks_l);
    Serial.print("  \t\t");
    Serial.print(num_ticks_r);

    // Drive
    drive(power_l, power_r);

    // Number of ticks counted since last time
    const int StraightDriveOffset = 1;                    // Increase this or even make it a negatie number to tweak the

    diff_l = num_ticks_l - enc_l_prev;                    // + StraightDriveOffset;
    diff_r = num_ticks_r - enc_r_prev;


    Serial.print("  \t\t");
    Serial.print(diff_l);
    Serial.print(" \t \t");
    Serial.print(diff_r);


    // Store current ticks counter for next time
    enc_l_prev = num_ticks_l;
    enc_r_prev = num_ticks_r;


    Serial.print("   \t \t");
    Serial.print(enc_l_prev);
    Serial.print(" \t \t");
    Serial.print(enc_r_prev);


    // If left is faster, slow it down and speed up right
    if ( diff_l > diff_r) {
      power_l -= motor_offset;
      power_r += motor_offset;
    }

    // If right is faster, slow it down and speed up right
    if ( diff_l < diff_r) {
      power_l += motor_offset;
      power_r -= motor_offset;
    }

    // Debugging
    Serial.print(" \t\t");
    Serial.print(power_l);
    Serial.print(" \t\t");
    Serial.println(power_r);


    // Brif pause to let motors respond
    delay(20);
  }

  StopCar();
  //Buzzer();
}



void drive(int power_a, int power_b)
{
  // Constrain power to between -255 and 255
  power_a = constrain(power_a, -255, 255);
  power_b = constrain(power_b, -255, 255);

  // Left motor direction
  if ( power_a < 0)
  {
    motor2.setSpeed(abs(power_a));       // Motor is the front left wheel
    motor2.run(BACKWARD);

    motor1.setSpeed(power_a);
    motor1.run(BACKWARD);
  }
  else
  {
    motor2.setSpeed(abs(power_a));       // Motor is the front left wheel
    motor2.run(FORWARD);

    motor1.setSpeed(power_a);
    motor1.run(FORWARD);
  }

  // Right Motor direction
  if ( power_b < 0)
  {
    motor4.setSpeed(abs(power_b));       // Motor is the front left wheel
    motor4.run(BACKWARD);

    motor3.setSpeed(power_b);
    motor3.run(BACKWARD);
  }
  else
  {
    motor4.setSpeed(abs(power_b));       // Motor is the front left wheel
    motor4.run(FORWARD);

    motor3.setSpeed(power_b);
    motor3.run(FORWARD);
  }

}

void countLeft()
{
  enc_l ++;
}

void countRight()
{
  enc_r ++;
}

Code for the Compass:

int compass_dev = 5;                                               // the amount of deviation that is allowed in the compass heading - Adjust as Needed

int Camp_Speed = 175;

  
void Compass_Forward()
{
  while (blueToothVal == 8)                                           // Go forward until Bluetooth 'Stop' command is sent

  {
    getCompass();                                                     // Update Compass Heading
    bluetooth();                                                      // Check to see if any Bluetooth commands have been sent
    if (blueToothVal == 5) {
      break; // If a Stop Bluetooth command ('5') is received then break from the Loop
    }

    if ( abs(desired_heading - compass_heading) <= compass_dev )      // If the Desired Heading and the Compass Heading are within the compass deviation, X degrees of each other then Go Forward
      // otherwise find the shortest turn radius and turn left or right
    {
      Forward();
    }

    else
    {
      int x = (desired_heading - 359);                          // x = the GPS desired heading - 360
      int y = (compass_heading - (x));                          // y = the Compass heading - x
      int z = (y - 360);                                        // z = y - 360

      if ((z <= 180) && (z >= 0))                               // if z is less than 180 and not a negative value then turn left
      { // otherwise turn right
        SlowLeftTurn();

      }
      else
      {
        SlowRightTurn();

      }
    }


  }
}                                                                    // End Compass_Forward


void SlowLeftTurn()
{
 motor1.setSpeed(150);
 // motor2.setSpeed(150);
  motor3.setSpeed(Camp_Speed);
 // motor4.setSpeed(Camp_Speed);

  motor1.run(BACKWARD);
  //motor2.run(BACKWARD);
  motor3.run(FORWARD);
  //motor4.run(FORWARD);
}

void SlowRightTurn()
{
  motor1.setSpeed(Camp_Speed);
 // motor2.setSpeed(Camp_Speed);
  motor3.setSpeed(Camp_Speed);
  //motor4.setSpeed(Camp_Speed);

  motor1.run(FORWARD);
  //motor2.run(FORWARD);
  motor3.run(BACKWARD);
 // motor4.run(BACKWARD);
}

And this is my attemp at combining them together.

void drive(int power_a, int power_b)
{
  getCompass();                                                     // Update Compass Heading
  bluetooth();                                                      // Check to see if any Bluetooth commands have been sent

  if (blueToothVal == 5) {
    break; // If a Stop Bluetooth command ('5') is received then break from the Loop
  }

  // Constrain power to between -255 and 255
  power_a = constrain(power_a, -255, 255);
  power_b = constrain(power_b, -255, 255);

  if ( abs(desired_heading - compass_heading) <= compass_dev )      // If the Desired Heading and the Compass Heading are within the compass deviation, X degrees of each other then Go Forward
    // otherwise find the shortest turn radius and turn left or right
  {
    // Left motor direction
    if ( power_a < 0)
    {
      motor2.setSpeed(abs(power_a));       // Motor is the front left wheel
      motor2.run(BACKWARD);

      motor1.setSpeed(power_a);
      motor1.run(BACKWARD);
    }
    else
    {
      motor2.setSpeed(abs(power_a));       // Motor is the front left wheel
      motor2.run(FORWARD);

      motor1.setSpeed(power_a);
      motor1.run(FORWARD);
    }

    // Right Motor direction
    if ( power_b < 0)
    {
      motor4.setSpeed(abs(power_b));       // Motor is the front left wheel
      motor4.run(BACKWARD);

      motor3.setSpeed(power_b);
      motor3.run(BACKWARD);
    }
    else
    {
      motor4.setSpeed(abs(power_b));       // Motor is the front left wheel
      motor4.run(FORWARD);

      motor3.setSpeed(power_b);
      motor3.run(FORWARD);
    }

  }

  else
  {
    int x = (desired_heading - 359);                          // x = the GPS desired heading - 360
    int y = (compass_heading - (x));                          // y = the Compass heading - x
    int z = (y - 360);                                        // z = y - 360

    if ((z <= 180) && (z >= 0))                               // if z is less than 180 and not a negative value then turn left
    { // otherwise turn right
      SlowLeftTurn();

    }
    else
    {
      SlowRightTurn();

    }
  }
}
2

So what is the problem? What does the code actually do? How does that deffer from what you want the code to do? Are there errors? Just telling us that it doessn't work is not enough information for us to be able to help.

3

How is this different than your post here: Car doesnt move straight using rotary encoders?

4

My car has four wheels. I've attached two encoders in the front wheels and I'm using the back two wheels to adjust the car if it deviates, by slowly turning it left or right. The problem is that when the program is executing the "SlowLeftTurn' or "SlowRightTurn". This somehow increments the "num_ticks_l" and "num_ticks_r" even when I'm holding the front two wheels.

Gfvalvo, Yes, that post was when I was testing my car on flat floor. But, now I want make the car move straight on grass by using the compass as a reference.

5

How are the motors powered? How is the mystery Arduino powered? It may be that electrical noise from the motors is causing the extra counts. A schematic would help.

6

This is the schematic.

Breadboard Diagram.pdf (1.66 MB)

7

What is the part number of the Hall sensors? Data sheet? Do they need pullups? How far are the Hall sensors from the processor?

8

I am using the UGN3503 hall effect sensor. The datasheet for this is at:

https://www.jaycar.com.au/medias/sys_master/images/9168322953246/ZD1902-dataSheetMain.pdf.

The hall sensors aren't too far from the processor. You can see the pictures of it in the attached files.

51165104_1051640815019396_5258433128845803520_n.jpg
51165104_1051640815019396_5258433128845803520_n.jpg540×960 86.5 KB

51318441_1775251245914987_5262081076498530304_n.jpg
51318441_1775251245914987_5262081076498530304_n.jpg960×540 93.6 KB