Indexing a gearmotor with an encoder

I am trying to write a code to index a gear-motor (continuous rotation servo) a set number of degrees with an encoder.

board is an Arduino Mega 2560

encoder has following pins A, A’, B, B’, Z, Z’
gear-motor has standard servo connection v+, v-, and white

below is the sequence and goals of the program.

  1. have a set “home” position that will initially align the mechanism when turned on
  2. read state of button, when pressed…
    2a) rotate the servo a set number of degrees (360/13) (13 position indexer)
    2b) stop and hold servo at set position.
    2c) repeat every time button is pressed
  3. have a time limit to reach position so that if there is a jam it will (hopefully) not burn out the servo

Below is what i have so far, a working counter code
I am having trouble understanding how to take that number and translate it into controlling the motor…

I don’t even know if this is the best way to go about doing it?
i don’t even know what i don’t know!!!

/*  Digital Pin 2 accepts external interrupts. Pin1 of a rotary encoder
    is attached to DigitalPin2. An interrupt routine will be called
    when pin1 changes state
    This will be made more efficient with hardware debouncing.
    */
int pin1 = 2;
int pin2 = 3;
int counter;
boolean goingUp = false;
boolean goingDown = false;
void setup()
{
  counter = 0;
  //Serial prints for debugging and testing
  Serial.begin(9600);

/* Setup encoder pins as inputs */
    pinMode(pin1, INPUT); // Pin 2
    pinMode(pin2, INPUT); // Pin 4 

// encoder pin on interrupt 0 (pin 2)
  attachInterrupt(0, decoder, FALLING);

}

void loop()
{
//using while statement to stay in the loop for continuous
//interrupts
while(goingUp==1) // CW motion in the rotary encoder
{
goingUp=0; // Reset the flag
counter ++;
Serial.println(counter);
}

while(goingDown==1) // CCW motion in rotary encoder
{
goingDown=0; // clear the flag
counter --;
Serial.println(counter);
}
}

void decoder()

{
if (digitalRead(pin1) == digitalRead(pin2))
{
goingUp = 1; //if encoder channels are the same, direction is CW
}
else
{
goingDown = 1; //if they are not the same, direction is CCW
}
}
boolean goingUp = false;
boolean goingDown = false;

I got this far and stopped. If the encoder is not going up, is there any possible direction for it to go besides down?

You need ONE boolean. It is true if going up and false if going down.

2a) rotate the servo a set number of degrees (360/13) (13 position indexer)

You can't rotate a continuous rotation not-actually-a-servo a set number of degrees. All you can control is it's speed.

2b) stop and hold servo at set position.

You can stop, but you can't hold it there.

You need ONE boolean. It is true if going up and false if going down. --gotcha, makes sense

You can't rotate a continuous rotation not-actually-a-servo a set number of degrees. All you can control is it's speed. --I am using an separate encoder attached to the "gear motor" to control its position

-I am using an separate encoder attached to the “gear motor” to control its position

So, what is the problem, then? The encoder tells you the position that the motor is at. Stop it when it gets there. Of course, you won’t want to overshoot, or you’ll have to go the other way. Probably several times.

PaulS:

-I am using an separate encoder attached to the "gear motor" to control its position

So, what is the problem, then? The encoder tells you the position that the motor is at. Stop it when it gets there. Of course, you won't want to overshoot, or you'll have to go the other way. Probably several times.

easier said than done. this is my first arduino/programming project

What you want is a PID regulator generating a pwm output to the motor. You control the motor by changing the setpoint.
I have some highly optimized code that does just this. But because of the optimisation it is a bit hard to understand. If you are interested i can show you the code and walk you through the inner working but be prepared to spend some time on doing this.

First you need to make sure you understand how to connect the encoder to the Arduino and how to read it properly, so that you know the orientation of the motor shaft at all times. This is an unusual pinout for an encoder:

encoder has following pins A, A', B, B', Z, Z'

Can you post a link to the data sheet or product page? How have you connected it and what results do you get?

After that part is done, you can just use the Arduino PID library (with appropriate tuning) to set the motor shaft to a desired position. http://playground.arduino.cc/Code/PIDLibrary

Attached is a .PDF of the encoder used

Note: TTL output used

right now i ONLY have pins v+, gnd, A, and B connected to the Arduino running the code i posted earlier

With this setup, the serial monitor shows increasing or decreasing values based on the direction of rotation. starting at zero

-I assume the “Z” pin is for absolute positioning but I could be wrong.

qd787-I.pdf (224 KB)

Z is the index mark and goes high once per revolution.

Your interrupt routine will give you only 1/4 of the theoretical resolution, but if that is high enough, OK. You can use pin change interrupts to get higher resolution, see this post (reply #9 by cattledog) http://forum.arduino.cc/index.php?topic=267426.0

Otherwise, just read up on how the PID library works. Use the desired shaft location (in counts) as "setpoint", the current shaft location as "input" and the output goes to a PWM motor controller. You do need to take care of the sign so that the motor rotates in the proper direction and as stated, tune the PID algorithm. Lots of on line tutorials for that.

Ok so PID it is then.

I was hoping for a simpler solution, as I previously looked into PID control but was a bit overwhelming for a rookie.

might take me awhile but i will figure it out..

thanks for the help

nilton61:
What you want is a PID regulator generating a pwm output to the motor. You control the motor by changing the setpoint.
I have some highly optimized code that does just this. But because of the optimisation it is a bit hard to understand. If you are interested i can show you the code and walk you through the inner working but be prepared to spend some time on doing this.

Please enlighten me professor

ok, lets take a stepwise approach. Here is a very good article on how to write a PID regulator:

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

Don't be scared by the math, the priciple behind a PID is quite simple to grasp and the writer does a very good work in taking it down to earth. My code is based on this article to some extent but i use integer type variables instead of floating point. Be sure to read throug all parts of the article, it will give you a good insight on the inner workings of a PID.

I also use Paul Stoffregens version of the TimerOne library found here: https://github.com/PaulStoffregen/TimerOne

I just discovered that my original code has gone fubar because of several experiments so i will need some time to reconstruct it. So i will be back in a while with some hands on examples.

Here is the code:

#include <TimerOne.h>

const int outPwm = 9;//pin for speed control
const int outDir = 8;//pin for direction control
const int signalPin = 12;//pin for signaling and triggering

//constants for PID calculation
const unsigned long samplePeriod = 400;//time between pid calculations in us
const long kp = 12;//proportional gain
const long ki = 3;//integral gain
const long kd = 2700;//derivative gain
const byte shift = 2;//for shifting/scaling output value
const long outMax = (1024<<shift)-1;//keep max output <=1023
const byte iShift = 11;//right shift for i term
const long iMax = (1024<<(iShift+shift))-1;//keep i term contribution <=1023 and avoid windup
const int stepFrame = 1500;//Number of values for step response


//variables for PID calculation
volatile long iTerm;//summing up integral error
volatile long lastPos;//for calculation of derivative
volatile int output;//output to speedcontrol 
enum {FWD, REW} dir;

//data for position calculation
const char encTable[16] ={0, 1, -1, -0, -1, 0, -0, 1, 1, -0, 0, -1, -0, -1, 1, 0};//gives -1, 0 or 1 depending on encoder movement
byte encState;//remembering the encoder output and acting as index for encTable[]
byte stepState;//for edge detection on step input = D10
byte inp;//for reading encoder pins
byte setInp;//input signals from second encoder
byte setState;//remembering setpoint encoder state
volatile long setPos;//position setpoint, updated by second encoder
volatile long actPos;//actual position from encoder
volatile long error;
volatile long dInput;


void setup(){
  pinMode(outPwm, OUTPUT);
  pinMode(outDir, OUTPUT);
  pinMode(signalPin, OUTPUT);
  Timer1.initialize(samplePeriod);
  Timer1.attachInterrupt(doPID);
  Timer1.pwm(outPwm, 0);
}//setup()

void loop(){
  while(1){
    //aquire input values
    inp = PIND;//read inputs 0..7
    encState = ((encState<<2)|((inp>>2)&3))&15;//use encoder bits and last state to form index
    actPos += encTable[encState];//update actual position on encoder movement
    setState = ((setState<<2)|((inp>>4)&3))&15;//use encoder bits and last state to form index
    setPos += encTable[setState];
  }//while(1)
}//loop()

void doPID(){
  /*Compute all the working error variables*/
  error = setPos-actPos;
  iTerm += (ki*error);
  if(iTerm>iMax) iTerm = iMax;
  else if(iTerm<-iMax) iTerm = -iMax;
  dInput = (actPos-lastPos);
   
  /*Compute PID Output*/
  output = kp*error+(iTerm>>iShift)-kd*dInput;
  if(output < 0){
    dir = REW;
    output = -output;
  }else dir = FWD;
  //if(output<0)
  if(output > outMax) output = outMax;  
  Timer1.setPwmDuty(outPwm, output);
  PORTB |= dir;
  lastPos = actPos;
}//doPID()

The output is fed to a H-bridge controller taking PWM and direction signals. The setpoint is taken from a second encoder so that the motor is following the rotation of the setpoint encoder, something called an electrical axle.

I do not use digitalRead or digitalWrite because these functions are slow. Instead input is taken from the PINx register and output is written to the PORTx register. This has another advantage when reading the encoders because you read all inputs at once.

The handling of the encoders in the loop functions may require some comments.
First, the while(1) is used to speed things up. The program never returns from the loop function which saves the time for the return an renewed call.
All input pins are read at once and stored in the inp variable. The inputs are arranged in such a way that the A and B signals from the encoders occupy to consecutive bits. These bits are then shifted right and masked so that they take values between 0 and 3 depending on the encoder input. This value is stored in the encState variable. On the next iteration this value is left shifted 2 steps and or’ed with the new encoder reading. We now have a value that reflects both the current and the previous reading so it can be used to compute encoder movement. In order to do this the encState variable is used as an index for a lookup table, encTable[16] which hold the values for encoder movement 1, 0 or -1. This value is then added to the actual position. If there is no movement we simply add 0.

nilton

Thanks for taking the time to post the code. However i think you overestimate my programming knowledge...

I have no idea how or were to start modifying your code to meet my application so here are a few questions

-I am confused with this code because of the second encoder, do you have anything a little more simple? -I don't need a h-bridge controller for my application do I? (if not what would the motor control code look like?)

many more to come....

You have to start somewhere… The loop function does read the encoder signals continuously. The doPID function is called by a timer interrupt at the intervall given by the constant samplePeriod in microseconds. This function is esentially the same as in the article with the difference of using inter variables instead of floating point.
The last two assignments in the loop()function are for the second encoder. The variable setPos holds the setpoint position. You can just set this varible anyway you want. The complicated thing in this code is the handling of the encoder signals. But i suggest you give it a try because it will teach you a lot about bit mangling.

You have 2 encoder signals A and B. In this application they are connected to inputs 2 and 3. These correspond to portd bits 2 and 3.
The line inp = PIND; reads all bits of portd at once.
(inp>>2)shifts these bits right so that A now corresponds to bit 0 and B corresponds to bit 1.
(inp>>2)&3) masks off all other bits except these 2. 3 is 11 binary. Bitwise anding means that when you and with 1 the masked bit keeps its value, bitwise anding with 0 resets the bit. So bitwise anding lets the 2 rightmost bits (the encoder input) keep its value and resets all others.
the lvalue of this line is a variable called encState. This variable is used to hold the last reading of the encoder so that we can detect changes in the encoder input. In order to do so the variable must hold both the previous and the current value. To achieve this we use the following two steps.
(encState<<2)This left shifts the previous reading in bits 0 and 1 up to bits 2 and 3. That way they will not be overwritten.
(encState<<2)|((inp>>2)&3)This makes a bitwise or, storing the two bits previously calculated in bits 0 and 1 of the result. now the four leftmost bits hold both the current and the previous reading in this format B’ A’ B A where B’ and A’are the A and B values of the previous reading. The reading before the previous will now be in bits 4 and 5 because of the left shift. We do not want these so we are doing a final and with 15 (0b111) to reset all bits except the 4 rightmost.
((encState<<2)|((inp>>2)&3))&15Finally this is stored in the encState variable.

encState = ((encState<<2)|((inp>>2)&3))&15;

Lets give an example. Suppose the last encoder reading was 00 and the current one is 00. This means that the encoder has not moved since the last reading. The value in encState will be 0b0000 or simply 0. This value is used as an index for the encTable array which holds 0 for index 0
If the encoder moves one step clockwise encState would have the value 0b0001 or 1 . encTable[1] is 1 indicating this movement. if the movement was counterclockwise encState would be 0b0010 or 2 and encTable[2] is -1 indicating this movement. All these incremental movements are added in the actPos variable

actPos += encTable[encState]

nilton61: You have to start somewhere..

was hoping my application would be a bit simpler baby step... Although I am following you on SOME things (thanks for the detailed explanations they are helping). I don't feel any more confident about being able to implement things from your example into my project.

Is it possible for you to provide a modified code more similar to what i need in order to accomplish the objectives of my code

1) have a set "home" position that will initially align the mechanism when turned on 2) read state of button, when pressed.... 2a) rotate the servo a set number of degrees (360/13) (13 position indexer) 2b) stop and hold servo at set position. 2c) repeat every time button is pressed 3) have a time limit to reach position so that if there is a jam it will (hopefully) not burn out the servo

I am the type of person that learns from reverse engineering things, so if i can find an example similar to what i need, i feel that I Have a better shot at modifying it to fit my needs.

It is not clear from your description if the motor needs to be reversed. There are a number of ways you can control a (DC)motor. For one direction you can use these

  • A relay that just makes and breaks the motor current
  • A changeover relay that short-circuits the motor when not running. This gives you much faster stop times
  • A transistor, either bipolar or CMOS that has its emitter/source connected to ground and collector/drain connected to the lower terminal of the motor and the high terminal of the motor connected to V+. You need a freewheeling diode for this.
  • A half bridge driver The last two solid state solutions let you control the motor speed by pwm. These need just one control signal for on/off. When using a relay put a snubber circuit over the contacts and they will last much longer.

If you need motor reversal you have these options

  • Two relays, on for forward one for reverse. These can be wired for either free running or short circuited motor
  • A full H-bridge

Both these need two signals. In the relay case one for forward and one for reverse. If you have a h-bridge you can either control each half of the bridge or have some glue-logic (internal or external) so that you can use PWM and direction signals.

As for the software the simplest solution would be turning the motor of a number of encoder pulses before it reaches its position. A short circuited motor stop is to prefer in this case. The number of pulses has to be experimented. So the total effort for these simpler solutions is not much less than going for a PID/PWM one which is much more general

Only politicians, priest and salespeople will try to make you believe that there is a simple solution to a complex problem

I only need the motor to go one way (at least for now)

nilton61: As for the software the simplest solution would be turning the motor of a number of encoder pulses before it reaches its position. A short circuited motor stop is to prefer in this case.

I believe this is the method i want to use.

-do you have any example code i can use and modify? -can you walk me through writing a codes using this method?

in the mean time i will wire my motor for the short circuit stop.

It seems to me that you already have most of the code for that. Just compare the counter to the value an turn off the motor

nilton61: A transistor, either bipolar or CMOS that has its emitter/source connected to ground and collector/drain connected to the lower terminal of the motor and the high terminal of the motor connected to V+. You need a freewheeling diode for this.

This is the method i will be using. I have a bunch of transistors laying around. This is my first Arduino/coding project, But not my first electronics project.

[/quote]

nilton61: It seems to me that you already have most of the code for that.

I agree, Which is I was looking for a simplier solution.

I got frustrated looking for examples on how to do that, And made this thread