Pixybot Construction Guide

20150311_131419This page will be an instructable about how to make the Pixybots. This may be a slightly different form than most until I roll most into a Make instructable.

 

There is also a main project page for this robot.

Pixybots

 

Below I have the .STL files for the 3D printed parts in a link and viewer. Feel free to use and change the files however you want.

Pixybot Camera/arduino support

Pixybot wheel

Before beginning construction, try and have all the tools you need, including access to a 3D printer. I ordered most components online, although you may find a few deals at local stores for the Arduino or plastic.

Building the rolling chassis:

I always like to start by getting a rolling chassis with most of my robots.  First, cut the acrylic base plate to size. Next, get the camera support printed. Drill out the Lego wheels, if those are the type you will be using. Drilling out the hub on these wheels with a large enough bit that a servo horn screw can fit through the hole. Then mix up a small batch of slow dry 2 part epoxy and affix the servo horns to the wheels. Set the wheels aside so they can dry. Proceed to solder the tail wheel assembly together and make sure that it is at the correct height so when you mount it, the base will sit parallel to the surface of the table/bench. Next, drill holes to a fix the camera support to the base plate leaving  3 cm or so in the front. Also attach the tail wheel in the same fashion to the back of the chassis plate. I used servo tape to attach the servos to the base plate just behind the screws holding the camera support to the bottom of the chassis. Attach wheels once they have dried overnight and use a screw to secure the wheel to the servo horn.

Building or buying a battery:

I for this project built 3 7.2volt 2800mAh Li-ion packs for the robots. I purchased the cells though Tenergy. The cells I used look like they are unavailable so I linked some 2600mAh equivalent versions. I selected the ones with solderable tabs. I soldered the two cells in series and added a polarized quick disconnect to the cell. I then covered the pack in a protective blue heat shrink. Now a word of warning I did not install a protective circuit on the pack though I would still recommend doing so. If you are not comfortable with creating a pack you can buy rather cheap RC car packs that will do the job. Also be sure that no matter what route you take that you have a proper li-ion charger for the batteries.  Once you have the battery go ahead and charge it up.

A li-po battery 2200mAh battery pre-made

Mount the electronics:

I used a few RC car hex head screws to mount the Arduino and Pixy. You can also use the hardware that came with the Pixy to mount it. Be sure that if you are using 3D printed parts that you predrill the mounting holes out. Mount the so that the Pixy is facing forward and that Arduino’s programming port is easy to get to.

Solder the shield:

I have added quite a few features with only a few components. I would suggest getting a button to act as a pause button. You will need a resistor for connecting the button to a open digital I/O port. For the servos you will need to supply power via the battery positive and negative. Then connect the control pin to a PWM port on the Arduino. Now I used a semi bad practice, as the servos are running off the battery and not though a voltage regulator or a DC to DC converter. If you are able try and supply the servos with a separate supply of 5-6 volts. I have linked  a few sites below to guide you with adding items to the shield.

Add a button for pausing the servos

You will need a push button and a 10k ohm resistor

Add a voltage divider for sensing voltage greater then 5volts

You will need 2 100k ohm resistors or 1 100k ohm and 1 1Meg ohm resistor

Troubleshooting:

Below are some common issues you may find when you have a robot up and running.

Q: My robot is avoiding the ball instead of following it.

A:Try swapping around the left and right drive servos they may be reversed. Also check the PID loop in the program.

 

Q: The robot is jerky or acting sporadic

A: Try resetting the Arduino, and retraining the Pixy for the color. Attempt using a different color like orange for the robot to track.

 

Q: My robot seem sluggish and or is losing the tracked object fast

A: Make sure the battery pack is charged and there is ample lighting.

 

Q: My robot just spins after I first turned it on

A: Verify the connection from the Pixy to the Arduino and If your like me I always forget to take off the protective lens cap.

 

Q: My robot is oscillating violently left and right as it tracks the ball

A: Try increasing the proportional error in the FollowBlock sub routine. This line of code “int32_t differential = (followError + (followError * forwardSpeed))>>6;”

Below is the Arduino code in the latest build for the robot. This will work best when paired with a Pixy with up to date firmware. The code has a few commented items out including picking up a ball. I have yet to finish those sections. Also, I will be adding a few more features to it before I’m happy. I have added voltage monitoring, a pause button and IR sersor code to the latest program.

// ArchReactor Pixybot
// Written by: Apollo W. Timbers
// 2015
//
// Code revision mark VI
//
// This code is a intigration of the Pixy vision system into a mobile platform to raise awerness of the maker space
// know as Arch Reactor.
// Sections of the code include following tracked objects, IR sensor object detecting, A PID control loop,
// A low battery warning/cuttoff. and pausing the drive servos so you can train the Pixy.
//
// Portions of this code are derived from Adafruit Industries Pixy Pet code.
// Portions of this code are derived from the Pixy CMUcam5 pantilt example code.
//

#include <SPI.h>
#include <Pixy.h>
#include <Servo.h>

#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)

// number of analog samples to take per reading
#define NUM_SAMPLES 10

// constants won't change. They're used here to
// set pin numbers:
int sum = 0; // sum of samples taken
unsigned char sample_count = 0; // current sample number
const int buttonPin = 2; // the number of the pushbutton pin
const int ledPin = 13; // the pin number of the LED pin
int ledgreen = 5; // the pin number of the LED pin
int ledred = 7; // the pin number of the LED pin
// variables will change:
int buttonState = 0; // variable for reading the pushbutton status
Servo leftServo; // Define the Servos
Servo rightServo;
//Servo gripperServo;
int analogInput = A0;
unsigned int raw;
double vcc = 0;
double voltage; // calculated voltage
const int irSenseleft = A1; // Connect sensor to analog pin A0
int distanceleft = 0;
const int irSenseright = A2; // Connect sensor to analog pin A0
int distanceright = 0;
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(200, 200);
ServoLoop tiltLoop(150, 200);

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");
 leftServo.attach(9); // servo on digital pin 9
 rightServo.attach(10); // servo on digital pin 10
 // gripperServo.attach(11); // servo on digital pin 11
 pixy.init();
 pinMode(ledPin, OUTPUT);
 // initialize the pushbutton pin as an input:
 pinMode(buttonPin, INPUT);
 pinMode(ledgreen, OUTPUT);
 pinMode(ledred, OUTPUT);
 digitalWrite(ledred, LOW);
 digitalWrite(ledgreen, LOW);
 //analogReference(INTERNAL);
 delay (500);
 Serial.print("Checking internal voltage...\n");
 readVcc(); // Check initial voltage
 Voltage();
}

uint32_t hold = 0;
int32_t size = 400;
uint32_t lastBlockTime = 0;
//---------------------------------------
// Main loop - runs continuously after setup
//---------------------------------------
void loop()
{
 static unsigned long timer=millis(); // initialize timer variable and set to millisecond timer
 static unsigned long timerA=millis(); // initialize timer variable and set to millisecond timer
 static int i = 0;
 int j;
 uint16_t blocks;
 char buf[32];
 int32_t panError, tiltError;
 blocks = pixy.getBlocks();
 // If we have blocks in sight, track and follow them
 if (blocks)
 {
 int trackedBlock = TrackBlock(blocks);
 FollowBlock(trackedBlock);
 lastBlockTime = millis();
 size += pixy.blocks[trackedBlock].width * pixy.blocks[trackedBlock].height;
 size -= size >> 3;
 if (size >= 500);
 // Grip();
 // if (millis() - hold > random(3000, 7000))
 // Drop();
 }
 else if (millis() - lastBlockTime > 3000)
 {
 leftServo.writeMicroseconds(1510);
 rightServo.writeMicroseconds(1510);
 ScanForBlocks();
 }
 if(millis()-timer>2999) // if at least 3000mS have passed
 {
 timer=millis(); // reset timer
 readVcc();
 Voltage();
 }
 if(millis()-timerA>49) //Wait 50 ms between each read for IR
 // According to datasheet time between each read
 // is -38ms +/- 10ms. Waiting 50 ms assures each
 // read is from a different sample
 {
 timerA=millis(); // reset timer
 Pausebutton();
 irRead();
 }
 if (voltage >= 6){
 digitalWrite(ledgreen, HIGH);
 }
 else if ((voltage >= 5) && (voltage < 6))
 {
 digitalWrite(ledgreen, LOW);
 digitalWrite(ledred, HIGH);
 }
 else if (voltage < 5)
 {
 leftServo.writeMicroseconds(1510); // Hold motors because of low poer state
 rightServo.writeMicroseconds(1510);
 digitalWrite(ledgreen, LOW);
 digitalWrite(ledred, HIGH); // LED to warn of low battery
 delay (500);
 digitalWrite(ledred, LOW);
 delay (500);
 }

}

int oldX, oldY, oldSignature;

//---------------------------------------
// Track blocks via the Pixy pan/tilt mech
// (based in part on Pixy CMUcam5 pantilt example)
//---------------------------------------
int TrackBlock(int blockCount)
{
 int trackedBlock = 0;
 long maxSize = 0;
 uint32_t hold = 0;

 Serial.print("blocks =");
 Serial.println(blockCount);

 for (int i = 0; i < blockCount; i++)
 {
 if ((oldSignature == 0) || (pixy.blocks[i].signature == oldSignature))
 {
 long newSize = pixy.blocks[i].height * pixy.blocks[i].width;
 if (newSize > maxSize)
 {
 trackedBlock = i;
 maxSize = newSize;
 }
 }
 }

 int32_t panError = X_CENTER - pixy.blocks[trackedBlock].x;
 int32_t tiltError = pixy.blocks[trackedBlock].y - Y_CENTER;

 panLoop.update(panError);
 tiltLoop.update(tiltError);

 pixy.setServos(panLoop.m_pos, tiltLoop.m_pos);

 oldX = pixy.blocks[trackedBlock].x;
 oldY = pixy.blocks[trackedBlock].y;
 oldSignature = pixy.blocks[trackedBlock].signature;
 return trackedBlock;
}

void FollowBlock(int trackedBlock)
{
 int32_t followError = pixy.blocks[0].x-X_CENTER; // How far off-center are we looking now?

 // Size is the area of the object.
 // We keep a running average of the last 8.
 size += pixy.blocks[trackedBlock].width * pixy.blocks[trackedBlock].height;
 size -= size >> 3;

 // Forward speed decreases as we approach the object (size is larger)
 int forwardSpeed = constrain(400 - (size/400), -100, 400); 

 // Steering differential is proportional to the error times the forward speed
 int32_t differential = (followError + (followError * forwardSpeed))>>6;

 // Adjust the left and right speeds by the steering differential.
 int leftSpeed = constrain(forwardSpeed + differential, -400, 400);
 int rightSpeed = constrain(forwardSpeed - differential, -400, 400);

leftSpeed = map(leftSpeed,-400,400,1650,1350); // Map to servo output values
rightSpeed = map(rightSpeed,-400,400,1350,1650); // Map to servo output values 

 // Update servos
 leftServo.writeMicroseconds(leftSpeed);
 rightServo.writeMicroseconds(rightSpeed);
}

int scanIncrement = (RCS_MAX_POS - RCS_MIN_POS) / 150;
uint32_t lastMove = 0;
//---------------------------------------
// Random search for blocks
//
// This code rotates slowly in a circle
// until a block is detected
//---------------------------------------
void ScanForBlocks()
{
 if (millis() - lastMove > 10)
 {
 lastMove = millis();
 //panLoop.m_pos += scanIncrement;
 //if ((panLoop.m_pos >= RCS_MAX_POS)||(panLoop.m_pos <= RCS_MIN_POS))
 //{
 tiltLoop.m_pos =(RCS_MAX_POS * 0.6);
 //scanIncrement = -scanIncrement;
 //if (scanIncrement < 0)

 leftServo.writeMicroseconds(1460);
 rightServo.writeMicroseconds(1460);
 delay (20);

 pixy.setServos(panLoop.m_pos, tiltLoop.m_pos);
 }
} 

// Take multiple readings, and average them out to reduce false readings
void irRead()
{
 int averagingleft = 0; // Holds value to average readings
 int averagingright = 0; // Holds value to average readings

 // Get a sampling of 5 readings from sensor
 //for (int i=0; i<3; i++)
 {
 distanceleft = analogRead(irSenseleft);
 averagingleft = averagingleft + distanceleft;
 distanceright = analogRead(irSenseright);
 averagingright = averagingright + distanceright;
 }
 distanceleft = averagingleft / 5; // Average out readings
 //return(distanceleft); // Return value
 Serial.println(distanceleft, DEC);
 distanceright = averagingright / 5; // Average out readings
 //return(distanceright); // Return value
 Serial.println(distanceright, DEC);
 if (distanceleft > 85)
 {
 leftServo.writeMicroseconds(1510); //allstop
 rightServo.writeMicroseconds(1510);
 delay(200);
 leftServo.writeMicroseconds(1660); //backup
 rightServo.writeMicroseconds(1460);
 delay(500);
 leftServo.writeMicroseconds(1460); //spin
 rightServo.writeMicroseconds(1460);
 delay(300);

 }
 else if (distanceright > 85)
 {
 //allstop
 leftServo.writeMicroseconds(1510);
 rightServo.writeMicroseconds(1510);
 delay (200);
 }
}

void Voltage()
{
 vcc = readVcc()/1000.0;
 raw = analogRead(analogInput);
 voltage = ((raw / 1023.0) * vcc) * 2;
 Serial.print ("Battery Voltage = ");
 Serial.println(voltage, DEC);
 Serial.print ("VCC Voltage = ");
 Serial.println(vcc, DEC );
}

long readVcc() {
 long result;
 // Read 1.1V reference against AVcc
 ADMUX = _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
 delay(2); // Wait for Vref to settle
 ADCSRA |= _BV(ADSC); // Convert
 while (bit_is_set(ADCSRA,ADSC));
 result = ADCL;
 result |= ADCH<<8;
 result = 1106400L / result; // Back-calculate AVcc in mV
 return result;
}

void Pausebutton()
{
 sample_count = 0;
 sum = 0;
 buttonState = digitalRead(buttonPin);

 // check if the pushbutton is pressed.
 // if it is, the buttonState is HIGH:
 if (buttonState == HIGH) {
 // turn LED on:
 leftServo.writeMicroseconds(1510);
 rightServo.writeMicroseconds(1510);
 delay(18000);
 }
}

//byte haveball = 0;
//void Grip()
//{
// hold = millis();
 // leftServo.writeMicroseconds(1505);
 // rightServo.writeMicroseconds(1500);
 // delay(50);
 // leftServo.writeMicroseconds(1660);
 // rightServo.writeMicroseconds(1460);
 //delay(200);
 // leftServo.writeMicroseconds(1660);
 // delay(50);
 // haveball = 1;
//}

//void Drop()
//{

// leftServo.writeMicroseconds(1405);
// rightServo.writeMicroseconds(1600);
// delay(200);
// leftServo.writeMicroseconds(1460);
// rightServo.writeMicroseconds(1460);
// delay(200);
// leftServo.writeMicroseconds(160);
// delay(50);
// haveball = 0;
//}

Creative Commons License
This work is licensed under a Creative Commons Attribution-NonCommercial 4.0 International License.