Building a robot with two motors(wheels) and i manually set the seed on them to lets say 100, then using the built in encoder i found out that one is rotating way faster than the other, then i had one of them to 100 and the other 108.7, wjhich made a difference of 10 (min i was able to get) but the robot still turn a bit to the right (cuz of difference of speed) ow can i sync them??? So it will roll 100% straight
thats pretty much a mechanical issue. IMHO, you should just find the difference between the motors and do it like you already have in the code.
Recently I had a similar problem. What I did was PWM motor 1 to a constant, but lower than 255, value (e.g. the PWM value to motor 1 is never greater than 245). After a time interval, check the encoder counts of the two motors and have the code automatically adjust, up or down, the PWM value of motor 2 by a fixed value (e.g. 5). I realize that your robot will most likely not move at the same speed all the time but the code could be written to have the PWM of motor 2 adjust to the the PWM value of motor 1. - Scotty
scottyjr:
Recently I had a similar problem. What I did was PWM motor 1 to a constant, but lower than 255, value (e.g. the PWM value to motor 1 is never greater than 245). After a time interval, check the encoder counts of the two motors and have the code automatically adjust, up or down, the PWM value of motor 2 by a fixed value (e.g. 5). I realize that your robot will most likely not move at the same speed all the time but the code could be written to have the PWM of motor 2 adjust to the the PWM value of motor 1. - Scotty
you mean like keep constantly checking the speed of both motors by the encoder rotation count, and for example if left motor count is high 20 counts decrease it speed and viceversa?
If you have encoders on your motor that is what they are for.
Depending on the wheels you are using you might need to adjust for slight differences in diameter because of the wheel diameter.
If you are using continuous rotation servos you have to adjust for full stop, and the deadband area where the servo doesn't move, as well as adjusting for the possibility that they may not turn the same speed in both directions.
The right way to do this is to measure the speed of each wheel and use a PID library (Arduino Playground - PIDLibrary) to set the PWM output to each motor in order to obtain a commanded motor speed. This is a nice way to do it because you can change the setpoints at will to change the speed or to steer (use different commands).
That does seem like a much more suitable method to solve the problem. PID - didn't know of such an animal till now. Thanks - Scotty
rocketgeek:
The right way to do this is to measure the speed of each wheel and use a PID library (Arduino Playground - HomePage) to set the PWM output to each motor in order to obtain a commanded motor speed. This is a nice way to do it because you can change the setpoints at will to change the speed or to steer (use different commands).
Thanks , I looked online for an example of how to use pid lib to sync the motor speed but failed to do so... Do you know any good example for it??
The trick is not to think of it as synchronizing the motors so much as it is setting them both to a commanded speed. That means each motor is its own separate unit, with its own PID loop. You then synchronize the motors by sending them the same command. This gives you the option to send them different commands, for things like steering or compensating for mismatched wheels.
For each motor, the first thing to do is measure the speed. You can do this by measuring the time between encoder counts and dividing the amount of distance covered in a single count by the time difference.
Now, you have the actual speed and you have the desired speed. Subtract that actual speed from the desired speed; this gives you the error. The error value is the input to the PID loop. The output from the PID loop, once it's correctly tuned, is the PWM value to write to the output for that motor.
The link I provided has a lot more information on how to set up and tune PID loops. It's pretty good; you should read it.
So this is what im working with right now:
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 encoder0PinALast;
byte encoder1PinALast;
int duration0;//Encoder0 number of pulses
int duration1;//Encoder1 number of pulses
boolean Direction0;
boolean Direction1;
void setup()
{
Serial.begin(9600);//Initialize the serial port
EncoderInit();//Initialize the module
}
void loop()
{
Serial.print("Left Pulse:");
Serial.println(duration0);
Serial.print("Right Pulse:");
Serial.println(duration1);
duration0 = 0;
duration1 = 0;
analogWrite (E1,100);
digitalWrite(M1,LOW);
analogWrite (E2,107.9);
digitalWrite(M2,LOW);
delay(500);
}
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--;
}
This is the serial output:
Left Pulse:1
Right Pulse:-1
Left Pulse:780
Right Pulse:708
Left Pulse:1437
Right Pulse:1321
Left Pulse:1618
Right Pulse:1491
Left Pulse:1675
Right Pulse:1549
Left Pulse:1697
Right Pulse:1571
Left Pulse:1704
Right Pulse:1579
Left Pulse:1707
Right Pulse:1589
Left Pulse:1709
Right Pulse:1589
Left Pulse:1712
Right Pulse:1591
Left Pulse:1714
Right Pulse:1594
Left Pulse:1716
Right Pulse:1591
Left Pulse:1717
Right Pulse:1592
Left Pulse:1719
Right Pulse:1593
Left Pulse:1722
Right Pulse:1592
Left Pulse:1723
Right Pulse:1597
Left Pulse:1722
Right Pulse:1597
Left Pulse:1724
Right Pulse:1600
Left Pulse:1725
Right Pulse:1604
At this speed it takes the Right wheel about 1.2s so right wheel average of 1596/1200 milliseconds = 1.33 more or less the speed that i want..
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.
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.
Got it. This is what i have so far:
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?
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...
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...
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)
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...
krazatchu:
Does it look like this?:
http://www.ebay.com/itm/FAULHABER-DC-12V-Motor-2342L012CR-Gear-64-1-Encoder-12CPR-free-ship-/130585677299The 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...
Ya mine is a round pcb on the back not a rectangle one..
few code modification:
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);
digitalWrite(M1,LOW); //Forward
digitalWrite(M2,LOW);
delay(1500);
digitalWrite(M1,HIGH); //Backwards
digitalWrite(M2,HIGH);
delay(1500);
speed = 0; //STOP!!!!!!
Kp = 0;
delay(4000);
speed = 100;
Kp = 25;
}
ISR(TIMER1_OVF_vect)
{
overflow++;
if(overflow>=10)
{
Preg(duration0, duration1);
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++;
}
Is there a better implementation of this????
SuperMiguel:
Is there a better implementation of this????
Is this just a general 'please make my code better' or is there some specific problem you want help with?
PeterH:
SuperMiguel:
Is there a better implementation of this????Is this just a general 'please make my code better' or is there some specific problem you want help with?
Ya is there a lib to help me find kp?? And is my pid implementation correct?? Should I try using the pid lib?? Any benefits??