Hi,
I;m trying to implement PID control over my 6vdc motor using this DIDEL magnetic quadrature encoder(>>http://didel.com/Rome.pdf) but Till now I have been unsuccessful ,The following is my code:
#include <PID_v1.h>
#define encoder0PinA 2
#define encoder0PinB 3
volatile unsigned int encoder0Pos = 0;
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint,2,5,1, DIRECT);
void setup() {
pinMode(encoder0PinA, INPUT);
pinMode(encoder0PinB, INPUT);
Setpoint = 9;
// encoder pin on interrupt 0 (pin 2)
attachInterrupt(0, doEncoderA, CHANGE);
// encoder pin on interrupt 1 (pin 3)
attachInterrupt(1, doEncoderB, CHANGE);
myPID.SetMode(AUTOMATIC);
Serial.begin (19200);
}
void loop(){
Input = encoder0Pos;
myPID.Compute();
analogWrite(5,Output);
}
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
}
}
}
AS per the code I have written only one way motor control on pin D5 is being utilised so I think the motor would moved by PID to the setpoint only If the position is turned to one particular way of turn(cw/ccw) as its controlled by pin D5.
Suggest some corrections.
If Output is negative you have to turn the motor in the other direction.
If Output is negative you have to turn the motor in the other direction.
for that I have to define the encoder0pos as Double then the software provides the negative or positive values from this variable
If Output is negative you have to turn the motor in the other direction.
Wasn't able to get it correct, Some more light?
The problem I'm facing here is that though I have tried inserting another PID function to control the other pin for the H-Bridge to turn the motor otherwise ,Still the code behaves in a erratic way the motor goes on rotating by itself like that only random turns sometimes speeds up and sometimes goes down and/or runs on one way by itself and then stops like that only.
NI$HANT:
If Output is negative you have to turn the motor in the other direction.
Wasn't able to get it correct, Some more light?
void loop(){
Input = encoder0Pos;
myPID.Compute();
if (Output > 0)
{
digitalWrite(directionPin, HIGH);
analogWrite(5,Output);
}
else
{
digitalWrite(directionPin, LOW);
analogWrite(5, -Output);
}
}
void loop(){
Input = encoder0Pos;
myPID.Compute();
if (Output > 0)
{
digitalWrite(directionPin, HIGH);
analogWrite(5,Output);
}
else
{
digitalWrite(directionPin, LOW);
analogWrite(5, -Output);
}
}
But the Direction pin is put in the analogWrite's itself , I believe(If I'm right) You want me to put the Enable pin of the H-Bridge in analogWrite and the 2 Drirection inside the digitalWrite controlling ? , but my Didel encoder bridge only has a vcc supply and two direction control pins and no enable pin like in L293D or TI's SN IC
What to do as such?
Sorry. I did the best I could with the very limited information provided. You didn't bother to specify what hardware you were using and what pins it was connected to. If you had given more detailed information I would have been able to provide more relevant assistance.
Hi Sir John,
Sorry for not defining in Full!
Please have the additional Information:
The encoder I'm using is a Magnetic encoder here >> http://didel.com/Rome.pdf
(The encoder has an on board H-Bridge and the whole system works from voltage fed between 3 to 6 volts to the encoder ,Please see the above link it's very elaborative and shows a lot of picture's) and Yes an Arduino is a NANO Arduino.
and The motor I'm using is a 6volt DC motor and the whole system is powered by a 7806 voltage regulator that gives 6 volt power coming to it from a wallwart with 1 Amp power @ 8 volt Encoder Channel 1 is connected to D2 Interrupt PIN and Encoder Channel 2 is connected to the D3 interrupt PIN on Arduino for the Motor Direction control pin's the Motor control 1 is connected to the Digital PIN 5 and Motor control 2 is connected to the Digital PIN 6 on Arduino, So this is regarding the connection's.
You have an unusual motor controller. The motor only runs when the two inputs are different. Usually there would be one input for Direction and one for Speed. The way yours works, the Speed goes from 0 to 255 (off to full) on one direction and 255 to 0 (off to full) in the other direction.
// Motor control 1 is connected to the Digital PIN 5
const int MotorControlPin1 = 5;
// Motor control 2 is connected to the Digital PIN 6
const int MotorControlPin2 = 6;
void loop(){
Input = encoder0Pos;
myPID.Compute();
if (Output > 0)
{
// Output is positive so set MotorControlPin2 (Direction) to HIGH
digitalWrite(MotorControlPin2, HIGH);
// When Direction is HIGH, a LOW on MotorControlPin1 (PWM Speed) causes the motor to run
analogWrite(MotorControlPin1, 255 - constrain((int)Output, 0, 255));
}
else
{
// Output is negative so set MotorControlPin2 (Direction) to LOW
digitalWrite(MotorControlPin2, LOW);
// When Direction is LOW, a HIGH on MotorControlPin1 (PWM Speed) causes the motor to run
analogWrite(MotorControlPin1, constrain((int)-Output, 0, 255));
}
}
Thanks! Mr.John, One more question I'm setting the setpoint of the PID value to the counts that are taken up by the encoder like 9 so I think when the PID sees the value counted isn;t 9 then it will strive to achieve that position by rotation forward or reverse to get hold to that position but mine isn;t working like that??
The following is the completed code:
#include <PID_v1.h>
#define encoder0PinA 2
#define encoder0PinB 3
volatile unsigned int encoder0Pos = 0;
// Motor control 1 is connected to the Digital PIN 5
const int MotorControlPin1 = 5;
// Motor control 2 is connected to the Digital PIN 6
const int MotorControlPin2 = 6;
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint,2,5,1, DIRECT);
void setup() {
pinMode(encoder0PinA, INPUT);
pinMode(encoder0PinB, INPUT);
Setpoint = 4;
// encoder pin on interrupt 0 (pin 2)
attachInterrupt(0, doEncoderA, CHANGE);
// encoder pin on interrupt 1 (pin 3)
attachInterrupt(1, doEncoderB, CHANGE);
myPID.SetMode(AUTOMATIC);
Serial.begin (19200);
}
void loop(){
Input = encoder0Pos;
myPID.Compute();
if (Output > 0)
{
// Output is positive so set MotorControlPin2 (Direction) to HIGH
digitalWrite(MotorControlPin2, HIGH);
// When Direction is HIGH, a LOW on MotorControlPin1 (PWM Speed) causes the motor to run
analogWrite(MotorControlPin1, 255 - constrain((int)Output, 0, 255));
}
else
{
// Output is negative so set MotorControlPin2 (Direction) to LOW
digitalWrite(MotorControlPin2, LOW);
// When Direction is LOW, a HIGH on MotorControlPin1 (PWM Speed) causes the motor to run
analogWrite(MotorControlPin1, constrain((int)-Output, 0, 255));
}
}
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
}
}
}
I have somewhat small success with the following code but still the hard grip of the motor at a particular position is not there, which parameter of the kp,ki or kd I must modify or what changes in the programme must be made?
BEHAVIOUR: Now the motor seems to catch up a position but moves pretty fast from one point to another , the distance between these two points is too less.
PLEASE! watch - Video>> Magnetic Encoder on Shaft - YouTube
#include <PID_v1.h>
#define encoder0PinA 2
#define encoder0PinB 3
volatile unsigned int encoder0Pos = 0;
// Motor control 1 is connected to the Digital PIN 5
const int MotorControlPin1 = 5;
// Motor control 2 is connected to the Digital PIN 6
const int MotorControlPin2 = 6;
double KD,KP,KI,lastError,sumError;
void setup() {
pinMode(encoder0PinA, INPUT);
pinMode(encoder0PinB, INPUT);
// encoder pin on interrupt 0 (pin 2)
attachInterrupt(0, doEncoderA, CHANGE);
// encoder pin on interrupt 1 (pin 3)
attachInterrupt(1, doEncoderB, CHANGE);
Serial.begin (19200);
}
void loop(){
int val; //= analogRead(14);
int target = map(212, 0, 1023, 0,12 );// set the seek to target by mapping the potentiometer range to the encoder max count
int error = encoder0Pos - target; // find the error term = current position - target
// generalized PID formula
//correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors)
// calculate a motor speed for the current conditions
int motorSpeed = 10 * error;//kp
motorSpeed += 5 * (error - lastError);//kd
motorSpeed += 10 * (sumError);//ki
// set the last and sumerrors for next loop iteration
lastError = error;
sumError += error;
if (motorSpeed > 0)
{
// Output is positive so set MotorControlPin2 (Direction) to HIGH
digitalWrite(MotorControlPin2, HIGH);
// When Direction is HIGH, a LOW on MotorControlPin1 (PWM Speed) causes the motor to run
analogWrite(MotorControlPin1, 255 - constrain((int)motorSpeed, 0, 255));
}
else
{
// Output is negative so set MotorControlPin2 (Direction) to LOW
digitalWrite(MotorControlPin2, LOW);
// When Direction is LOW, a HIGH on MotorControlPin1 (PWM Speed) causes the motor to run
analogWrite(MotorControlPin1, constrain((int)-motorSpeed, 0, 255));
}
}
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
}
}
}
I suspect that your PID parameters are WAY off. You should probably read up on PID tuning.