Autonomous straight line Robot

Other Hardware Robotics
1

Hello,

I'm building an autonomous robot that can go in a straight line on a synthetic floor for a determined distance, in this case 30m. This is what I'm using:

  • 2 magnetic encoders, pololu 12 CPR, #1523
  • 2 dc motors 6v, 120:1, pololu mini plastic gearmotor, #1516
  • 1 Arduino Yùn
  • 2 60mm diameter wheels

And here is the code:

const int drive_distance = 3000;   
const int motor_power = 220;      
const int motor_offset = 5;       
const int wheel_d = 60;          
const float wheel_c = PI * wheel_d; 
const int counts_per_rev = 1444;   


const int enc_l_pin = 2;          
const int enc_r_pin = 3;          

const int ain1_pin = 11;
const int ain2_pin = 10;

const int bin1_pin = 5;
const int bin2_pin = 6;



volatile unsigned long enc_l = 0;
volatile unsigned long enc_r = 0;

void setup() {

  
  Serial.begin(115200);

  
  pinMode(enc_l_pin, INPUT_PULLUP);
  pinMode(enc_r_pin, INPUT_PULLUP);

  pinMode(ain1_pin, OUTPUT);
  pinMode(ain2_pin, OUTPUT);
  
  pinMode(bin1_pin, OUTPUT);
  pinMode(bin2_pin, OUTPUT);
 
  
  attachInterrupt(digitalPinToInterrupt(enc_l_pin), countLeft, CHANGE);
  attachInterrupt(digitalPinToInterrupt(enc_r_pin), countRight, CHANGE);

  
  delay(1000);
  
  driveStraight(drive_distance, motor_power);
}

void loop() {
  // Do nothing
}

void driveStraight(float dist, int power) {

  unsigned long num_ticks_l;
  unsigned long num_ticks_r;

  
  int power_l = motor_power;
  int power_r = motor_power;

  
  unsigned long diff_l;
  unsigned long diff_r;

  
  enc_l = 0;
  enc_r = 0;

  
  unsigned long enc_l_prev = enc_l;
  unsigned long enc_r_prev = enc_r;

  
  float num_rev = (dist * 10) / wheel_c;  
  unsigned long target_count = num_rev * counts_per_rev;
  
  
  Serial.print("Driving for ");
  Serial.print(dist);
  Serial.print(" cm (");
  Serial.print(target_count);
  Serial.print(" ticks) at ");
  Serial.print(power);
  Serial.println(" motor power");

 
  while ( (enc_l < target_count) && (enc_r < target_count) ) {

    
    num_ticks_l = enc_l;
    num_ticks_r = enc_r;

    
    Serial.print(num_ticks_l);
    Serial.print("\t");
    Serial.println(num_ticks_r);

    
    drive(power_l, power_r);

    
    diff_l = num_ticks_l - enc_l_prev;
    diff_r = num_ticks_r - enc_r_prev;

    
    enc_l_prev = num_ticks_l;
    enc_r_prev = num_ticks_r;

   
    if ( diff_l > diff_r ) {
      power_l -= motor_offset;
      power_r += motor_offset;
    }

    
    if ( diff_l < diff_r ) {
      power_l += motor_offset;
      power_r -= motor_offset;
    }

    
    delay(20);
  }

  
  brake();
}



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 ) {
    digitalWrite(ain1_pin, LOW);
    digitalWrite(ain2_pin, HIGH);
  } else {
    digitalWrite(ain1_pin, HIGH);
    digitalWrite(ain2_pin, LOW);
  }

  // Right motor direction
  if ( power_b < 0 ) {
    digitalWrite(bin1_pin, LOW);
    digitalWrite(bin2_pin, HIGH);
  } else {
    digitalWrite(bin1_pin, HIGH);
    digitalWrite(bin2_pin, LOW);
  }

  // Set speed
 
}

void brake() {
  digitalWrite(ain1_pin, LOW);
  digitalWrite(ain2_pin, LOW);
  digitalWrite(bin1_pin, LOW);
  digitalWrite(bin2_pin, LOW);
  
  
}

void countLeft() {
  enc_l++;
}

void countRight() {
  enc_r++;
}

I have a problem, it still doesn't go straight. I guess it's a problem in the code but I don't know what. Can anyone help me?

2

@dzepedanovoa, I see that you are a new-ish member.
Do yourself a favor and go to the top of the forum and read the post titled 'How to use this forum - please read'

It covers much that will help you including how to post code.

3

vinceherman:
@dzepedanovoa, I see that you are a new-ish member.
Do yourself a favor and go to the top of the forum and read the post titled 'How to use this forum - please read'

It covers much that will help you including how to post code.

Sorry, I fixed it now.

4

Good, that makes it easier for us to look at your code and easier to load into an editor.

Now, tell us more about the actual results. What does it actually do and how does that differ from your expectations?

5

Agree, need a better description of what happens.

After glancing through the code, it appears that the general approach might work. However, how did you arrive at magic numbers like this?

const int motor_offset = 5;

Note that when you make a copy of multi-byte variables that can be modified by an interrupt routine, the interrupts MUST be turned off while making the copy, or the copy will be at times corrupted.

    num_ticks_l = enc_l;
    num_ticks_r = enc_r;
6

vinceherman:
Good, that makes it easier for us to look at your code and easier to load into an editor.

Now, tell us more about the actual results. What does it actually do and how does that differ from your expectations?

I need my robot to go in a really straight line but in reality it still turns to the right.

7

Put in Serial.print() statements to see if and how power_l and power_r are changing:

    drive(power_l, power_r);
8

As a test, I would also power up the vehicle with the wheels off the ground.
While they are rolling, restrict the movement of one wheel.

Does the power to the opposite wheel decrease? Does the power to the restricted wheel increase?
During this restricted movement, do the wheels turn the same number of revolutions?

Have you investigated using PID? (google it)

9

Hi,
No two motors are made alike, so if you expect both motors to turn at the same speed for the same control voltage, then as you have found it is not possible.

The solution is to count pulses on BOTH wheels, so when going straight the number of pulses from the left motor equals the number of pulses from the right.

You will need to write code that uses say the left motor pulses as the reference and you adjust the speed of the right motor to keep its pulse count equal to the left.

Tom.... :slight_smile: