Help me please position control motor code

Projects Programming
1

I do this problem 2 month . I can't solve this problem code. I use arduino due ,H-bridge,dc motor and power supply.
I want to control position of motor and use pid control.
Please Help me to solve this code. Thank you for help me.
My code is

#define encoder0PinA  2
#define encoder0PinB  4

//#define encoder1PinA  3
//#define encoder1PinB  5

#define SpeedPin     9
//#define DirectionPin 8
#define Direction1Pin 10
#define Direction2Pin 11

volatile long encoder0Pos = 0;
//volatile long encoder1Pos = 0;

long target = 90;

//correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors)
//PID controller constants
float KP = 120 ; //position multiplier (gain)
float KI = 50;//.25; // Intergral multiplier (gain)
float KD = 15;//1.0; // derivative multiplier (gain)

int lastError = 0;
int sumError = 0;

//Integral term min/max (random value and not yet tested/verified)
int iMax = 100;
int iMin = 0;

long previousTarget = 0;
long previousMillis = 0;        // will store last time LED was updated
long interval = 5;           // interval at which to blink (milliseconds)

void setup() { 

  pinMode(encoder0PinA, INPUT); 
  pinMode(encoder0PinB, INPUT); 
    
//  pinMode(encoder1PinA, INPUT);   
//  pinMode(encoder1PinB, INPUT); 
//  
  pinMode(Direction1Pin, OUTPUT); 
  pinMode(Direction2Pin, OUTPUT);   
  pinMode(SpeedPin, OUTPUT); 

  attachInterrupt(2, doEncoderMotor0, CHANGE);  // encoder pin on interrupt 0 - pin 2
  attachInterrupt(4, doEncoderMotor0, CHANGE);  
  
  Serial.begin (115200);
  Serial.println("start");                // a personal quirk

} 

void loop(){
  
  
  // do some stuff here - the joy of interrupts is that they take care of themselves  
  if (millis() - previousTarget > 1000)
  {
      Serial.print ( encoder0Pos );
      Serial.print ( " , " );
      Serial.println ( target );
      
      previousTarget = millis();
           
      //target += 1024; //for single ch
//      target += 2048; //for dual ch
  }
  
//  target = 5.12 * encoder1Pos;
  docalc();
}

void docalc() {
  
  if (millis() - previousMillis > interval) 
  {
    previousMillis = millis();   // remember the last time we blinked the LED
    
    long error = (encoder0Pos*0.72) - target ; // find the error term of current position - target    
    
    //generalized PID formula
    //correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors)
    long ms = KP * error + KD * (error - lastError) +KI * (sumError);
       
    lastError = error;    
    sumError += error;
    
    //scale the sum for the integral term
    if(sumError > iMax) {
      sumError = iMax;
    } else if(sumError < iMin){
      sumError = iMin;
    }
    
    if(ms > 0){
//      digitalWrite ( Direction1Pin ,HIGH );      
//      digitalWrite ( Direction2Pin ,LOW );    
      digitalWrite ( Direction1Pin , LOW );     
      digitalWrite ( Direction2Pin ,HIGH ); 
    }
    if(ms < 0){
//      digitalWrite ( Direction1Pin , LOW );     
//      digitalWrite ( Direction2Pin ,HIGH );   
      digitalWrite ( Direction1Pin ,HIGH );      
      digitalWrite ( Direction2Pin ,LOW );        
      ms = -1 * ms;
    }
//    if(ms==0)
//{digitalWrite ( Direction1Pin ,HIGH );      
//      digitalWrite ( Direction2Pin ,HIGH );   
//ms=0;
//}
    int motorSpeed = map(ms,0,1024,0,255/80); 
    //analogWrite ( SpeedPin, (255 - motorSpeed) );
    analogWrite ( SpeedPin,  motorSpeed );
  }  
}

void doEncoderMotor0(){
  if (digitalRead(encoder0PinA) == HIGH) {   // found a low-to-high on channel A
    if (digitalRead(encoder0PinB) == LOW) {  // check channel B to see which way
                                             // encoder is turning
      encoder0Pos = encoder0Pos - 1;         // CCW
    } 
    else {
      encoder0Pos = encoder0Pos + 1;         // CW
    }
  }
  else                                        // found a high-to-low on channel A
  { 
    if (digitalRead(encoder0PinB) == LOW) {   // check channel B to see which way
                                              // encoder is turning  
      encoder0Pos = encoder0Pos + 1;          // CW
    } 
    else {
      encoder0Pos = encoder0Pos - 1;          // CCW
    }

  }
 
}
2

You don't actually say what problem you have.

One thing I noticed are lines like this

    long error = (encoder0Pos*0.72) - target ;

and

volatile long encoder0Pos = 0;

The error and encoder0Pos variables are declared as a long which implies that their value could be between -2,147,483,648 and 2,147,483,647. Do you need such large numbers ?

Also, in the first example line above you multiply encoder0Pos by 0.72 and put the result in a long integer, not a float. This may not matter but it is probably not doing exactly what you think.

3

I resolve my code follower you tell me but encoder is read wrong value . Enocoder is run in one direction but some time it read + sometime read - .

#define encoder0PinA  2
#define encoder0PinB  3

//#define encoder1PinA  3
//#define encoder1PinB  5

#define SpeedPin     9
//#define DirectionPin 8
#define Direction1Pin 10
#define Direction2Pin 11

volatile float encoder0Pos = 0;
//volatile long encoder1Pos = 0;

long target = 90;

//correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors)
//PID controller constants
float KP = 120 ; //position multiplier (gain)
float KI = 50;//.25; // Intergral multiplier (gain)
float KD = 15;//1.0; // derivative multiplier (gain)

int lastError = 0;
int sumError = 0;

//Integral term min/max (random value and not yet tested/verified)
int iMax = 100;
int iMin = 0;

long previousTarget = 0;
long previousMillis = 0;        // will store last time LED was updated
long interval = 5;           // interval at which to blink (milliseconds)

void setup() { 

  pinMode(encoder0PinA, INPUT); 
  pinMode(encoder0PinB, INPUT); 
    
//  pinMode(encoder1PinA, INPUT);   
//  pinMode(encoder1PinB, INPUT); 
//  
  pinMode(Direction1Pin, OUTPUT); 
  pinMode(Direction2Pin, OUTPUT);   
  pinMode(SpeedPin, OUTPUT); 

  attachInterrupt(2, doEncoderMotor0, CHANGE);  // encoder pin on interrupt 0 - pin 2
  attachInterrupt(3, doEncoderMotor0, CHANGE);  
  
  Serial.begin (115200);
  Serial.println("start");                // a personal quirk

} 

void loop(){
  
  
  // do some stuff here - the joy of interrupts is that they take care of themselves  
  if (millis() - previousTarget > 1000)
  {
      Serial.print ( encoder0Pos );
      Serial.print ( " , " );
      Serial.println ( target );
      
      previousTarget = millis();
           
      //target += 1024; //for single ch
//      target += 2048; //for dual ch
  }
  
//  target = 5.12 * encoder1Pos;
  docalc();
}

void docalc() {
  
  if (millis() - previousMillis > interval) 
  {
    previousMillis = millis();   // remember the last time we blinked the LED
    
    float error = (encoder0Pos*0.72) - target ; // find the error term of current position - target    
    
    //generalized PID formula
    //correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors)
    float ms = KP * error + KD * (error - lastError) +KI * (sumError);
       
    lastError = error;    
    sumError += error;
    
    //scale the sum for the integral term
    if(sumError > iMax) {
      sumError = iMax;
    } else if(sumError < iMin){
      sumError = iMin;
    }
    
    if(ms > 0){
//      digitalWrite ( Direction1Pin ,HIGH );      
//      digitalWrite ( Direction2Pin ,LOW );    
      digitalWrite ( Direction1Pin , LOW );     
      digitalWrite ( Direction2Pin ,HIGH ); 
    }
    if(ms < 0){
//      digitalWrite ( Direction1Pin , LOW );     
//      digitalWrite ( Direction2Pin ,HIGH );   
      digitalWrite ( Direction1Pin ,HIGH );      
      digitalWrite ( Direction2Pin ,LOW );        
      ms = -1 * ms;
    }
//    if(ms==0)
//{digitalWrite ( Direction1Pin ,HIGH );      
//      digitalWrite ( Direction2Pin ,HIGH );   
//ms=0;
//}
    int motorSpeed = map(ms,0,1024,0,255/80); 
    //analogWrite ( SpeedPin, (255 - motorSpeed) );
    analogWrite ( SpeedPin,  motorSpeed );
  }  
}

void doEncoderMotor0(){
  if (digitalRead(encoder0PinA) == HIGH) {   // found a low-to-high on channel A
    if (digitalRead(encoder0PinB) == LOW) {  // check channel B to see which way
                                             // encoder is turning
      encoder0Pos = encoder0Pos - 1;         // CCW
    } 
    else {
      encoder0Pos = encoder0Pos + 1;         // CW
    }
  }
  else                                        // found a high-to-low on channel A
  { 
    if (digitalRead(encoder0PinB) == LOW) {   // check channel B to see which way
                                              // encoder is turning  
      encoder0Pos = encoder0Pos + 1;          // CW
    } 
    else {
      encoder0Pos = encoder0Pos - 1;          // CCW
    }

  }
 
}
4

What kind of "encoder" are you using? The standard ones do not allow you to position a motor.

Mark

5

My encoder is Incremental Encoder. thx for your help. My code wrong ?

6

Your encoder0Pos variable should be an integer type not a float. Whether it needs to be byte, int, long etc would depend on the range of values it needs to hold. All your other floating point calculations derived from it should be using integer arithmetic too.

When accessing volatile variables bigger than a byte within the main context, you should disable interrupts around the access. The recommended approach is to suspend interrupts, copy the volatile value to a local variable, and re-enable interrupts as quickly as possible.

7

how many pulses per revolution does it give you. How do you find the motors start position?

Basic rules with motors

  1. Standard DC motors - good control of speed/direction at best poor control of position

  2. Servo - good+ control of position, limited control of speed (needs complex code)

  3. Stepper motor - good control of position/speed direction.

But none of them have any way to tell where they are to start with.

Mark

PS by the way

long previousTarget = 0;
long previousMillis = 0;        // will store last time LED was updated

need to be unsigned long not long.

M

8

My pluse is 500 pluse per revolution.I do follower you suggestion it good than my old code but it has problem when i start new run motor it near my angle requirement but not repeat old position .Sorry if my english isn't good. thx for your suggest. :slight_smile:

9

Which Arduino are you using?

  attachInterrupt(2, doEncoderMotor0, CHANGE);  // encoder pin on interrupt 0 - pin 2
  attachInterrupt(3, doEncoderMotor0, CHANGE);

attachInterrupt() uses the interrupt number NOT the pin number! ( except on the Due)

Mark

10

holmes4:
Which Arduino are you using?

  attachInterrupt(2, doEncoderMotor0, CHANGE);  // encoder pin on interrupt 0 - pin 2

attachInterrupt(3, doEncoderMotor0, CHANGE);




attachInterrupt() uses the interrupt number NOT the pin number! ( except on the Due)

Mark

The OP says its a Due on the first posting!

11 
void doEncoderMotor0(){
  if (digitalRead(encoder0PinA) == HIGH) {   // found a low-to-high on channel A
    if (digitalRead(encoder0PinB) == LOW) {  // check channel B to see which way
                                             // encoder is turning
      encoder0Pos = encoder0Pos - 1;         // CCW
    } 
    else {
      encoder0Pos = encoder0Pos + 1;         // CW
    }
  }
  else                                        // found a high-to-low on channel A
  { 
    if (digitalRead(encoder0PinB) == LOW) {   // check channel B to see which way
                                              // encoder is turning  
      encoder0Pos = encoder0Pos + 1;          // CW
    } 
    else {
      encoder0Pos = encoder0Pos - 1;          // CCW
    }

  }
 
}

Is an incredibly long-winded way to say:

void doEncoderMotor0()
{
  if (digitalRead(encoder0PinA))  // found a low-to-high on channel A
    encoder0Pos += digitalRead(encoder0PinB) ? 1 : -1 ;
  else                                        // found a high-to-low on channel A
    encoder0Pos += digitalRead(encoder0PinB) ? -1 : 1 ;
}

or even

void doEncoderMotor0()
{
  encoder0Pos += (digitalRead (encoder0PinA) ^ digitalRead (encoder0PinB)) ? -1 : 1 ;
}

And you have to only attach the interrupt routine to one of the pins for this code to work at all.

If you want to react to changes on both pins you must record the
previous state to interpret the current values as a direction.

12

thx every body to help me.
My update code is

#define encoder0PinA  2
#define encoder0PinB  3

//#define encoder1PinA  3
//#define encoder1PinB  5

#define SpeedPin     9
//#define DirectionPin 8
#define Direction1Pin 10
#define Direction2Pin 11

volatile int encoder0Pos = 0;
//volatile long encoder1Pos = 0;

long target = 90;

//correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors)
//PID controller constants
float KP = 150 ; //position multiplier (gain)
float KI = 50;//.25; // Intergral multiplier (gain)
float KD = 15;//1.0; // derivative multiplier (gain)

int lastError = 0;
int sumError = 0;

//Integral term min/max (random value and not yet tested/verified)
int iMax = 100;
int iMin = 0;

unsigned long previousTarget = 0;
unsigned long previousMillis = 0;        // will store last time LED was updated
long interval = 5;           // interval at which to blink (milliseconds)

void setup() { 

  pinMode(encoder0PinA, INPUT); 
  pinMode(encoder0PinB, INPUT); 
    
//  pinMode(encoder1PinA, INPUT);   
//  pinMode(encoder1PinB, INPUT); 
//  
  pinMode(Direction1Pin, OUTPUT); 
  pinMode(Direction2Pin, OUTPUT);   
  pinMode(SpeedPin, OUTPUT); 

  attachInterrupt(2, doEncoderMotor0, CHANGE);  // encoder pin on interrupt 0 - pin 2
  attachInterrupt(3, doEncoderMotor0, CHANGE);  
  
  Serial.begin (115200);
  Serial.println("start");                // a personal quirk

} 

void loop(){
  
  
  // do some stuff here - the joy of interrupts is that they take care of themselves  
  if (millis() - previousTarget > 1000)
  {
    
    Serial.print ("encoder");
    Serial.print ( " , " );
      Serial.println("target");
    Serial.print ( encoder0Pos );
       Serial.print ( " , " );
      Serial.println ( target );
      
      previousTarget = millis();
           
      //target += 1024; //for single ch
//      target += 2048; //for dual ch
  }
  
//  target = 5.12 * encoder1Pos;
  docalc();
}

void docalc() {
  
  if (millis() - previousMillis > interval) 
  {
    previousMillis = millis();   // remember the last time we blinked the LED
    //if(encoder0Pos>250){encoder0Pos=250;}
   //if(encoder0Pos<-250){encoder0Pos=-250;}
    
    float error = (encoder0Pos*0.72) - target ; // find the error term of current position - target    
    
    //generalized PID formula
    //correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors)
    float ms = KP * error + KD * (error - lastError) +KI * (sumError);
       
    lastError = error;    
    sumError += error;
    
    //scale the sum for the integral term
    if(sumError > iMax) {
      sumError = iMax;
    } else if(sumError < iMin){
      sumError = iMin;
    }
    
    if(ms > 0){
//      digitalWrite ( Direction1Pin ,HIGH );      
//      digitalWrite ( Direction2Pin ,LOW );    
      digitalWrite ( Direction1Pin , LOW );     
      digitalWrite ( Direction2Pin ,HIGH ); 
    }
    if(ms < 0){
//      digitalWrite ( Direction1Pin , LOW );     
//      digitalWrite ( Direction2Pin ,HIGH );   
      digitalWrite ( Direction1Pin ,HIGH );      
      digitalWrite ( Direction2Pin ,LOW );        
      ms = -1 * ms;
    }
    if(ms==0)
{digitalWrite ( Direction1Pin ,HIGH );      
      digitalWrite ( Direction2Pin ,HIGH );   
ms=0;
}
    int motorSpeed = map(ms,0,1024,0,255/120); 
    //analogWrite ( SpeedPin, (255 - motorSpeed) );
    analogWrite ( SpeedPin,  motorSpeed );
  }  
}
void doEncoderMotor0()
{
  encoder0Pos += (digitalRead (encoder0PinA) ^ digitalRead (encoder0PinB)) ? -1 : 1 ;
}

My result in first time ![](http://first time)
My result in second time ![](http://second time)
when start value from encoder in first time result is inequality second time result.
In end value in first time result is inequality second time result.
What should I do ???

en.png
en.png1365×727 17 KB

en2.png
en2.png1363×725 17.1 KB