Using the Arduino PID Library for position control of X and Y axis on RepScrap printer

I've updated the test code I'm using to manage my X and Y axis DC motor / linear encoder closed loop controller. 

I am currently using the Arduino PID Library by Brett Beauregard  for this, and having great success.  Videos to come tomorrow. 

I am *NOT* going to explain what PID is, or how PID works.... I couldn't possibly do it justice.  I'll simply point you to Brett's wonderful explanation:

 http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction

In the following example, I set up two axis, X and Y, each using a DC motor run from the Adafruit Motor Shield V2.  This shield provides PWM control for up to four separate DC motors via I2C communications.

I then set up two Quadrature encoders, one for each axis, using the Hardware Interrupts 0 and 1 (Arduino digital pins 2 and 3) and high speed digital port reads for one half of each encoder, and then validate the state of the other phase pin of the encoder during the interrupt routine: 
Graciously borrowed from http://forum.arduino.cc/index.php?topic=41615.20;wap2

The ZERO endstop for each axis is set up using the Arduino PinChangeInterrupt library watching a pin attached to a photo-interrupter.

I would certainly accept any advice on a proper sequence to initialize each axis to the ZERO endstop.

Right now, I arbitrarily send the carriage forward for 100ms assuming this is enough time to get on the positive side of the endstop, if we were beyond it.  Then I set my current position to the maximum possible location, and start travelling back to the endstop, knowing that once I actually reach it, the interrupt routine will Zero out my position, and initialize my PID setpoint to zero as well, thus stopping travel at ZERO. 

Is there a more efficient way of doing this? 

Inside the loop portion of my code, I run the PID controls as per the library, providing motor speed control via the Adafruit motor class, and periodically check to see if both X and Y axis have reached their goal.  At which time, I randomly select a new target for each.  When I get to the real application of this, the random selection of X and Y axis targets will be replaced by GRBL coordinates. 

And without further ado, here is my working code for precise position control in X and Y axis using the Arduino PID library:

/***************************************************************************************

*  Lin_Enc_02.ino   05-12-2014   unix_guru at hotmail.com   @unix_guru on twitter

*  http://arduino-pi.blogspot.com

*

*  This sketch allows you to run two salvaged printer carriages for X/Y axis using their 

*  linear encoder strips for tracking. 

*  This example uses the Arduino PID Library found at:

*  https://github.com/br3ttb/Arduino-PID-Library/archive/master.zip

*

*  Hardware Interrupt 0 on Digital pin2 is used to determine X-Axis position

*  Hardware Interrupt 1 on Digital pin3 is used to determine Y-Axis position

*  PinchangeInterrupt is used to identify the Zero Endstop for X and Y axis

*****************************************************************************************/

#include <Wire.h>

#include <Adafruit_MotorShield.h>

#include "utility/Adafruit_PWMServoDriver.h"

#include <PID_v1.h> 

#include <PinChangeInt.h>

#define frontstop = 100                          // Right most encoder boundary

#define backstop = 3600                         // Left most encoder boundary

// Create the motor shield object with the default I2C address

Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 

// Select which 'port' M1, M2, M3 or M4. In this case, M1

Adafruit_DCMotor *XaxisMotor = AFMS.getMotor(1);

Adafruit_DCMotor *YaxisMotor = AFMS.getMotor(2);

const int XaxisENCPinA = 2;                  // X-AXIS  encoder 1 on pins 2 and 4

const int XaxisENCPinB = 4;

const int XaxisENDSTOP = 10;               // Endstop photointerrupter for X-Axis

volatile double XaxisENCPos = 0;

const int YaxisENCPinA = 3;                  // Y-AXIS  encoder 2 on pins 3 and 5

const int YaxisENCPinB = 5;

const int YaxisENDSTOP = 11;               // Endstop photointerrupter for Y-Axis

volatile double YaxisENCPos = 0;

double XaxisSpd,  YaxisSpd;                  // Carriage speed from 0-255

double XaxisPos, YaxisPos;                   // Current Carriage position

/*working variables for PID routines*/

// Tuning parameters

float KpX=0,  KpY=0;                          //Initial Proportional Gain 

float KiX=10, KiY=10;                         //Initial Integral Gain 

float KdX=0,  KdY=0;                          //Initial Differential Gain 

double XaxisSetpoint, YaxisSetpoint;      // Taget position for carriage

// Instantiate X and Y axis PID controls

PID XaxisPID(&XaxisPos, &XaxisSpd, &XaxisSetpoint, KpX, KiX, KdX, DIRECT); 

PID YaxisPID(&YaxisPos, &YaxisSpd, &YaxisSetpoint, KpY, KiY, KdY, DIRECT); 

const int sampleRate = 1; 

long int reportTime;

void setup() {

  Serial.begin(115200);

  Serial.println("Linear Encoder Test  05-12-2014");

  AFMS.begin();  // Set up Motors

  

  XaxisMotor->run(BACKWARD);                  // Bring carriage to home position. 

  XaxisMotor->setSpeed(70); 

  delay(100);                                                // Get endstop limiter working here

  XaxisMotor->run(FORWARD);                    // Bring carriage to home position. 

  XaxisMotor->setSpeed(0); 

  

  YaxisMotor->run(BACKWARD);                  // Bring carriage to home position. 

  YaxisMotor->setSpeed(70); 

  delay(100);                                                // Get endstop limiter working here

  YaxisMotor->run(FORWARD);                    // Bring carriage to home position. 

  YaxisMotor->setSpeed(0); 

  

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

  attachInterrupt(1, doYaxisENC, CHANGE);     // encoder pin on interrupt 1 (pin 3)

  PCintPort::attachInterrupt(XaxisENDSTOP,doXaxisEndstop,FALLING); //X-axis Endstop ISR

  PCintPort::attachInterrupt(YaxisENDSTOP,doYaxisEndstop,FALLING); //Y-axis Endstop ISR

  randomSeed(analogRead(0));                          // Used to select random setpoints for testing

  XaxisPID.SetMode(AUTOMATIC);                //Turn on the PID loop 

  XaxisPID.SetSampleTime(sampleRate);         //Sets the sample rate 

  YaxisPID.SetMode(AUTOMATIC);                //Turn on the PID loop 

  YaxisPID.SetSampleTime(sampleRate);         //Sets the sample rate 

  reportTime = millis()+2000;

}

void loop() {

uint8_t oldSREG = SREG;                           // Store interrupt status register

  cli();

  XaxisPos = XaxisENCPos;  

  YaxisPos = YaxisENCPos;

  SREG = oldSREG;                                    // Restore interrupt status register

  

  // Temporary to create random X and Y axis setpoints for testing

  if(millis() > reportTime) {                               // Only validate this every 2 seconds

    if(XaxisPos == XaxisSetpoint && YaxisPos == YaxisSetpoint) {   

      // If both X-axis and Y-axis have reached their target - get new targets

      XaxisSetpoint =  random(200,3500);             // Keep target within bounds of Endpoints

      YaxisSetpoint =  random(200,3500);             // Keep target within bounds of Endpoints

    }    

    reportTime = millis()+2000;

}

  

  

  // Manage X-axis positioning

  XaxisPID.Compute();                          //Run the PID loop 

  if(XaxisSetpoint < XaxisPos) XaxisMotor->run(BACKWARD);  // Determine direction of travel

  else  XaxisMotor->run(FORWARD);      

  XaxisMotor->setSpeed(XaxisSpd);              // Apply PID speed to motor

  // Manage Y-axis positioning

  YaxisPID.Compute();                          //Run the PID loop 

  if(YaxisSetpoint < YaxisPos) YaxisMotor->run(BACKWARD);  // Determine direction of travel

  else  YaxisMotor->run(FORWARD);      

  YaxisMotor->setSpeed(YaxisSpd);              // Apply PID speed to motor

}

/***************************************************************************************

The following code was taken from   http://forum.arduino.cc/index.php?topic=41615.20;wap2

to utilize the fast port based encoder logic.  Thank you Lefty!

please go there for a full explanation of how this works.  I have truncated the comments 

here for brevity.

***************************************************************************************/

void doXaxisENC() {                                  // ************** X- AXIS ****************

    if (PIND & 0x04) {                              // test for a low-to-high interrupt on channel A, 

        if ( !(PIND & 0x10)) {                      // check channel B for which way encoder turned, 

           XaxisENCPos = ++XaxisENCPos;               // CW rotation

          }

        else {

           XaxisENCPos = --XaxisENCPos;               // CCW rotation

          }

    }

    else {                                          // it was a high-to-low interrupt on channel A

        if (PIND & 0x10) {                          // check channel B for which way encoder turned, 

           XaxisENCPos = ++XaxisENCPos;               // CW rotation

           }

        else {

           XaxisENCPos = --XaxisENCPos;               // CCW rotation

        }

     }

}                                                   // End of interrupt code for encoder #1

                                                   

void doYaxisENC(){                                  // ************** X- AXIS ****************

  if (PIND & 0x08) {                                // test for a low-to-high interrupt on channel A, 

     if (!(PIND & 0x20)) {                          // check channel B for which way encoder turned, 

      YaxisENCPos = ++YaxisENCPos;                  // CW rotation

     }

     else {

      YaxisENCPos = --YaxisENCPos;                  // CCW rotation

     }

  }

  else {                                            // it was a high-to-low interrupt on channel A

     if (PIND & 0x20) {                             // check channel B for which way encoder turned, 

      YaxisENCPos = ++YaxisENCPos;                  // CW rotation

      }

     else {

      YaxisENCPos = --YaxisENCPos;                  // CCW rotation

     }

  }

}                                                   // End of interrupt code for encoder #2

void doXaxisEndstop() {

  XaxisENCPos=0;                                    // X-Axis Endstop indicates ZERO position 

}

void doYaxisEndstop() {

  YaxisENCPos=0;                                    // Y-Axis Endstop indicates ZERO position 

}

            https://github.com/michaeljball/RepScrap

Updated diagram for reference:

References:

DIYDrones: Tutorial series for new Arduino PID library
http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/
http://robotics.stackexchange.com/questions/1232/how-can-i-use-the-arduino-pid-library-to-drive-a-robot-in-a-straight-line
Tim Wescott's PID without a PHD
http://abigmagnet.blogspot.ca/2008/10/dc-motor-control-part-one.html
http://www.pdx.edu/nanogroup/sites/www.pdx.edu.nanogroup/files/2013_Arduino%20PID%20Lab_0.pdf

https://www.youtube.com/watch?v=ZZYgZjMnGXU
https://www.youtube.com/watch?v=wbmEUi2p-nA
http://blog.solutions-cubed.com/pid-motor-control-with-an-arduino/
http://forum.arduino.cc/index.php/topic,45169.0.html

http://playground.arduino.cc/Code/PIDLibrary
http://playground.arduino.cc/Code/PIDAutotuneLibrary

Arduino Playground: PinChangeInterrupt Library
https://code.google.com/p/arduino-pinchangeint/downloads/list

LetsMakeRobots: PID Control by Big Face
LetsMakeRobots: PID Tutorials for Line Following by Enigmerald
LetsMakeRobots: PID without a PHD by BDK6
LetsMakeRobots: Using Motor Encoders to Control Speed by Oddbot

My Repscrap: DC motors and rotary encoders for Z-Axis too?

 I took a few pictures this morning, to give me something to think about while I was at the cottage for the weekend. 

Pictured here, to the right, is the frame I made up to house the Y-Axis table. You can see the carriage assembly, motor and rail running horizontally in this image.  There are ball bearing slider rails in the "top" and "bottom" sections of the frame that will be attached to the Y-Axis carriage and table.

The frame itself is made from 3 inch extruded Vinyl exterior railing. It is 28" long to accommodate the travel of the Y-Axis table, and it is 16" wide.  I will be putting threaded adjustable feet in each corner for leveling. The ball bearing slider rails fit very snugly inside the extrusion.

I have laid the X-Axis carriage and rail across it, simply to visualize the mounting options and location. 

Now:  On to the good stuff... 

 Pictured here, are two identical paper feed roller assemblies from scavenged Canon inkjet printers. 
They are driven by brushless 12v DC motors, and have a high resolution rotary optical encoder on the end of the hollow paper feed shafts.  

My train of thought is something like this... 

If I were to cut  the hollow shafts short... say 3 inches in length, then support them with bushings, I could cement a threaded rod inside them, and theoretically create a Z-Axis identical in function to many of the existing Reprap stepper motor Z-Axis implementations.  But with 0.08mm resolution.... 

This is RepRrap Prusa implementation using two Z-Axis steppers...










This is just another view of the potential Z-Axis assemblies I have to work with.














I have another couple days until I'm home again to work on them.  I would *love* to get some input and ideas as to how I should/could implement this.


Comments please...

Arduino Sketch to manage high resolution - high speed linear encoders

In the ongoing saga of building my RepStrap 3D printer from salvaged printer parts...

The first axis is sorted out:

This is the printhead carriage salvaged out of a inkjet printer.  It basically consists of a DC gear motor driving a tensioned belt, pulling the print carriage across a high resolution optical encoder strip. 

There have been plenty of people do this before me, but this is my kick at repurposing salvaged printer parts.  

This is the print head circuit board from the carriage.  In the center, you will see the solder pads for the Optical Encoder Sensor.  My original intent was to keep the flat cables intact, and pull the signals off of those, but at about 50mil pitch... I can't even think about soldering that... 

(yeah, I'm getting old)

As they have already done the work of wiring the IR LED with a resistor to VCC, all I really need is four wires.  VCC/GND/Encoder Phase A and B. 

The Optical Encoder provides logic level outputs to the Interrupt pins on the Arduino.  The sketch below only addresses the X-AXIS for demonstration purposes, and only uses one Interrupt on Phase A, while polling the signal level on Phase B.  This provides half the resolution capable from this Quadrature encoder. 

In a future article, we will demonstrate using PinChange Interrupts to get the full resolution from this arrangement.

/***************************************************************************************

*  Lin_Enc_01.ino   04-29-2014   unix_guru at hotmail.com   @unix_guru on twitter

*  http://arduino-pi.blogspot.com

*

*  This sketch allows you to run two salvaged printer carriages for X/Y axis using their 

*  linear encoder strips for tracking. 

*  Both interrupt routines are below, but for the sake of demonstration, I've only set up

*  the X-AXIS for now.

*

*****************************************************************************************/

#include <Wire.h>

#include <Adafruit_MotorShield.h>

#include "utility/Adafruit_PWMServoDriver.h"

#define frontstop = 100            // Right most encoder boundary

#define backstop = 3600            // Left most encoder boundary

// Create the motor shield object with the default I2C address

Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 

// Select which 'port' M1, M2, M3 or M4. In this case, M1

Adafruit_DCMotor *myMotor = AFMS.getMotor(1);

const int encoder1PinA = 2;        // X-AXIS  encoder 1 on pins 2 and 4

const int encoder1PinB = 4;

volatile int encoder1Pos = 0;

const int encoder2PinA = 3;        // Y-AXIS  encoder 2 on pins 3 and 5

const int encoder2PinB = 5;

volatile int encoder2Pos = 0;

boolean CarriageDir = 0;           // Carriage Direction '0' is Right to left

byte spd = 220;                    // Carriage speed from 0-255

int newpos = 0;                    // Taget position for carriage

int posrchd = 1;                   // Flag for target reached

int Pos1, Pos2;

void setup() {

  Serial.begin(115200);

  Serial.println("Linear Encoder Test  04-29-2014");

  AFMS.begin();  // Set up Motors

  

  myMotor->run(BACKWARD);        // Bring carriage to home position. 

  myMotor->setSpeed(spd); 

  delay(100); 

  myMotor->run(FORWARD);        // Bring carriage to home position. 

  myMotor->setSpeed(0); 

  

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

  // attachInterrupt(1, doEncoder2, CHANGE);  // encoder pin on interrupt 1 (pin 3)

  

  randomSeed(analogRead(0));

}

void loop() {

static int oldPos1, oldPos2;

uint8_t oldSREG = SREG;

uint8_t i;

  cli();

  Pos1 = encoder1Pos;  

  Pos2 = encoder2Pos;

  SREG = oldSREG;

  

  if(Pos1 != oldPos1){

     Serial.print("Encoder 1=");

     Serial.println(Pos1,DEC);

     oldPos1 = Pos1;

  }

  if(Pos2 != oldPos2){

     Serial.print("Encoder 2=");

     Serial.println(Pos2,DEC);

     oldPos2 = Pos2;

  }  

  

  //sweep_carriage();

  

  if(posrchd) {                           // If target has been reached clear flag, and get new target

    newpos =  random(200,3500);

    posrchd = 0;

  }    

    

  posrchd = go_to_target(newpos);

  

}

/***************************************************************************************

The following code was taken from   http://forum.arduino.cc/index.php?topic=41615.20;wap2

to utilize the fast port based encoder logic.  Thank you Lefty!

please go there for a full explanation of how this works.  I have truncated the comments 

here for brevity.

***************************************************************************************/

void doEncoder1() {                                  // ************** X- AXIS ****************

    if (PIND & 0x04) {                              // test for a low-to-high interrupt on channel A, 

        if ( !(PIND & 0x10)) {                      // check channel B for which way encoder turned, 

           encoder1Pos = ++encoder1Pos;               // CW rotation

           PORTD = PIND | 0x40;                     // set direction output pin to 1 = forward, 

          }

        else {

           encoder1Pos = --encoder1Pos;               // CCW rotation

           PORTD =PIND & 0xBF;                      // Set direction output pin to 0 = reverse, 

          }

    }

    else {                                          // it was a high-to-low interrupt on channel A

        if (PIND & 0x10) {                          // check channel B for which way encoder turned, 

           encoder1Pos = ++encoder1Pos;               // CW rotation

           PORTD = PIND | 0x40;                     // Set direction output pin to 1 = forward, 

           }

        else {

           encoder1Pos = --encoder1Pos;               // CCW rotation

           PORTD =PIND & 0xBF;                      // Set direction output pin to 0 = reverse, 

           }

         }

    PORTD = PIND | 0x80;                            //  digitalWrite(encoderstep, HIGH);   generate step pulse high

    PORTD = PIND | 0x80;                            //  digitalWrite(encoderstep, HIGH);   add a small delay

    PORTD = PIND & 0x7F;                            //  digitalWrite(encoderstep, LOW);    reset step pulse

}                                                   // End of interrupt code for encoder #1

                                                   

void doEncoder2(){                                  // ************** X- AXIS ****************

  if (PIND & 0x08) {                                // test for a low-to-high interrupt on channel A, 

     if (!(PIND & 0x20)) {                          // check channel B for which way encoder turned, 

      encoder2Pos = ++encoder2Pos;                  // CW rotation

      PORTB = PINB | 0x01;                          // Set direction output pin to 1 = forward, 

     }

     else {

      encoder2Pos = --encoder2Pos;                  // CCW rotation

      PORTD =PIND & 0xFE;                           // Set direction output pin to 0 = reverse, 

     }

  }

  else {                                            // it was a high-to-low interrupt on channel A

     if (PIND & 0x20) {                             // check channel B for which way encoder turned, 

      encoder2Pos = ++encoder2Pos;                  // CW rotation

      PORTB = PINB | 0x01;                          // Set direction output pin to 1 = forward, 

      }

     else {

      encoder2Pos = --encoder2Pos;                  // CCW rotation

      PORTB =PINB & 0xFE;                           // Set direction output pin to 0 = reverse, 

     }

  }

  PORTB = PINB | 0x02;                              // digitalWrite(encoder2step, HIGH);   generate step pulse high

  PORTB = PINB | 0x02;                              // digitalWrite(encoder2step, HIGH);   used to add a small delay

  PORTB = PINB & 0xFD;                              // digitalWrite(encoder2step, LOW);    reset step pulse

}                                                   // End of interrupt code for encoder #2

/***************************************************************************************

go_to_target() determines the distance and direction from current position to target 

position, then maps speed to decellerate close to the target so as not to overshoot.

***************************************************************************************/

int go_to_target(int target)

{

  int temp = 0;

  int delta = abs(Pos1-target);                   // Distance to target

  spd = map(delta,3600,0,255,150);                // Decellerate as you get closer

  if(target < 3600 && target > 100) {

     if(Pos1 < target) {

       myMotor->run(FORWARD);

       myMotor->setSpeed(spd); 

       temp = 0;

     } else if(Pos1 > target) {

       myMotor->run(BACKWARD);

       myMotor->setSpeed(spd); 

       temp = 0;

     }  else temp =1;

  }

  return temp;

}

/***************************************************************************************

sweep_carriage() is just a test routine to track back and forth testing overshoot. 

I will likely remove it soon.

***************************************************************************************/

void sweep_carriage()

{

    if(CarriageDir == 0) {              // Carriage Moving Right to Left

    if (Pos1 < 3600) {      

      myMotor->run(FORWARD);

      if (Pos1 > 3400) { 

       myMotor->setSpeed(spd-80); 

      } else myMotor->setSpeed(spd);  

    } else {      

      myMotor->setSpeed(0);  

      CarriageDir = !CarriageDir;  

    }

  } else {                              // Carriage Moving Left to Right

    if (Pos1 > 100) {

      myMotor->run(BACKWARD);

      if (Pos1 < 300) { 

       myMotor->setSpeed(spd-80); 

      } else myMotor->setSpeed(spd);  

     } else {      

      myMotor->setSpeed(0);  

      CarriageDir = !CarriageDir;  

    }

  }

}

This is the endstop sensor circuit.  I haven't wired it up to the Arduino as yet, but it is pretty straight forward. 

In the true nature of this project, I searched for a used motor controller to go with my Arduino .   Pictured here, is the Adafruit I2C Motor Shield V2 that was on my original robot.  This project will breath new life into it. 

This is a wonderful board in that it contains TWO dual h-bridge FET drivers for four DC motors, or two steppers, or one stepper and two motors... 

Wiring up the Optical Encoder.  Only four wires are needed...

12volt DC gear motor used to drive the carriage assembly.








References:

http://playground.arduino.cc/Main/RotaryEncoders#OnSpeed
http://reprap.org/wiki/Optical_encoders_01
http://mechatronics.mech.northwestern.edu/design_ref/sensors/encoders.html

http://forum.arduino.cc/index.php/topic,17695.0.html

http://www.arduino.cc/en/Reference/PortManipulation

http://capolight.wordpress.com/2010/11/24/making-endstops-from-printer-photo-interrupters/

http://www.instructables.com/id/Improved-Optical-End-Stop/
http://playground.arduino.cc/Main/PinChangeInt
http://reprap.org/wiki/Microstepping_with_optical_feedback