JeVois Tutorials  1.9
JeVois Smart Embedded Machine Vision Tutorials
Share this page:
Making a motorized pan-tilt head for JeVois and tracking objects

Several JeVois machine vision modules detect and locate target objects in video, either based on their visual salience, on a specific color, on a specific type of shape (e.g., ArUco marker), or on a generic learned appearance. Some of these modules issue messages to the JeVois serial ports that indicate the coordinates of the objects that were found (and adding such messages to those which do not yet send them is a simple modification).

In this tutorial, we use a motorized pan-tilt head and a small Arduino-compatible board to control the pan-tilt head, so that it will always try to center the detected object in the camera's field of view. That is, we will use the coordinates issued by JeVois, to counter-rotate the pan-tilt head (onto which JeVois should be mounted) so as to center the objects of interest. The net effect will be that JeVois mounted on the pan-tilt head will track the objects of interest.

This tutorial is a direct extension, with more details, step-by-step instructions, and with pictures, of the Tutorial on how to write Arduino code that interacts with JeVois in the main JeVois documentation.

Getting started

For this tutorial, we will use:

UserPanTiltMaterials.jpg

Basic plan of attack

Hardware work

Programming the Arduino

1 // JeVois control steering or a pan/tilt head from the output of JeVois modules
2 //
3 // We handle messages "T2 <targetx> <targety>", "T1 <targetx>", "PANGAIN <gain>", and "TILTGAIN <gain>".
4 // targetx and targety are assumed to be in the -1000 ... 1000 range as output by the JeVois Kalman filters.
5 // Here we only do simple PD control under the assumption that target coordinates have already been filtered upstream.
6 #include <Servo.h>
7 // Pin for LED, blinks as we receive serial commands:
8 #define LEDPIN 13
9 // Serial port to use: on chips with USB (e.g., 32u4), that usually is Serial1. On chips without USB, use Serial:
10 #define SERIAL Serial1
11 // Pins for up to two servos:
12 Servo panservo;
13 #define PANPIN 3
14 Servo tiltservo;
15 #define TILTPIN 5
16 // Initial servo values in degrees:
17 #define PANZERO 90
18 #define TILTZERO 90
19 // With updates typically coming in at 60Hz or up to 120Hz, we will often need to move by a fraction of a
20 // degree. Hence we keep track of the pan and tilt values multiplied by SCALE. For the gains, a gain of 100
21 // means we will update servo angle by the 0.1*(target value/SCALE) degrees on each update. Higher gains mean
22 // larger angular updates.
23 #define SCALE 100
24 long pangain = 100;
25 long tiltgain = 100;
26 long panval = PANZERO * SCALE;
27 long tiltval = TILTZERO * SCALE;
28 // Buffer for received serial port bytes:
29 #define INLEN 128
30 char instr[INLEN + 1];
31 void setup()
32 {
33  SERIAL.begin(115200);
34  SERIAL.setTimeout(1000000);
35  pinMode(LEDPIN, OUTPUT);
36  digitalWrite(LEDPIN, LOW);
37 
38  panservo.attach(PANPIN);
39  panservo.write(panval / SCALE);
40  tiltservo.attach(TILTPIN);
41  tiltservo.write(tiltval / SCALE);
42  // We are ready to rock, disable logs and turn on serial outputs on JeVois platform:
43  SERIAL.println("setpar serlog None");
44  SERIAL.println("setpar serout Hard");
45 }
46 void loop()
47 {
48  digitalWrite(LEDPIN, LOW);
49  byte len = SERIAL.readBytesUntil('\n', instr, INLEN);
50  instr[len] = 0;
51  digitalWrite(LEDPIN, HIGH);
52  char * tok = strtok(instr, " \r\n");
53  int state = 0; int targx = 0, targy = 0;
54  while (tok)
55  {
56  // State machine:
57  // 0: start parsing
58  // 1: T2 command, parse targx
59  // 2: T2 command, parse targy
60  // 3: T2 command complete
61  // 4: T1 command, parse targx
62  // 5: T1 command complete
63  // 6: PANGAIN command, parse pangain
64  // 7: PANGAIN command complete
65  // 8: TILTGAIN command, parse tiltgain
66  // 9: TILTGAIN command complete
67  // 1000: unknown command
68  switch (state)
69  {
70  case 0:
71  if (strcmp(tok, "T2") == 0) state = 1;
72  else if (strcmp(tok, "T1") == 0) state = 4;
73  else if (strcmp(tok, "PANGAIN") == 0) state = 6;
74  else if (strcmp(tok, "TILTGAIN") == 0) state = 8;
75  else state = 1000;
76  break;
77 
78  case 1: targx = atoi(tok); state = 2; break;
79  case 2: targy = atoi(tok); state = 3; break;
80  case 4: targx = atoi(tok); state = 5; break;
81  case 6: pangain = atoi(tok); state = 7; break;
82  case 8: tiltgain = atoi(tok); state = 9; break;
83  default: break; // Skip any additional tokens
84  }
85  tok = strtok(0, " \r\n");
86  }
87  // Target coordinates are in range -1000 ... 1000. Servos want 0 ... 180.
88  // We also need to negate as needed so that the servo turns to cancel any offset from center:
89  if (state == 3 || state == 5)
90  {
91  panval -= (targx * pangain) / 1000;
92  if (panval < 5 * SCALE) panval = 5 * SCALE; else if (panval > 175 * SCALE) panval = 175 * SCALE;
93  panservo.write(panval / SCALE);
94  }
95 
96  if (state == 3)
97  {
98  tiltval += (targy * tiltgain) / 1000;
99  if (tiltval < 5 * SCALE) tiltval = 5 * SCALE; else if (tiltval > 175 * SCALE) tiltval = 175 * SCALE;
100  tiltservo.write(tiltval / SCALE);
101  }
102 }
Servo tiltservo
#define PANZERO
Servo panservo
#define TILTZERO
void loop()
long panval
#define INLEN
long pangain
#define SCALE
#define SERIAL
type_with_N_bits< unsigned char, 8 >::type byte
#define TILTPIN
long tiltval
long tiltgain
#define PANPIN
#define LEDPIN
void setup()

Arduino code analysis

Tuning the pan-tilt head servo directions

Depending on the construction of your pan-tilt head, increasing the pan angle may turn the head rightwards or leftwards, and, likewise, increasing the tilt angle could turn the head up or down. To figure this out, we will first connect to the pan-tilt head manually. We will send hand-written commands to the head.

Before you start this, make sure you understand the following:

Now let's try the following:

setpar serout Hard
serout T2 10000 0

As the command is received, the pan-tilt head should turn right by some amount. If it turns left, your pan gain should be negated. The Arduino code supports two commands, in addition to T2: PANGAIN and TILTGAIN. You can negate PANGAIN as follows if your pan-tilt head just turned left:

serout PANGAIN -100

Then try again

serout T2 10000 0

and the camera should now pan in the other direction than previously.

serout T2 0 10000

to tilt the camera up, and

serout TILTGAIN -100

Once you are done playing, make a note of what you did and you can later put these commands in a script.cfg file in the directory of the machine vision module you will be using (see how we did that in Tutorial on how to write Arduino code that interacts with JeVois).

Let's rock

We are ready to get that pan-tilt head controlled by JeVois.

serout PANGAIN 500

(or use -500 if you need a negative gain for your particular pan-tilt head). See how much faster the head can track now.

Running with no USB output

Once you are comfortable with the operation of the algorithm, you can also try it as a true standalone machine vision system with no USB output. The steps are as follows:

Note
Because we changed framerate here (from 60 fps while we were seeing the video over USB to 30 fps with no USB), the color tracking algorithm may need re-tuning. If it does not appear to track well, first try with 60 fps in your setmapping2 command, to match what we had before. You can then edit JEVOIS:/config/videomapping.cfg to change the framerate of the mode with USB output: Search for ObjectTracker and change this entry:
YUYV 320 254 60.0 YUYV 320 240 60.0 JeVois ObjectTracker
to
YUYV 320 254 30.0 YUYV 320 240 30.0 JeVois ObjectTracker
and see User guide to video modes and mappings for details about the format of video mappings. Next time you start your video capture software, be sure to select YUYV 320x254 at 30 fps. Then have a look at Tuning the color-based object tracker using a python graphical interface for tuning. You can save your tuned parameters to the file JEVOIS:/modules/JeVois/ObjectTracker/script.cfg as we did in this tutorial.

Additional activities