close loop mobile robot using 2 dc motor and optical encoder

Using Arduino Programming Questions
1

Please can somebody help me with my program. I'm having difficulty in making 2 dc motor run straightly because they have different speed. i've tried manually lowering the speed of the faster motor but that didnt help at all. I'm using 3-6v dc motors and opro interrupter as optical encoder.

Someone suggested PID to me to resolve the problem but base on my research i think i only need the P. To tell the truth i dont have any slight idea how to implemtnt it on my program any possible help would be appreciated.

thanks in advance

#include <SoftwareSerial.h>
#define RxD 11
#define TxD 12
#define total_slots 20
#define distance_bet_wheels 13.5
#define diameter_of_wheels 6.4
 // threshold is the percentage by which the program corrects the motor
 // but it should not be less than the percent difference of the two motors
 // the bigger the threshold, the faster it corrects but it can make the motors slower or turn drastically
#define threshold 20

 
 int stepper_r;
 int stepper_l;

 SoftwareSerial bt(RxD, TxD);
 int encoder_pin_1 = 3;
 int encoder_pin_2 = 2;
 int motor_left[] = {5, 6};
 int motor_right[] = {9, 10};

 int globalR;
 int globalL;
 int global_percent;
 int btAvailable;
 char last_command = '.';
 int diff;
 String data;
 char received;
 int perL; //used for storing percent value when accesing individual motors
 int left_is_forward; //used when accesing individual motors
 int perR; //used for storing percent value when accesing individual motors
 int right_is_forward; //used when accesing individual motors
 int new_command; //used when accesing individual motors

 void setup()
 {
   Serial.begin(9600);
   clear_globals();
   pinMode(RxD, INPUT);
   pinMode(TxD, OUTPUT);
   new_command = 1;
   btAvailable = 0;
   bt.begin(9600);
   attachInterrupt(digitalPinToInterrupt(encoder_pin_1), stepper_r_increment, RISING);
   attachInterrupt(digitalPinToInterrupt(encoder_pin_2), stepper_l_increment, RISING);
 }


 void loop()
 {

 }

 void clear_globals(){
   globalR = -1;
   globalL = -1;
   global_percent = -1;
   stepper_r = 0;   // counts the slots of encoder disc in the right wheel
   stepper_l = 0;   // counts the slots of encoder disc in the left wheel
   perL = -1;
   perR = -1;
   left_is_forward = -1;
   right_is_forward = -1;
 }

  void clear_globals_without_stepper(){
   globalR = -1;
   globalL = -1;
   global_percent = -1;
 }

 void stepper_r_increment()
 {
   stepper_r++;
 }

 void stepper_l_increment()
 {
   stepper_l++;
 }

 void access_individual_motors(){
    if(left_is_forward){
      analogWrite( motor_left[0], perL * 2.55);
      analogWrite( motor_left[1], 0);
    }
    else{
      analogWrite( motor_left[0], 0);
      analogWrite( motor_left[1], perL * 2.55);
    }
    if(right_is_forward){
      analogWrite( motor_right[0], perR * 2.55);
      analogWrite( motor_right[1], 0);
    }
    else{
      analogWrite( motor_right[0], 0);
      analogWrite( motor_right[1], perR * 2.55);
    }
 }

 void dynamic_turn(int clockwise, int angle ){
    int expected_slots;
    if (angle>=80){
      expected_slots = (total_slots * distance_bet_wheels * (angle-36)) / (360 * diameter_of_wheels);
    }
    else{
      expected_slots = (total_slots * distance_bet_wheels * (angle-10)) / (360 * diameter_of_wheels);
    }
    if(clockwise){
      analogWrite( motor_right[0], 255 );
      analogWrite( motor_right[1], 0 );
      analogWrite( motor_left[0], 0 );
      analogWrite( motor_left[1], 255 );
    }
    else{
      analogWrite( motor_right[0], 0 );
      analogWrite( motor_right[1], 255 );
      analogWrite( motor_left[0], 255 );
      analogWrite( motor_left[1], 0);
    }
    int continueR = 1;
    int continueL = 1;
    while(continueR && continueL && !btAvailable){
        if(expected_slots <= stepper_r){
          analogWrite( motor_right[0], 0 );
          analogWrite( motor_right[1], 0 );
          continueR = 0;
        }
        if(expected_slots <= stepper_l){
          analogWrite( motor_left[0], 0 );
          analogWrite( motor_left[1], 0 );
          continueL = 0;
        }
        if(bt.available())
            btAvailable = 1;
    }
    if(continueR){
        while(continueR && !btAvailable){
            if(expected_slots <= stepper_r){
                analogWrite( motor_right[0], 0 );
                analogWrite( motor_right[1], 0 );
                continueR = 0;
            }
            if(bt.available())
                btAvailable = 1;
        }
    }
    else if(continueL){
        while(continueL && !btAvailable){
            if(expected_slots <= stepper_l){
                analogWrite( motor_left[0], 0 );
                analogWrite( motor_left[1], 0 );
                continueL = 0;
            }
            if(bt.available())
                btAvailable = 1;
        }
    }
 }

 void stop_motors(){
    analogWrite( motor_right[0], 0 );
    analogWrite( motor_right[1], 0 );
    analogWrite( motor_left[0], 0 );
    analogWrite( motor_left[1], 0 );
  }

 void move_forward(){
  int percentR;
  int percentL;
  if(globalR == -1)
    percentR = global_percent;
  else 
    percentR = globalR;
  if(globalL == -1)
    percentL = global_percent;
  else 
    percentL = globalL;
    
  int upper = global_percent;
  int lower = global_percent - threshold;
  if(lower < 0)
    lower = 0;

    access_motors(1, percentR, percentL);
    int diff = stepper_r - stepper_l;
    if(diff > 0){
      if(percentL < upper)
        percentL++;
      else if(percentR > lower)
        percentR--;
    }
    else if(diff < 0){
      if(percentR < upper)
        percentR++;
      else if(percentL > lower)
        percentL--;
    }
    globalR = percentR;
    globalL = percentL;
 } 

void move_backward(){
  int percentR;
  int percentL;
  if(globalR == -1)
    percentR = global_percent;
  else 
    percentR = globalR;
  if(globalL == -1)
    percentL = global_percent;
  else 
    percentL = globalL;
    
  int upper = global_percent;
  int lower = global_percent - threshold;
  if(lower < 0)
    lower = 0;
  
    access_motors(0, percentR, percentL);
    int diff = stepper_r - stepper_l;
    if(diff > 0){
      if(percentL < upper)
        percentL++;
      else if(percentR > lower)
        percentR--;
    }
    else if(diff < 0){
      if(percentR < upper)
        percentR++;
      else if(percentL > lower)
        percentL--;
    }
    globalR = percentR;
    globalL = percentL;
 } 
  
 void access_motors(int forward, int percentR, int percentL){
    if(forward){
      analogWrite( motor_right[0], percentR * 2.55);
      analogWrite( motor_right[1], 0);
      analogWrite( motor_left[0], percentL * 2.55);
      analogWrite( motor_left[1], 0);
    }
    else{
      analogWrite( motor_right[0], 0);
      analogWrite( motor_right[1], percentR * 2.55);
      analogWrite( motor_left[0], 0);
      analogWrite( motor_left[1], percentL * 2.55);
    }
  }

robotf.ino.ino (8.76 KB)

2

What I did when experimenting with 2 wheeled vehicles was to read the encoder on each wheel for a period of time, say 1 second, using interrupts. At the end of the period I compared the 2 readings and adjusted the speed of both motors to speed up the one that had the lowest count and slow down the one with the highest count. It was crude but relatively effective. Obviously the length of the timing period is important as is the size of the speed adjustment made.

3

can you give me some code snippets i dont really know how to do that i'm actually a newbie in arduino..please :slight_smile:

4

The program that you posted has some of what I did in it already. You seem to have removed everything from the loop() function. Put back the code that runs the motors and add code to print the values of stepper_l, stepper_r and the difference between them every second. Change the declaration of these two variable to volatile as they are updated in the ISRs.

You need to look into using millis() for timing as in the BlinkWithoutDelay example and Several things at the same time
Save the time an event happens, such as the start of timing, then each time through loop() check whether the required period has elapsed since the event. If not then go round loop() again reading inputs etc. If the period has elapsed then output the counts as suggested above and save the time again for the next counting period.

Once you have got the counts printing you can consider adding code to adjust the motor speeds.