Go Down

Topic: How to sync motor speed??? (Read 4 times) previous topic - next topic

krazatchu

As already mentioned, PID is what you want.
In addition, you want to command a position, not a speed.
As a loose PID will normally suffer from some velocity following error.
If you find it difficult to tune and/or are going slowly, drop the D and just do PI.
Michael
----------------------
http://www.krazatchu.ca

SuperMiguel

#11
Apr 11, 2012, 04:20 am Last Edit: Apr 11, 2012, 05:11 am by SuperMiguel Reason: 1

As already mentioned, PID is what you want.
In addition, you want to command a position, not a speed.
As a loose PID will normally suffer from some velocity following error.
If you find it difficult to tune and/or are going slowly, drop the D and just do PI.


Got it. This is what i have so far:
Code: [Select]

int E1 = 5;     //Right Motor Power
int E2 = 6;     //Left Motor Power
int M1 = 4;    //Right Motor Direction
int M2 = 7;    //Left Motor Direction
const byte encoder0pinA = 2; //Interrupt 0
const byte encoder0pinB = 12;
const byte encoder1pinA = 3; //Interrupt 1
const byte encoder1pinB = 13;
byte overflow;
byte encoder0PinALast;
byte encoder1PinALast;
int duration0;//Encoder0 number of pulses
int duration00;
int duration1;//Encoder1 number of pulses
int duration11;
boolean Direction0;
boolean Direction1;

int SetPoint = 0; // 0 would be straight ahead
double Kp = 25; // needs tunning
int error = 0;
int speed = 100; // set the speed of which the vechicle should move
int output = 0;

void setup()

  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  Serial.begin(9600);//Initialize the serial port
  EncoderInit();//Initialize the module
  timer_init();
 
}

void loop()
{
//  Serial.print("Left Pulse:");
//  Serial.println(duration00);
//  Serial.print("Right Pulse:");
//  Serial.println(duration11);
  Preg(duration0, duration1); //Go Forward
  digitalWrite(M1,LOW);
  digitalWrite(M2,LOW);
  delay(2500);
  Preg(duration0, duration1); //Go Backward
  digitalWrite(M1,HIGH);
  digitalWrite(M2,HIGH);
  delay(2500);
  digitalWrite(E1,LOW); //STOP!!!!!!
  digitalWrite(E2,LOW);
  delay(2500);
}
ISR(TIMER1_OVF_vect)
{
  overflow++;
  if(overflow>=10)
  {
  duration00=duration0;
  duration0=0;
  duration11=duration1;
  duration1=0;
  overflow=0;
  }
}
void Preg(int SpeedL, int SpeedR)
{
  noInterrupts(); // Disable interrupts, no need to slow down the P regulator
  error = ((SpeedL-SpeedR)-SetPoint);
  output = (Kp*error);
  if(error >= 0) // its turning left of the setpoint, reduce E1 (right motor)
  {
    analogWrite(E2, speed);
    analogWrite(E1, (speed-output)); // Subtract the error value multiplied by Kp from E1
  }
  if(error < 0) // turning right of our setpoint, reduce E0
  {
    analogWrite(E2, (speed+output)); // This time we add the error since its negative
    analogWrite(E1, speed);
  }
  interrupts(); //Enable interrupts again
}

void timer_init(void)
{
  TIMSK1 |= (1<<TOIE1); // Enable Timer1 overflow interrupt at 16MHz = 16 000 000 / 2^16 = 244Hz
}
void EncoderInit()
{
  Direction0 = true;
  Direction1 = true; 
  pinMode(encoder0pinB,INPUT);
  attachInterrupt(0, wheelSpeed0, CHANGE);
  pinMode(encoder1pinB,INPUT); 
  attachInterrupt(1, wheelSpeed1, CHANGE);
}


void wheelSpeed0()
{
  //Encoder 0 Code
  int Lstate = digitalRead(encoder0pinA);
  if((encoder0PinALast == LOW) && Lstate==HIGH)
  {
    int val0 = digitalRead(encoder0pinB);
    if(val0 == LOW && Direction0)
    {
      Direction0 = false; //Reverse
    }
    else if(val0 == HIGH && !Direction0)
    {
      Direction0 = true;  //Forward
    }
  }
  encoder0PinALast = Lstate;

  if(!Direction0)  duration0++;
  else  duration0++;
}

void wheelSpeed1()
{
  //Encoder 1 Code
  int Lstate1 = digitalRead(encoder1pinA);
  if((encoder1PinALast == LOW) && Lstate1==HIGH)
  {
    int val1 = digitalRead(encoder1pinB);
    if(val1 == LOW && Direction1)
    {
      Direction1 = false; //Reverse
    }
    else if(val1 == HIGH && !Direction1)
    {
      Direction1 = true;  //Forward
    }
  }
  encoder1PinALast = Lstate1;

  if(!Direction1)  duration1++;
  else  duration1++;
}


Still trying to tune my Kp value, Is this the best approach? is there a better way to go about this?

krazatchu

The main points of your code look good.
You are using interrupts to read the encoders.
The PID loop is on constant timebase.

My approach would be to run PID on each motor individually, which also makes for an incremental approach.
And command them to turn by specifying a different number of steps for each wheel for the same time.

If the behavior is to run away, try reversing either the motor wires, or reversing the direction bit in sw...
What is the transition count of your encoders per revolution? Low count can be tricky...
Michael
----------------------
http://www.krazatchu.ca

SuperMiguel


The main points of your code look good.
You are using interrupts to read the encoders.
The PID loop is on constant timebase.

My approach would be to run PID on each motor individually, which also makes for an incremental approach.
And command them to turn by specifying a different number of steps for each wheel for the same time.

If the behavior is to run away, try reversing either the motor wires, or reversing the direction bit in sw...
What is the transition count of your encoders per revolution? Low count can be tricky...



I updated the code a bit... just to get more control on it..
Also im not sure on the encoders count per rev (this is the motor: http://www.specamotor.com/en/Faulhaber/motors/2342-012CR/datasheet_motor.html)

krazatchu

#14
Apr 11, 2012, 05:11 am Last Edit: Apr 11, 2012, 05:13 am by krazatchu Reason: 1
Does it look like this?:
http://www.ebay.com/itm/FAULHABER-DC-12V-Motor-2342L012CR-Gear-64-1-Encoder-12CPR-free-ship-/130585677299

The 12CPR likely indicates it's using a pair of hall sensors and a multipole disk magnet for the encoder.
Which would be 64:1 x 12 CPR = 768 CPR at the shaft. With the gear head that is a pretty good count.

Edit: Just saw the back of the motor, it's slotted disk and opto, not multipole magnetic and hall...
Michael
----------------------
http://www.krazatchu.ca

Go Up