<< return to Pixycam.com

Code to center servos

Pixy Software
#1

I’m using a Pixy on a different 2-axis gimbal than the demo kit, and I’m running my code on an Arduino Nano v3.1 board. I’ve modified the pantilt demo to work with my hardware, but would now like to add some code to “center” the gimbal on a set of known RCS position values (200, 570). I’d like to do this in two places in the code, at startup (e.g. center on power-up, then begin tracking after a short delay), and a poor man’s interrupt, using an SPST switch to toggle an input pin on the Arduino. The “in loop” interrupt would let me center the camera so I could train Pixy on a new color for Signature #1 and not have to fight with the servos swinging around at random. The switch would be flipped back to the “track” position, and the loop would start following the newly programmed color.

(Project overview: The gimbal also has a USB webcam attached to it. I intend to train Pixy on my child’s shirt/pants color and let the Pixy/webcam gimbal rig follow them around the living room, while Grandma & Grandpa watch/interact over Skype. The center-on-demand switch would let me hold the little one up and teach Pixy the current color to track)

I tried to brute-force the centering function, but was only able to make it work inside the loop() function; I simply commented out “pixy.setServos(panLoop.m_pos, tiltLoop.m_pos)” and added “pixy.setServos(200, 570)” in its place. That proved the concept, I just need to code up an if statement to check the state of the digital input pin wired to the switch. However, I tried the same thing inside the setup() function to perform a center-on-startup, but the servos didn’t seem to respond, even after adding a long (10 sec) delay after the setServos(200,570) call.

Any suggestions?
Thanks!

#2

Where in the setup function did you try to set the servos? Was it after @pixy.init()@?

Scott

#3

Somewhere along the line I must have accidentally deleted the pixy.init() call in the setup routine. I’ll confirm and see if that fixes the issue (guess that’s what I get for tinkering at midnight!)

Scott H

#4

I went back and looked at the example for pantilt, and it does not call out pixy.init() in the setup function, just serial.begin(9600) and serial.print(“Starting…”). The first function to call to pixy is in loop(), and that is blocks = pixy.getBlocks(). I tried adding pixy.init() and .Init(), but the compiler returns the error…

96: error: 'class Pixy' has no member named 'init'

Here’s the example file that came along with my recently downloaded copy of arduino_pixy-x.x.x.zip. It works with minor modifications (reduce the loop gain variables to ~100 and reverse the subtraction on the y axis, as my tilt servo is reversed relative to the pantilt demo unit).

//
// begin license header
//
// This file is part of Pixy CMUcam5 or "Pixy" for short
//
// All Pixy source code is provided under the terms of the
// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
// Those wishing to use Pixy source code, software and/or
// technologies under different licensing terms should contact us at
// [email protected]. Such licensing terms are available for
// all portions of the Pixy codebase presented here.
//
// end license header
//

#include   
#include 

#define X_CENTER    160L
#define Y_CENTER    100L
#define RCS_MIN_POS     0L
#define RCS_MAX_POS     1000L
#define RCS_CENTER_POS	((RCS_MAX_POS-RCS_MIN_POS)/2)

class ServoLoop
{
public:
  ServoLoop(int32_t pgain, int32_t dgain);

  void update(int32_t error);
   
  int32_t m_pos;
  int32_t m_prevError;
  int32_t m_pgain;
  int32_t m_dgain;
};


ServoLoop panLoop(500, 800);
ServoLoop tiltLoop(700, 900);

ServoLoop::ServoLoop(int32_t pgain, int32_t dgain)
{
  m_pos = RCS_CENTER_POS;
  m_pgain = pgain;
  m_dgain = dgain;
  m_prevError = 0x80000000L;
}

void ServoLoop::update(int32_t error)
{
  long int vel;
  char buf[32];
  if (m_prevError!=0x80000000)
  {	
    vel = (error*m_pgain + (error - m_prevError)*m_dgain)>>10;
    //sprintf(buf, "%ld\n", vel);
    //Serial.print(buf);
    m_pos += vel;
    if (m_pos>RCS_MAX_POS) 
      m_pos = RCS_MAX_POS; 
    else if (m_pos<RCS_MIN_POS) 
      m_pos = RCS_MIN_POS;

    //cprintf("%d %d %d\n", m_axis, m_pos, vel);
  }
  m_prevError = error;
}


Pixy pixy;

void setup()
{
  Serial.begin(9600);
  Serial.print("Starting...\n");
}

void loop()
{ 
  static int i = 0;
  int j;
  uint16_t blocks;
  char buf[32]; 
  int32_t panError, tiltError;
  
  blocks = pixy.getBlocks();
  
  if (blocks)
  {
    panError = X_CENTER-pixy.blocks[0].x;
    tiltError = pixy.blocks[0].y-Y_CENTER;
    
    panLoop.update(panError);
    tiltLoop.update(tiltError);
    
    pixy.setServos(panLoop.m_pos, tiltLoop.m_pos);
    
    i++;
    
    if (i%50==0)
    {
      sprintf(buf, "Detected %d:\n", blocks);
      Serial.print(buf);
      for (j=0; j<blocks; j++)
      {
        sprintf(buf, "  block %d: ", j);
        Serial.print(buf); 
        pixy.blocks[j].print();
      }
    }
  }  
}

I’ve been troubleshooting some servo issues and built a simple for loop to run the servos from RCS_MIN_POS to MAX_POS and back. It only likes to work if I call pixy.getBlocks right before I issue the setServo function, each time through the for loops.

Can you explain why that’s the case?

Thanks!
Scott

#5

Hmm I wonder if it has to do with a timing problem. I’ll poke around and see what I can find.

Scott

#6

Hi Scott H. Could you make sure you have the latest arduino-pixy library from CMUCAM. that may be the reason why you get the error when you include pixy.init(); I was having a similar issue earlier. Hope it helps.