Controlling servos with rotary encoders

Projects General Guidance
1

Hi,

I purchased an Arduino Uno R3, the tilt/pan assembly with micro servos, the motor shield V2.3 and 2 rotary encoders with the aim of being able to control the tilt/pan servos with the encoders.
I've hooked everything up and tested the servos using the swing sketch and they work fine. I haven't tested the encoders because that's kind of where I'm stuck.
I got hold of the code for a rotary encoder volume control here:

I butchered this code to include another encoder and then make the changes move the servos... however my code doesn't seem to work.
When I upload it, the servos move to there central positions and the only thing which happens is if I push an encoder button one servo will go all the way to the 0 or 180 position and keep trying to move past that point till I reset the board.
Here is my butchered code:

#include <Servo.h>
Servo servo1;
Servo servo2;
#define PIN_ENCODER1_A      2
#define PIN_ENCODER2_A      3
#define PIN_ENCODER1_B      4
#define PIN_ENCODER2_B      5
#define PIN_ENCODER1_SWITCH 6
#define PIN_ENCODER2_SWITCH 7
#define TRINKET_PINx       PIND //wtf is this...

 
static uint8_t enc1_prev_pos   = 0;
static uint8_t enc1_flags      = 0;
static char    sw1_was_pressed = 0;
static uint8_t enc2_prev_pos   = 0;
static uint8_t enc2_flags      = 0;
static char    sw2_was_pressed = 0;
int pos1 = 0;
int pos2 = 0;
 
void setup()
{
  servo1.attach(10);
  servo2.attach(9);
  // set pins as input with internal pull-up resistors enabled
  pinMode(PIN_ENCODER1_A, INPUT_PULLUP);
  pinMode(PIN_ENCODER2_A, INPUT_PULLUP);
  pinMode(PIN_ENCODER1_B, INPUT_PULLUP);
  pinMode(PIN_ENCODER2_B, INPUT_PULLUP);
  pinMode(PIN_ENCODER1_SWITCH, INPUT_PULLUP);
  pinMode(PIN_ENCODER2_SWITCH, INPUT_PULLUP);
 
  // get an initial reading on the encoder pins
  if (digitalRead(PIN_ENCODER1_A) == LOW) {
    enc1_prev_pos |= (1 << 0);
  }
  if (digitalRead(PIN_ENCODER1_B) == LOW) {
    enc1_prev_pos |= (1 << 1);
  }
   if (digitalRead(PIN_ENCODER2_A) == LOW) {
    enc2_prev_pos |= (1 << 0);
  }
  if (digitalRead(PIN_ENCODER2_B) == LOW) {
    enc2_prev_pos |= (1 << 1);
  }
}
 
void loop()
{
  int8_t enc1_action = 0; // 1 or -1 if moved, sign is direction
  int8_t enc2_action = 0;
 
  // note: for better performance, the code will use
  // direct port access techniques
  // http://www.arduino.cc/en/Reference/PortManipulation
  uint8_t enc1_cur_pos = 0;
  uint8_t enc2_cur_pos = 0;
  // read in the encoder state first
  if (bit_is_clear(TRINKET_PINx, PIN_ENCODER1_A)) {
    enc1_cur_pos |= (1 << 0);
  }
  if (bit_is_clear(TRINKET_PINx, PIN_ENCODER1_B)) {
    enc1_cur_pos |= (1 << 1);
  }
  if (bit_is_clear(TRINKET_PINx, PIN_ENCODER2_A)) {
    enc2_cur_pos |= (1 << 0);
  }
  if (bit_is_clear(TRINKET_PINx, PIN_ENCODER2_B)) {
    enc2_cur_pos |= (1 << 1);
  }
 
  // if any rotation at all
  if (enc1_cur_pos != enc1_prev_pos)
  {
    if (enc1_prev_pos == 0x00)
    {
      // this is the first edge
      if (enc1_cur_pos == 0x01) {
        enc1_flags |= (1 << 0);
      }
      else if (enc1_cur_pos == 0x02) {
        enc1_flags |= (1 << 1);
      }
    }
  }
 if (enc2_cur_pos != enc2_prev_pos)
  {
    if (enc2_prev_pos == 0x00)
    {
      // this is the first edge
      if (enc2_cur_pos == 0x01) {
        enc2_flags |= (1 << 0);
      }
      else if (enc2_cur_pos == 0x02) {
        enc2_flags |= (1 << 1);
      }
    }
  }
    if (enc1_cur_pos == 0x03)
    {
      // this is when the encoder is in the middle of a "step"
      enc1_flags |= (1 << 4);
    }
    else if (enc1_cur_pos == 0x00)
    {
      // this is the final edge
      if (enc1_prev_pos == 0x02) {
        enc1_flags |= (1 << 2);
      }
      else if (enc1_prev_pos == 0x01) {
        enc1_flags |= (1 << 3);
      }
    }
 if (enc2_cur_pos == 0x03)
    {
      // this is when the encoder is in the middle of a "step"
      enc2_flags |= (1 << 4);
    }
    else if (enc2_cur_pos == 0x00)
    {
      // this is the final edge
      if (enc2_prev_pos == 0x02) {
        enc2_flags |= (1 << 2);
      }
      else if (enc2_prev_pos == 0x01) {
        enc2_flags |= (1 << 3);
      }
    }
      // check the first and last edge
      // or maybe one edge is missing, if missing then require the middle state
      // this will reject bounces and false movements
      if (bit_is_set(enc1_flags, 0) && (bit_is_set(enc1_flags, 2) || bit_is_set(enc1_flags, 4))) {
        enc1_action = 1;
      }
      else if (bit_is_set(enc1_flags, 2) && (bit_is_set(enc1_flags, 0) || bit_is_set(enc1_flags, 4))) {
        enc1_action = 1;
      }
      else if (bit_is_set(enc1_flags, 1) && (bit_is_set(enc1_flags, 3) || bit_is_set(enc1_flags, 4))) {
        enc1_action = -1;
      }
      else if (bit_is_set(enc1_flags, 3) && (bit_is_set(enc1_flags, 1) || bit_is_set(enc1_flags, 4))) {
        enc1_action = -1;
      }
      if (bit_is_set(enc2_flags, 0) && (bit_is_set(enc2_flags, 2) || bit_is_set(enc2_flags, 4))) {
        enc2_action = 1;
      }
      else if (bit_is_set(enc2_flags, 2) && (bit_is_set(enc2_flags, 0) || bit_is_set(enc2_flags, 4))) {
        enc2_action = 1;
      }
      else if (bit_is_set(enc2_flags, 1) && (bit_is_set(enc2_flags, 3) || bit_is_set(enc2_flags, 4))) {
        enc2_action = -1;
      }
      else if (bit_is_set(enc2_flags, 3) && (bit_is_set(enc2_flags, 1) || bit_is_set(enc2_flags, 4))) {
        enc2_action = -1;
      }
      enc1_flags = 0; // reset for next time
      enc2_flags = 0; // reset for next time
    
  
 
  enc1_prev_pos = enc1_cur_pos;
  enc2_prev_pos = enc2_cur_pos;
 
  if (enc1_action > 0) {
    pos1 += 1;
    servo1.write(pos1); // Clockwise
  }
  else if (enc1_action < 0) {
    pos1 -= 1;
    servo1.write(pos1); // Counterclockwise,
  }
  if (enc2_action > 0) {
     pos2 += 1;
     servo1.write(pos2);  // Clockwise
  }
  else if (enc2_action < 0) {
     pos2 -= 1;
     servo1.write(pos2); // Counterclockwise
  }
  // remember that the switch is active low 
  if (bit_is_clear(TRINKET_PINx, PIN_ENCODER1_SWITCH)) 
  {
    if (sw1_was_pressed == 0) // only on initial press, so the keystroke is not repeated while the button is held down
    {
      pos1=0; // Encoder pushed down, toggle mute or not
      servo1.write(pos1);
      delay(5); // debounce delay
    }
    sw1_was_pressed = 1;
  }
  else
  {
    if (sw1_was_pressed != 0) {
      delay(5); // debounce delay
    }
    sw1_was_pressed = 0;
  }
  if (bit_is_clear(TRINKET_PINx, PIN_ENCODER2_SWITCH)) 
  {
    if (sw2_was_pressed == 0) // only on initial press, so the keystroke is not repeated while the button is held down
    {
      pos2=0; // Encoder pushed down, toggle mute or not
      servo1.write(pos2);
      delay(5); // debounce delay
    }
    sw2_was_pressed = 1;
  }
  else
  {
    if (sw2_was_pressed != 0) {
      delay(5); // debounce delay
    }
    sw2_was_pressed = 0;
  }
 
 
  }

I'm very useless at coding so I'm sure there are errors in there, probably in the way I duplicated the nested if and else statements. I don't know how to think about how it could be wrong yet without example so I hope someone can help.

Cheers,

Tim

2

Hi Bored,

I suggest you try to get the encoder code working separately from the servo code.

Create a sketch that just processes the encoders and prints their values to the screen to make sure you've got them working.

Then try to combine the two. It will be much easier to debug this way.

Let us know how it goes!

3

Hi Pat,

I kind of gave up on this piece of code because direct port manipulation is above my head currently and I still haven't figured out how to flow chart the logic so I can see if it's all tight...
What I ended up doing was getting an encoder library and using that to make two instances and just call them over and over.
It's neat code, much smaller than what's posted here.

#include <Encoder.h>
#include <Servo.h> 
Servo servo1;
Servo servo2;

Encoder knobLeft(2, 4);
Encoder knobRight(3, 5);

void setup() {
  Serial.begin(9600);
  Serial.println("TwoKnobs Encoder Test:");
  servo1.attach(10);
  servo2.attach(9);
}

long positionLeft  = -999;
long positionRight = -999;

void loop() {
  long newLeft, newRight;
  newLeft = knobLeft.read() / 4;
  newRight = knobRight.read() / 4;
  if (newLeft != positionLeft || newRight != positionRight) {
    Serial.print("Left = ");
    Serial.print(newLeft);
    Serial.print(", Right = ");
    Serial.print(newRight);
    Serial.println();
    positionLeft = newLeft;
    positionRight = newRight;
    servo1.write(newLeft);
    servo2.write(newRight);
  }
  // if a character is sent from the serial monitor,
  // reset both back to zero.
  if (Serial.available()) {
    Serial.read();
    Serial.println("Reset both knobs to zero");
    knobLeft.write(0);
    knobRight.write(0);
  }
}

The code has serial monitoring in it, I pinched it from the example included with the encoder library.

The next step for me is to figure interrupts and write and code which handles the encoders...

The purpose for the code is to control a tilt pan assembly with a laser on it so when two of these assemblies are used from known positions the geometry of an object can be mapped in 3D space.
So I built this using servos and it's not accurate enough, I intended to use an IMU for an inclinometer which I still want to do but I can't get a reading for the panning axis accurately. So I'm going to use geared steppers with absolute encoders attached.

It's a work in progress and one I'm enjoying.
Thanks for taking the time to reply.

Cheers,

Tim