Intro to CMUcam5 Pixy Vision Camera Sensor Part 2 – Creating a Ball Balance Beam

Silard Gal

Published by Silard Gal at August 29, 2016

Brief Introduction to Processing

camera sensor

Processing is a very useful, flexible piece of software. It is mostly used for visual arts and visual literacy within technology. It has over 100 libraries and support for various projects. It’s very well documented and there are guide books available covering topics from programming basics to visualization. It supports all operating systems (GNU/Linux, Mac OS X, and Windows). The design of the software is almost identical to the Arduino IDE.

Today, we’ll use Processing to communicate with the Arduino via serial communication.

Figure 1: GUI of Processing

Project Overview

For this project I will make a ball balancing beam. A “tunnel” made of wood will be balancing the ball like a scale (Figure 2). The Beam is 44cm in width and 3cm in height. I made it narrow like a tunnel so the ball that we’ll be tracking can’t fall out.

To move this entire beam I’m using a S06NF servo motor, which will be controlled by the Arduino. We’ll go over the code later in the tutorial. I’ve placed the servo ¼ of the way from the left end of the beam.

Figure 2: S06NF STD Servo Motor / ©RobotShop inc.

The servo will move the beam up and down and the ball will follow this path.

Figure 3: Beam moving up and down

The pixy camera will sit above the beam. I’ll set it up so the visual field of the camera is only limited to the beam. It will only track the ball and nothing else (Figure 14).

Construction of the Beam

First we will need supplies for the construction. I’ll be using a simple XXMM wood (20 cm x 27 cm). I cut the wood using a circular saw, but you can use whatever type of saw you have available that will allow you to make straight, even cuts.

Figure 4: XXMM wood

Remember, only the right tools will allow you to make this beam perfect! I’m using a hammer, a ruler, nails, sandpaper, hot glue, a drill, and a saw.

Figure 5: Tools

First, I’ll make a tunnel where the ball will move left or right. The tunnel will be comprised of 4 wood panels (21cm x 3cm each) for the sides. Two pieces of wood (4cm x 3cm) in height will be attached to each end of the tunnel. The dimensions of the base are 42cm x 3cm x 1cm.

I’m using 15mm clout nails to connect the parts.

Figure 6: Close up of the tunnel

There are many ways to make a tilt point in the middle of the tunnel. I used a very simple method mainly because it is the cheapest and the easiest method. I used a long nail and two small tubes that act like bearings. I marked the center of the tunnel first, then hot glued the bearings to it and inserted the nail.

Figure 7: Nail and tubes for tilt point construction

To make the tilt point, we will also need to make holders for the nail. I used 2 pieces of 8cm x 2cm wood as shown in Figure 8. I also made a small platform that will hold everything together. The dimensions are 12cm x 4.5cm.

Figure 8: Tilt point holders

I used a small piece of wood to mount the servo and to raise it.

Figure 9: Servo mounted on the wood

In this tutorial, I’ve used Arduino UNO but feel free to use any other Arduino that has an SPI connector for connecting to the Pixy Camera.

Connecting Everything Together

Once the construction is done, it’s time to move on to connecting the Pixy camera to the Arduino and then to the servo. The schematic is the same as in Intro to: CMUcam5 Pixy Vision Camera Sensor. I’m still using an external 5V supply for the servo.

!WARNING! Don’t forget to connect the GND’s together. If you don’t connect the Power supply, Servo, and Arduino GND together, the servo will freak out!

Figure 10: The wiring diagram

Next, I need to set up the Pixy somewhere above the beam construction so it can detect the ball at any time. Adjust the settings so it only detects the ball. Please refer back to Part 1 to set the settings.

Figure 11: Pixy vision

Now let’s look at some code. To test if the servo works correctly, I modified the middle, far-left and far-right angles, so it fits my construction:

#include <Servo.h>



uint8_t leveled = 110;     //middle positon for s1 to keep the board leveled

uint8_t far_right = 180;     //far left positon for s1 to keep the board leveled

uint8_t far_left = 0;     //far right positon for s1 to keep the board levele

Servo s;  

void setup(){

s.write(leveled);

  delay(2000);

  s.write(far_right);

  delay(2000);

  s.write(far_left);

  delay(2000);

}

void loop(){

}

#include <Servo.h>

uint8_t leveled = 110; //middle positon for s1 to keep the board leveled

uint8_t far_right = 180; //far left positon for s1 to keep the board leveled

uint8_t far_left = 0; //far right positon for s1 to keep the board levele

Servo s;

void setup(){

s.write(leveled);

delay(2000);

s.write(far_right);

delay(2000);

s.write(far_left);

delay(2000);

}

void loop(){

}

Of course, you can adjust the variables to your preference.

Earlier, I introduced a software called Processing. I’ll be using this to communicate with the Arduino via serial communication.

Arduino code

Simple serial communication:

#include 
#include  

char val; // Data received from the serial port
 int ledPin = 13; // Set the pin to digital I/O 13
void setup() {
   pinMode(ledPin, OUTPUT); // Set pin as OUTPUT
   Serial.begin(9600); // Start serial communication at 9600 bps
 }
 void loop() {
   if (Serial.available())
   { // If data is available to read,
 	val = Serial.read(); // read it and store it in val
   }
   if (val == '1')
   { // If 1 was received
 	digitalWrite(ledPin, HIGH); // turn the LED on
   } else {
 	digitalWrite(ledPin, LOW); // otherwise turn it off
   }
   delay(10); // Wait 10 milliseconds for next reading
}

#include

char val; // Data received from the serial port

int ledPin = 13; // Set the pin to digital I/O 13

void setup() {

pinMode(ledPin, OUTPUT); // Set pin as OUTPUT

Serial.begin(9600); // Start serial communication at 9600 bps

}

void loop() {

if (Serial.available())

{ // If data is available to read,

val = Serial.read(); // read it and store it in val

}

if (val == '1')

{ // If 1 was received

digitalWrite(ledPin, HIGH); // turn the LED on

} else {

digitalWrite(ledPin, LOW); // otherwise turn it off

}

delay(10); // Wait 10 milliseconds for next reading

}

Processing code

import processing.serial.*;

Serial myPort;  // Create object from Serial class

void setup()
{
  size(200,200); //make our canvas 200 x 200 pixels big
  String portName = Serial.list()[0]; //change the 0 to a 1 or 2 etc. to match your port
  myPort = new Serial(this, portName, 9600);
}
void draw() {
  if (mousePressed == true)
  {                       	//if we clicked in the window
   myPort.write('1');     	//send a 1
   println("1");  
  } else
  {                       	//otherwise
  myPort.write('0');      	//send a 0
  }  
}

import processing.serial.*;

Serial myPort; // Create object from Serial class

void setup()

{

size(200,200); //make our canvas 200 x 200 pixels big

String portName = Serial.list()[0]; //change the 0 to a 1 or 2 etc. to match your port

myPort = new Serial(this, portName, 9600);

}

void draw() {

if (mousePressed == true)

{ //if we clicked in the window

myPort.write('1'); //send a 1

println("1");

} else

{ //otherwise

myPort.write('0'); //send a 0

}

It creates a 200x200px window and initilizes the serial port. Void draw() function checks if a mouse is pressed on the window (1 if it was pressed or otherwise it’ll write a 0).

Now, let’s test the code. Click run and try clicking anywhere on the window. Your LED should flash. That means it’s working correctly!

Figure 12: Basic Processing and Arduino code testing

Programming with Processing

I got values of the servo and processed them in Processing so it could make a small graph like the one shown below:

Figure 13 The graph example

To create a graph, use the following code:

import processing.serial.*;

Serial myPort;        // The serial port
int xPos = 1;         // horizontal position of the graph
float inByte = 0;

void setup () {
  // set the window size:
  size(400, 300);

  // List all the available serial ports
  // if using Processing 2.1 or later, use Serial.printArray()
  println(Serial.list());

  // I know that the first port in the serial list on my mac
  // is always my  Arduino, so I open Serial.list()[0].
  // Open whatever port is the one you're using.
  myPort = new Serial(this, Serial.list()[0], 9600);

  // don't generate a serialEvent() unless you get a newline character:
  myPort.bufferUntil('\n');

  // set inital background:
  background(0);
}
void draw () {
  // draw the line:
  stroke(127, 34, 255);
  line(xPos, height, xPos, height - inByte);

  // at the edge of the screen, go back to the beginning:
  if (xPos >= width) {
    xPos = 0;
    background(0);
  } else {
    // increment the horizontal position:
    xPos++;
  }
}


void serialEvent (Serial myPort) {
  // get the ASCII string:
  String inString = myPort.readStringUntil('\n');

  if (inString != null) {
    // trim off any whitespace:
    inString = trim(inString);
    // convert to an int and map to the screen height:
    inByte = float(inString);
    println(inByte);
    inByte = map(inByte, 0, 1023, 0, height);
  }
}

import processing.serial.*;

Serial myPort; // The serial port

int xPos = 1; // horizontal position of the graph

float inByte = 0;

void setup () {

// set the window size:

size(400, 300);

// List all the available serial ports

// if using Processing 2.1 or later, use Serial.printArray()

println(Serial.list());

// I know that the first port in the serial list on my mac

// is always my Arduino, so I open Serial.list()[0].

// Open whatever port is the one you're using.

myPort = new Serial(this, Serial.list()[0], 9600);

// don't generate a serialEvent() unless you get a newline character:

myPort.bufferUntil('\n');

// set inital background:

background(0);

}

void draw () {

// draw the line:

stroke(127, 34, 255);

line(xPos, height, xPos, height - inByte);

// at the edge of the screen, go back to the beginning:

if (xPos >= width) {

xPos = 0;

background(0);

} else {

// increment the horizontal position:

xPos++;

}

void serialEvent (Serial myPort) {

// get the ASCII string:

String inString = myPort.readStringUntil('\n');

if (inString != null) {

// trim off any whitespace:

inString = trim(inString);

// convert to an int and map to the screen height:

inByte = float(inString);

println(inByte);

inByte = map(inByte, 0, 1023, 0, height);

}

Arduino code:

#include 
#include 
#include 
#include  

//37       	164       	288

uint8_t leveled = 110;    	//middle positon for s1 to keep the board leveled	
uint8_t far_right = 180;    	//far left positon for s1 to keep the board leveled
uint8_t far_left = 0;    	//far right positon for s1 to keep the board levele
int current_pos = leveled;
int percentage,var,_percen;


Servo s;        	
Pixy pixy;

void test_board(){
  while(Serial.read() != 'b');
  Serial.write("Starting test");
  s.write(leveled);
  delay(2000);
  s.write(far_right);
  delay(2000);
  s.write(far_left);
  delay(2000);
  Serial.write("Finished test, press any key to continue");
  while(Serial.read() != 'c');
  s.write(current_pos);
  Serial.write("Continued");
}

void setup() {
  Serial.begin(9600);
  s.attach(9);
  pixy.init();
  while (!Serial);
  //test_board();
  s.write(current_pos);
}
void _servo(unsigned char side,int var){
  //by the % we get how "hard" we need to wip :D
  var = var - 90;
  if(side == 'L'){
	//Serial.write("LEFT");
	//90  180
	_percen = 90 + var;
	s.write(_percen);
  }else{
	//Serial.write("RIGHT");
	//0  90
	_percen = 90 - var;
	s.write(_percen);
  }
}
void loop() {
  static int i = 0;
  int j;
  uint16_t blocks;
  char buf[32];
 
  // grab blocks!
  blocks = pixy.getBlocks();
 
  // If there are detect blocks, print them!
  if (blocks)
  {
	i++;
	
	// do this (print) every 50 frames because printing every
	// frame would bog down the Arduino
	if (i%1 ==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();
    	uint32_t x_pos= pixy.blocks[j].x;
    	//Serial.write(x_pos);

    	
    	
    	percentage = (x_pos - 35) / 2.5;
    	if(percentage <= 40 && percentage >= 0){
    	//  Serial.write("LEFT");
      	var = percentage / 0.4;
      	_servo('L',var);
    	}else if(percentage >= 60 && percentage <= 110){
      	//Serial.write("RIGHT");
      	var = (percentage - 60) / 0.5;
      	_servo('R',var);
    	}else{
      	//Serial.write("MIDDLE");
    	}
  	}
	}
  } 
 
}

100

101

102

#include

//37 164 288

uint8_t leveled = 110; //middle positon for s1 to keep the board leveled

uint8_t far_right = 180; //far left positon for s1 to keep the board leveled

uint8_t far_left = 0; //far right positon for s1 to keep the board levele

int current_pos = leveled;

int percentage,var,_percen;

Servo s;

Pixy pixy;

void test_board(){

while(Serial.read() != 'b');

Serial.write("Starting test");

s.write(leveled);

delay(2000);

s.write(far_right);

delay(2000);

s.write(far_left);

delay(2000);

Serial.write("Finished test, press any key to continue");

while(Serial.read() != 'c');

s.write(current_pos);

Serial.write("Continued");

}

void setup() {

Serial.begin(9600);

s.attach(9);

pixy.init();

while (!Serial);

//test_board();

s.write(current_pos);

}

void _servo(unsigned char side,int var){

//by the % we get how "hard" we need to wip :D

var = var - 90;

if(side == 'L'){

//Serial.write("LEFT");

//90 180

_percen = 90 + var;

s.write(_percen);

}else{

//Serial.write("RIGHT");

//0 90

_percen = 90 - var;

s.write(_percen);

}

void loop() {

static int i = 0;

int j;

uint16_t blocks;

char buf[32];

// grab blocks!

blocks = pixy.getBlocks();

// If there are detect blocks, print them!

if (blocks)

{

i++;

// do this (print) every 50 frames because printing every

// frame would bog down the Arduino

if (i%1 ==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();

uint32_t x_pos= pixy.blocks[j].x;

//Serial.write(x_pos);

percentage = (x_pos - 35) / 2.5;

if(percentage <= 40 && percentage >= 0){

// Serial.write("LEFT");

var = percentage / 0.4;

_servo('L',var);

}else if(percentage >= 60 && percentage <= 110){

//Serial.write("RIGHT");

var = (percentage - 60) / 0.5;

_servo('R',var);

}else{

//Serial.write("MIDDLE");

}

Code explanation

I convert the x position from the Pixy to 0-100% and with that I know exactly where the ball is. By knowing where the ball is, I adjust the intensity of the servo swings. If the ball is <=10% then the servo will swing very fast to balance; if it’s around ~40% then the servo will only swing slightly to balance. Balancing is very tricky. We can certainly improve this algorithm to make it more precise.

Here are some suggestions for imporvement:

Try out various algorithms
- There are various types of mathmatical algorithms to calculate this. I tried at least 2 or 3, but in the end I decided to go with this one. I suggest writing your own algorithm to get a better grasp of this balance method.
Better hardware
- Nothing is perfect, especially wood, for something like this. If I had the resources readily available, I would have constructed this out of metal. The whole project would be more stable and precise.
Even faster
- How can we make it so it balances faster? I’m using a simple servo here. We can replace it with servos like UART or AX-12 servos. They are much stronger and faster. This also has to do with the algorithm. Again, I recommend trying out different algorithms to find the one that suits your purpose.

There are many projects out there that use a similar concept to balance an object. In additional to Pixy, you can use OpenCV with any web camera to detect objects and colors. Next to Processing, there is also Max/MSP version 5. You can utilize distance sensors, pressure sensors, etc. So there are many ways you can improve this project to make it sturdier, steadier, and faster.

Silard Gal

Hailing from Serbia, Silard loves everything about electronics and programming. He likes to work on various projects using Arduino, Raspberry Pi, Atmel, etc. and is proud to be a member of an autonomous robotics team.