Go Down

Topic: Position control with DC motor (Read 4813 times) previous topic - next topic

SimonR

Hello everybody,

I bought a 12V brushed DC motor with a 18.75:1 metal gearbox and an integrated quadrature encoder that provides a resolution of 64 counts per revolution of the motor.

In my application I want to control the position of the motor. Most of time it will not turn more than 360° degrees.

I tried to read the value from the two encoder outputs by using the Interrupt example uses both interrupt pins, but I'm not able to have a correct value (one tour = 64 or 64x19).

I check on the scope the outputs and they are fine.

This is the code I used :

Code: [Select]
// MD03A_Motor_basic + encoder

#define InA1            10                      // INA motor pin
#define InB1            11                      // INB motor pin
#define PWM1            3                       // PWM motor pin
#define encoder0PinA      5                       // encoder A pin
#define encoder0PinB      6                       // encoder B pin

#define LOOPTIME        100                     // PID loop time
#define FORWARD         1                       // direction of rotation
#define BACKWARD        2                       // direction of rotation

unsigned long lastMilli = 0;                    // loop timing
unsigned long lastMilliPrint = 0;               // loop timing
long count = 0;                                 // rotation counter
long countInit;
long tickNumber = 0;
boolean run = false;                                     // motor moves

volatile unsigned int encoder0Pos = 0;

void setup()
{
  
 Serial.begin (9600);
 pinMode(InA1, OUTPUT);
 pinMode(InB1, OUTPUT);
 pinMode(PWM1, OUTPUT);
 
 pinMode(encoder0PinA, INPUT);
 pinMode(encoder0PinB, INPUT);
 
 digitalWrite(encoder0PinA, HIGH);                      // turn on pullup resistor
 digitalWrite(encoder0PinB, HIGH);
 
// attachInterrupt(1, rencoder, FALLING);


  attachInterrupt(0, doEncoderA, CHANGE);
//  attachInterrupt(1, doEncoderB, CHANGE);  
}

void loop() {
// moveMotor(FORWARD, 100, 464*2);                        // direction, PWM, ticks number
 moveMotor(FORWARD, 100, 2000);
 delay(3000);
// moveMotor(BACKWARD, 100, 464*2);                           // 464=360°
 moveMotor(BACKWARD, 100, 2000);
 delay(3000);
}

void moveMotor(int direction, int PWM_val, long tick)  
{
 // countInit = count;    // abs(count)
 countInit = encoder0Pos;    // abs(count)
 
 tickNumber = tick;
 if(direction==FORWARD)          motorForward(PWM_val);
 else if(direction==BACKWARD)    motorBackward(PWM_val);
}


void motorForward(int PWM_val)  {
 analogWrite(PWM1, PWM_val);
 digitalWrite(InA1, LOW);
 digitalWrite(InB1, HIGH);
 run = true;
}

void motorBackward(int PWM_val)  {
 analogWrite(PWM1, PWM_val);
 digitalWrite(InA1, HIGH);
 digitalWrite(InB1, LOW);
 run = true;
}

void motorBrake()  {
 analogWrite(PWM1, 0);
 digitalWrite(InA1, HIGH);
 digitalWrite(InB1, HIGH);
 run = false;
}
void doEncoderA()
{

  // look for a low-to-high on channel A
  if (digitalRead(encoder0PinA) == HIGH) {

    // check channel B to see which way encoder is turning
    if (digitalRead(encoder0PinB) == LOW) {  
      encoder0Pos = encoder0Pos + 1;         // CW
    }
    else {
      encoder0Pos = encoder0Pos - 1;         // CCW
    }
  }

  else   // must be a high-to-low edge on channel A                                      
  {
    // check channel B to see which way encoder is turning  
    if (digitalRead(encoder0PinB) == HIGH) {  
      encoder0Pos = encoder0Pos + 1;          // CW
    }
    else {
      encoder0Pos = encoder0Pos - 1;          // CCW
    }
  }
  Serial.println (encoder0Pos, DEC);          
  // use for debugging - remember to comment out

}

void doEncoderB()
{

  // look for a low-to-high on channel B
  if (digitalRead(encoder0PinB) == HIGH) {  

   // check channel A to see which way encoder is turning
    if (digitalRead(encoder0PinA) == HIGH) {  
      encoder0Pos = encoder0Pos + 1;         // CW
    }
    else {
      encoder0Pos = encoder0Pos - 1;         // CCW
    }
  }

  // Look for a high-to-low on channel B

  else {
    // check channel B to see which way encoder is turning  
    if (digitalRead(encoder0PinA) == LOW) {  
      encoder0Pos = encoder0Pos + 1;          // CW
    }
    else {
      encoder0Pos = encoder0Pos - 1;          // CCW
    }
  }

}


The motor turns weel and changes side, but however the number of tick I want he will always do 2,5 turn.





I have for objective to do an application like that :

360°degrees of amplitude. When the analog input is 0V motor doesn't move. When the input is 2,5V, the motor goes to 180° degree, etc... And it moves through 0 to 360° according to the analog input voltage.

Thanks for your help. (Sorry if there are grammatical english mistakes, I come from France ;) )

Regards

Simon

MarkT

Is the encoder analog output (bare phototransistors) or logic outputs?  If analog you will need
schmitt-triggers to clean up the signals.

Here's the way I code this sort of thing:
Code: [Select]

void setup ()
{
  attachInterrupt (0, encoder_fn, CHANGE) ;
  attachInterrupt (1, encoder_fn, CHANGE) ;
}

volatile long encoder_pos = 0L ;
volatile long encoder_errors = 0L ;
volatile byte prev_pins = 0 ;
void encoder_fn ()
{
  byte pins = (PIND & 0x0C) >> 2 ; // read both pins, yield 00, 01, 11, 10 in sequence
  if (pins & 2)
    pins ^= 1 ;  // convert to 00 01 10 11 sequence
  byte diff = 3 & (pins - prev_pins) ; // compute difference from last time (should be 01 or 11)
  prev_pins = pins ; // store for next time
  if (diff == 1)
    encoder_pos += 1 ;
  else if (diff == 3)
    encoder_pos -= 1 ;
  else if (diff == 0)
    ;
  else
    encoder_errors ++ ;
}


Using direct port manipulation means reading both pins simultaneously and its faster,
converting the 00, 01, 11, 10 sequence to 0,1,2,3 simplifies direction detection, and you
can detect unexpected 00->11 or 11->00 transitions (perhaps your encoder spinning too
fast?)

You do need to sample both pins in an ISR with CHANGE to catch all the transitions
(which matters if the motor changes direction or stops).

BTW you are calling Serial.print inside an ISR, which can jam up the system - don't expect
that to work.
[ I DO NOT respond to personal messages, I WILL delete them unread, use the forum please ]

SimonR

#2
Jul 23, 2015, 10:06 am Last Edit: Jul 23, 2015, 10:07 am by SimonR
Encoder signals are logic outputs. See attachment image.

I tried your example interrupt, and the result in the same as previously.

I want to see the value of the encoder_pos and the return values are like that :
"3 / 3 / 3 / 4 / 4 / 3 / 3 / 4 / 4 / 4 / 4 / 4 / 5 / ...." and sometimes only " 0 / 1 / 1 / 0 / 0 / -1 / 0 ..."

I have no idea where the problems come from.


TomGeorge

Hi, I presume you have a x10 probe connected to get that display.
2V per Div and not 200mV,   need to change SONDE to x10.


I was going to suggest a pull up resistor of each output, as most encoders are open collector outputs so any logic level voltage can used, but as you say it is logic level output.

Can you post a picture of the motor assembly, and or a part number/spec link?

Thanks...Tom...... :)
Everything runs on smoke, let the smoke out, it stops running....

MarkT

Encoder signals are logic outputs. See attachment image.

I tried your example interrupt, and the result in the same as previously.

I want to see the value of the encoder_pos and the return values are like that :
"3 / 3 / 3 / 4 / 4 / 3 / 3 / 4 / 4 / 4 / 4 / 4 / 5 / ...." and sometimes only " 0 / 1 / 1 / 0 / 0 / -1 / 0 ..."

I have no idea where the problems come from.


Yes, you will get oscillation like that, its quite normal when the encoder is on the boundary
between states.  The point is to do the accounting correctly so there is never drift.
[ I DO NOT respond to personal messages, I WILL delete them unread, use the forum please ]

SimonR

HEllo Tom and Mark,

Yeah I forgot to put the x10 factor to the scope, so the signals are 0 to 5V logic.

The motor I use is a Pololu : https://www.pololu.com/product/1442 : 19:1 Metal Gearmotor 37Dx52L mm with 64 CPR Encoder



When I have this return value from the encoder_pos, the motor rotates clokwise slowly. The  direction never changes.

Is that possible to increment the encoder position to 64, like that we know we do 360° rotation.

Regards !

MarkT

Have you checked both pins 2 and 3 actually work on your Arduino?
[ I DO NOT respond to personal messages, I WILL delete them unread, use the forum please ]

SimonR

Actually I mapped it on pins 5 and 6. They worked well. I also tried with other pins (2 and 4) and the problems are still there.

Example of the return value for encoder when the motor rotates

" 197 / 197 / 200 / 199 / 195 / 198 / 198 / 198 / 195 / 199 / 199 / 198 / 202 / 198 / 199 / 200 / 196 / 197 / 197 / 200 / 195 / 197 / 196 / 196 / 196 / 193 / 197 / 197 / 196 / 196 / 196 / 197 / 194 / 194 / 191 / 191 / 191 / 193 / 193 / 189 / 192 / 188 / 189 / 189 / 185 / 188 / 188 / 188 / 189 / 186 / 190 / 187 / 187 / 187 / 189 / 189 / 189 / 188 / 188 / 189 / 189 / 189 / 188 / 186 / 187 / 187 / 188 / 185 / 185 / 185 / 188 / 183 / 183 / 183 / 182 / 182 / 183 / 183 / 179 / 182 / 178 / 178 / 179 / 176 / 180 / 177 / 177 / 177 / 179 / 179 / 175 / 178 / 178 / 178 / 175 / 171 / 175 / 174 / 174 / 175 / 174 / 174 / 171 / 171 / 171 / 173 / 177 / 177 / 176 / 172 / 172 / 173 / 169 / 172 / 172 / 172 / 169 / 170 / 174 / 171 / 171 / 174 / 173 / 173 / 173 / 172 / 172 / 173 / 173 / 173 / 173 / 172 / 172 / 169 / 174 / 174 / 171 / 171 / 167 / 169 / 169 / 165 / 168 "

The interrupt detects the transitions but the incrementation is not good.

SimonR

I tried to do it without interrupt. See the code below

Code: [Select]
int val;
 
 int pinInput1 = 11;    // Commande de sens moteur, Input 1
 int pinInput2 = 10;    // Commande de sens moteur, Input 2
 
 int encoder0PinA = 5;
 int encoder0PinB = 6;
 
 int enable_L293 = 3;
 
 int encoder0Pos = 0;
 
 int encoder0PinALast = LOW;

 
// Compteur de tours : 1 tours = 64 ticks roue codeuse
 int nb_tours = 0;
 int nb_tours_1 = 0;
 
 int n = LOW;

 void setup()
 {
   
   
   pinMode (encoder0PinA,INPUT);
   pinMode (encoder0PinB,INPUT);
   
   pinMode (pinInput1,OUTPUT);
   pinMode (pinInput2,OUTPUT);
   
   pinMode (enable_L293,OUTPUT);
   
   digitalWrite( pinInput1, LOW );
   digitalWrite( pinInput2, HIGH );
   
   
   Serial.begin (9600);
   Serial.print ("Nombres de tours init");
   Serial.print (nb_tours);
   Serial.print (" \n ");
 }

 void loop()
 {
 
   digitalWrite( enable_L293, HIGH );
   
   n = digitalRead(encoder0PinA);
   


  if ((encoder0PinALast == LOW) && (n == HIGH))
   {
     if (digitalRead(encoder0PinB) == LOW)
     {
       encoder0Pos--;
     }
     else
     {
       encoder0Pos++;
     }
   }
   encoder0PinALast = n;
 
  if(encoder0Pos == 303)
  {
    nb_tours = nb_tours + 1;
    Serial.print (nb_tours);
    Serial.print (" / ");
    if(nb_tours == 1)
    {
      motorBackward(1);
      nb_tours = 0;
    } 
    encoder0Pos = 0;   
  }
  else
  {
    if(encoder0Pos == -303)
    {
      nb_tours_1 = nb_tours_1 + 1;
      Serial.print (nb_tours_1);
      Serial.print (" / ");
      if(nb_tours_1 == 1)
      {
        motorForward(1);
        nb_tours_1 = 0;
      }
      encoder0Pos = 0;     
    }
  }
 
//  Serial.print (encoder0Pos);
//  Serial.print (" / ");
     
}


 
void motorForward(int PWM_val) 
{
  analogWrite(enable_L293, PWM_val);
  digitalWrite(pinInput1, LOW);
  digitalWrite(pinInput2, HIGH);
}

void motorBackward(int PWM_val) 
{
 analogWrite(enable_L293, PWM_val);
 digitalWrite(pinInput1, HIGH);
 digitalWrite(pinInput2, LOW);
}

void motorStop() 
{
 analogWrite(enable_L293, 0);
 digitalWrite(pinInput1, LOW);
 digitalWrite(pinInput2, LOW);
}



The motor changes side correcty. I'm able to control the number of rotation well. I need to put 303 (1216/4 -1) to turn only 360°.

Unfortunatelly, when I want to see the encoder value (Serial.print (encoder)) the program doesn't work as previously.

I tried some code to work between 0° to 360° but it doesn't work. Just like it doesn't know the value of the Encoder

cattledog

Quote
Actually I mapped it on pins 5 and 6. They worked well. I also tried with other pins (2 and 4) and the problems are still there.
You are using external interrupts 0 and 1. They should be on pins 2 and 3.

In the last code you posted you only increment encoder0PinA rising which will give you only 1/4 of the available quadrature which is what you are seeing with the count of 303 for one turn. Mark T's code should give you all four transitions if you need that accuracy.

You should be able to print out values of encoder0pos with our latest code. But the Serial.print statement is directly in the loop which is executing very fast and is called even if the encoder isn't changing. Try moving the print call up into the if statement where the value is changing. Depending on how fast the motor is turning, you could also put the serial print on a timer, and only print out the value every second.

Code: [Select]
if ((encoder0PinALast == LOW) && (n == HIGH))
   {
     if (digitalRead(encoder0PinB) == LOW)
     {
       encoder0Pos--;
     }
     else
     {
       encoder0Pos++;
     }
     Serial.print (encoder0Pos);
     Serial.print (" / ");
   }

SimonR

So much better when mapped to pins 2 and 3.

The count is good know both with interrupt and without. I know that without interrupt I only receive 1/4 quadrature but i didn't know why it didn't work when the serial.print is outside the if. However it's fixed now.

It also work with the interrupt now, I need to do some tests on it during the weekend I let you know how it goes on monday, if I still have problem or not.

Thanks guy's, have a nice day !

MarkT

Just for completeness I happened to need to test a high res encoder today and knocked up
a pin-change interrupt version (which should be faster than using attachInterrupt() as
its a direct ISR rather than one redirected from an ISR by pointer indirection.

Code: [Select]

void setup()
{
  pinMode (2, INPUT_PULLUP) ;
  pinMode (3, INPUT_PULLUP) ;
  Serial.begin (115200) ;
  PCICR |= 0x04 ;   // enable pin change interrupt #2 (PORTD on Uno)
  PCMSK2 |= 0x0C ; // enable pins 2&3 for pin change interrupt
}

inline byte get_encoder_phase ()
{
  byte val = (PIND & 0x0C) >> 2 ;  // pins 2&3
  return val ^ (val >> 1) ;
}

volatile byte old_pinch = get_encoder_phase () ;
volatile long count = 0L ;
volatile int errors = 0 ;

ISR (PCINT2_vect)
{
  byte new_pinch = get_encoder_phase () ;
  switch ((new_pinch - old_pinch) & 3)
  {
  case 0b01:  count ++ ; break ;   // difference +1
  case 0b11:  count -- ; break ;   // difference -1
  case 0b10:  errors ++ ; break ;  // difference +/-2, definitely error
  }
  old_pinch = new_pinch ;
}

unsigned long targ = 0L ;
unsigned long prev = 0L ;
int preve = 0 ;
void loop()
{
  if (millis () - targ > 1000)
  {
    targ += 1000 ;
    noInterrupts () ;
    long c = count ;
    interrupts () ;
    noInterrupts () ;
    int e = errors ;
    interrupts () ;
    if (e - preve > 0)
    {
      Serial.print ("ERRORS ") ;
      Serial.println (errors) ;
      preve = e ;
    }
    long Hz = c - prev ;
    prev = c ;
    Serial.print (Hz * 60.0 / 1600.0) ; Serial.print (" ") ; Serial.println (c) ;
  }
}



Tested at 1880 rpm with 1600 count-per-revolution encoder (50kHz interrupt rate) with zero
error count.
[ I DO NOT respond to personal messages, I WILL delete them unread, use the forum please ]

SimonR

Hello guys,

Thank for the other code. I will try it later.

I tried to implement the movement of the motor according to an analog input. 0V is equal to 0° and 5V is equal to 360°.

But my code doesn't work.

Code: [Select]
val = analogRead(analogPin);  // Read analog pin A0
 encoder = (val*1216)/1023;           // Do the conversion between coder position and voltage
                                                  // 1 turn is 360° or 1216 coder values. So we can have the 
                                                  // desired value. Example 3.3V = (675*1216)/1023 = 802 (coder)
                                                  // is equal to 237°

  if(val > previous_val)                  // check the side we need to turn
  {
    if(encoder > encoder_pos)        // if the value we want is still superior as the current value
                                               // the motor turn
    {
      motorForward(100);
    }
    else                                      // else the motor stop
    {
    motorStop();
    }   
  }
  else if(val < previous_val)
  {
    if(encoder < encoder_pos)
    {
      motorBackward(100);
    }
    else
    {
    motorStop();
    }   
  }
 
 previous_val = val;
}


With this code the motor turn but never stop and never changes direction.

I pretty sure that i missed something but I don't know what.


SimonR

Actually the motor changes direction, but only when I pass under 1V. I just slow down when I pass from 5V to 4V to 3V etc ..

But It never stops.

MarkT

What are you using two sets of variables for?  Just use one set, subtract to give the
error, then drive everything from the error.

Code: [Select]

void loop ()
{
  int desired_position = analogRead (pot_pin) * scalefactor ;
  int encoder_position = read_encoder () ;
  int error = desired_position - encoder_position ;
  float output = P * error ; // simple P only PID
  bool direction = output < 0.0 ;
  int drive = int (abs (output)) ;
  analogWrite (pwm_pin, drive) ;
  digitalWrite (direction_pin, direction) ;

  // do other stuff if relevant
}
[ I DO NOT respond to personal messages, I WILL delete them unread, use the forum please ]

Go Up