JeVois Tutorials  1.9
JeVois Smart Embedded Machine Vision Tutorials
Share this page:
Build a simple visually-guided toy robot car for under $100 with JeVois

In this tutorial we build a simple visually-guided robot car, using the cheapest possible mechanical components and compensating for their brittleness by using high-framerate, closed-loop vision processing.

As a first application, the toy robot card will detect, track, and follow ArUco markers (small 2D barcodes).

In this tutorial, you will learn:

Theory of operation

The robot car will feature a pan/tilt head onto which the JeVois camera will be mounted. This is so that we can very quickly track a moving target before it escapes out of the field of view of the JeVois camera, even though the car may be quite sluggish to turn and orient towards it.

JeVois will detect an ArUco marker and will communicate its coordinates and size to an Arduino responsible for control. The Arduino implements two PD (proportional, differential) controllers to control the pan-tilt head smoothly so that it tracks the moving target. The Arduino then also implements two PID (proportional, integral, differential) controllers to steer and move the car, so as to 1) maintain a fixed distance from the target and 2) steep to zero out any pan angle on the pan-tilt head (i.e., re-center the pan-tilt head by rotating the whole vehicle).

RobotCarOperation.png

Bill of materials

Part Price Where to buy
JeVois A33 Smart Camera $49.99 https://www.jevoisinc.com/collections/jevois-hardware/products/jevois-a33-smart-machine-vision-camera?variant=36249051018
Dual motor robot chassis $8.58 http://www.ebay.com/itm/Smart-Robot-Car-Chassis-Kit-Speed-Encoder-Battery-Box-DIY-Kit-2WD-2-Motor-1-48-/132228276291?hash=item1ec96b6043:g:5x0AAOSwwzhZQlla
Pan/tilt head hardware $1.16 http://www.ebay.com/itm/PT-Pan-Tilt-Camera-Platform-Anti-Vibration-Camera-Servo-Mount-for-Aircraft-FPV-I-/390961293160?hash=item5b071be768:g:5IgAAOSw4YdYz0ug
2x 9g micro servos $2.64 http://www.ebay.com/itm/NT-9G-Servo-Mini-Micro-For-Trex-Align-450-Rc-Helicopter-Airplane-Foamy-Plane-/112489494262?epid=25005818319&hash=item1a30e576f6:g:zX4AAOSwAuZX18~p
2600mAh battery bank, 1A output $7.99 https://www.jevoisinc.com/collections/accessories/products/usb-power-bank-2600mah
Mini + Micro USB splitter cable $4.99 https://www.jevoisinc.com/collections/accessories/products/mini-usb-micro-usb-splitter-cable-15cm-6-in-long
8GB class 10 Micro SD card $8.99 https://www.jevoisinc.com/collections/accessories/products/high-speed-8gb-microsd-card-with-jevois-software-pre-loaded
Arduino 32u4 compatible $6.99 https://www.jevoisinc.com/collections/accessories/products/atmega32u4-16mhz-5v-arduino-compatible-micro-controller
Dual 1.2A motor driver PWM $1.33 http://www.ebay.com/itm/TB6612FNG-Dual-DC-Stepper-Motor-Drive-Controller-Board-Module-Replace-L298N-/191661587054?epid=719645256&hash=item2c9febba6e:g:1PUAAOSwI3RW-22c
AA batteries, screws, wires $2.34 misc
Total $95.00 (Prices include shipping to the USA. Shipping to other countries may vary.)

Assembly

RobotCarChassis.jpg
Arduino pin Connect to
RX JeVois micro-serial port white wire
TX JeVois micro-serial port yellow wire
VCC JeVois micro-serial port red wire
GND JeVois micro-serial port black wire
3 Motor driver PWM for left motor
2 Motor driver IN1 for left motor
21 Motor driver IN2 for left motor
5 Motor driver PWM for right motor
20 Motor driver IN1 for right motor
4 Motor driver IN2 for right motor
9 PWM control wire of pan servo
10 PWM control wire of tilt servo
Note
The pins used leave enough free pins to connect an optional additional SPI OLED screen should you want to have one on your robot. Search for, e.g., 0.96" OLED SPI on eBay to find a screen.

Also connect the 6V power (from the 4xAA pack) to the power lines of the pan and tilt servos, and to the motor input voltage of the motor controller board. The Arduino and JeVois will be powered directly from the USB battery bank using the splitter mini + micro USB cable. Finally connect the Arduino GND (from USB battery) to motor GND (from 4xAA batteries).

Your rig should roughly look as follows:

RobotCarAssembled.jpg

JeVois configuration

For our initial testing of this robot we will use the ArUco module with no USB output. Just edit JEVOIS:/config/initscript.cfg as follows:

setmapping2 YUYV 320 240 50.0 JeVois DemoArUco
setpar serlog None
setpar serout Hard
setpar serstyle Normal
streamon

Arduino code

We will use the Arduino PID Library by Brett Beauregard, make sure to install it in your Arduino IDE.

Here is a simple sketch to follow a single ArUco marker and to attempt to stay at a fixed distance from it:

1 // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
2 //
3 // JeVois Smart Embedded Machine Vision Toolkit - Copyright (C) 2017 by Laurent Itti, the University of Southern
4 // California (USC), and iLab at USC. See http://iLab.usc.edu and http://jevois.org for information about this project.
5 //
6 // This file is part of the JeVois Smart Embedded Machine Vision Toolkit. This program is free software; you can
7 // redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software
8 // Foundation, version 2. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
9 // without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public
10 // License for more details. You should have received a copy of the GNU General Public License along with this program;
11 // if not, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
12 //
13 // Contact information: Laurent Itti - 3641 Watt Way, HNB-07A - Los Angeles, CA 90089-2520 - USA.
14 // Tel: +1 213 740 3527 - itti@pollux.usc.edu - http://iLab.usc.edu - http://jevois.org
15 // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
16 
17 // You need to install the Arduino PID Library by Brett Beauregard
18 #include <PID_v1.h>
19 #include <Servo.h>
20 
21 // Pins setup: Use RXLED(17) on Arduino pro micro 32u4 which does not have LED(13)
22 #define LPWMPIN 3
23 #define LIN1PIN 2
24 #define LIN2PIN 21
25 #define RPWMPIN 5
26 #define RIN1PIN 20
27 #define RIN2PIN 4
28 #define LEDPIN 17
29 #define PANPIN 9
30 #define TILTPIN 10
31 
32 // Serial port to use for JeVois:
33 // On chips with USB (e.g., 32u4), that usually is Serial1. On chips without USB, use Serial.
34 #define SERIAL Serial1
35 
36 // Serial port for debugging (optional):
37 #define DBGSERIAL Serial
38 
39 // Buffer for received serial port bytes:
40 #define INLEN 128
41 char instr[INLEN + 1];
42 
43 // Gain values to compensate for unbalanced motors: Do not exceed 655. Give a lower value to the motor that is faster.
44 // E.g., if your car vires to the left when both motors are 100% forward, your right motor is a bit faster, so decrease
45 // rightgain to compensate.
46 long leftgain = 655;
47 long rightgain = 655;
48 
49 // Desired target width in standardized JeVois coordinates (a value of 2000 would occupy the whole field of view):
50 int TARGW = 300;
51 
52 // Create a PID for target width, and one for steering angle:
53 double wset, win, wout, aset, ain, aout;
54 PID wpid(&win, &wout, &wset, 1.0, 0.01, 0.01, DIRECT);
55 PID apid(&ain, &aout, &aset, 0.5, 0.05, 0.02, DIRECT);
56 
57 // current speed and steering:
58 double speed = 0;
59 double steer = 0;
60 
61 int ledstate = 0;
62 
63 // Pan and tilt servos zero values and +/- angular range, in degrees:
64 #define PANZERO 90
65 #define PANRANGE 60
66 #define TILTZERO 70
67 #define TILTRANGE 40
68 
69 // ###################################################################################################
70 // Simple PD servo controller
71 class ServoPD
72 {
73 public:
74  ServoPD(long Kp, long Kd, long zero, long range, long scalebits = 8) :
75  itsKp(Kp), itsKd(Kd), itsPos(zero << scalebits),
76  itsPrevTarget(zero << scalebits), itsZero(zero << scalebits),
77  itsRange(range << scalebits), itsScaleBits(scalebits)
78  { }
79 
80  void attach(int pin, int pos)
81  {
82  itsServo.attach(pin);
83  itsPos = (pos << itsScaleBits);
84  itsServo.write(pos);
85  }
86 
87  long get() const
88  {
89  return (itsPos >> itsScaleBits);
90  }
91 
92  void update(long targetpos)
93  {
94  targetpos <<= itsScaleBits;
95  long diff = itsKp * targetpos + itsKd * (targetpos - itsPrevTarget);
96  itsPos += (diff >> 16);
97  itsPos = constrain(itsPos, itsZero - itsRange, itsZero + itsRange);
98  itsServo.write(itsPos >> itsScaleBits);
99  itsPrevTarget = targetpos;
100  }
101 
102  void reset(long targetpos)
103  {
104  targetpos <<= itsScaleBits;
105  itsPos = constrain(itsPos, itsZero - itsRange, itsZero + itsRange);
106  itsPrevTarget = targetpos;
107  }
108 
109  long rawget()
110  {
111  return itsPos >> itsScaleBits;
112  }
113 
114  void rawset(long rawval)
115  {
116  itsPos = rawval << itsScaleBits;
117  itsServo.write(rawval);
118  }
119 
120  private:
121  Servo itsServo;
122  long itsKp, itsKd, itsPos, itsPrevTarget, itsZero, itsRange, itsScaleBits;
123 };
124 
125 // Create one servo PD controler for camera pan and another for camera tilt:
126 ServoPD panservo(400, 200, PANZERO, PANRANGE);
127 ServoPD tiltservo(300, 100, TILTZERO, TILTRANGE);
128 
129 // ###################################################################################################
130 void setup()
131 {
132  SERIAL.setTimeout(50);
133  SERIAL.begin(115200);
134  SERIAL.setTimeout(50);
135  pinMode(LPWMPIN, OUTPUT);
136  pinMode(LIN1PIN, OUTPUT);
137  pinMode(LIN2PIN, OUTPUT);
138  pinMode(RPWMPIN, OUTPUT);
139  pinMode(RIN1PIN, OUTPUT);
140  pinMode(RIN2PIN, OUTPUT);
141  pinMode(LEDPIN, OUTPUT);
142  pinMode(PANPIN, OUTPUT);
143  pinMode(TILTPIN, OUTPUT);
144  motor(0, 0);
145 
146  panservo.attach(PANPIN, PANZERO);
147  tiltservo.attach(TILTPIN, TILTZERO);
148 
149  win = TARGW; wset = TARGW; wout = 0;
150  wpid.SetMode(AUTOMATIC);
151  wpid.SetSampleTime(10);
152  wpid.SetOutputLimits(-120, 120);
153 
154  ain = 0; aset = 0; aout = 0;
155  apid.SetMode(AUTOMATIC);
156  apid.SetSampleTime(10);
157  apid.SetOutputLimits(-50, 50);
158 }
159 
160 // ###################################################################################################
161 void loop()
162 {
163  byte len = SERIAL.readBytesUntil('\n', instr, INLEN);
164  instr[len] = 0;
165 
166  char * tok = strtok(instr, " \r\n");
167  int state = 0; int id, targx, targy, targw, targh;
168 
169  while (tok)
170  {
171  // State machine:
172  // 0: start parsing
173  // 1: N2 command, parse id
174  // 2: N2 command, parse targx
175  // 3: N2 command, parse targy
176  // 4: N2 command, parse targw
177  // 5: N2 command, parse targh
178  // 6: N2 command complete
179  // 1000: unknown command
180  switch (state)
181  {
182  case 0: if (strcmp(tok, "N2") == 0) state = 1; else state = 1000; break;
183  case 1: id = atoi(&tok[1]); state = 2; break; // ignore prefix
184  case 2: targx = atoi(tok); state = 3; break;
185  case 3: targy = atoi(tok); state = 4; break;
186  case 4: targw = atoi(tok); state = 5; break;
187  case 5: targh = atoi(tok); state = 6; break;
188  default: break; // Skip any additional tokens
189  }
190  tok = strtok(0, " \r\n");
191  }
192 
193  // If a complete new N2 command was received, act:
194  if (state == 6)
195  {
196  // Actuate the pan/tilt head to track the target:
197  panservo.update(-targx);
198  tiltservo.update(targy);
199 
200  // Move the car forward/backward to track the derired target width:
201  win = targw; wpid.Compute(); speed = wout;
202 
203  // Steer the car to zero out the camera's pan angle:
204  ain = panservo.get() - PANZERO; apid.Compute(); steer = aout * (1.0 + fabs(speed) * 0.05);
205  }
206  else
207  {
208  // Slow down if we lost track:
209  speed *= 0.95;
210  steer *= 0.8;
211  }
212 
213  // Actuate the motors:
214  motor(speed + steer, speed - steer);
215 
216  // Blink the LED on every loop:
217  digitalWrite(LEDPIN, ledstate);
218  ledstate = !ledstate;
219 }
220 
221 // ###################################################################################################
222 // Actuate the motors: Values in [-100 .. 100]; positive = forward, negative = backward, 0 = brake
223 void motor(long left, long right)
224 {
225  if (left > 100) left = 100; else if (left < -100) left = -100;
226  if (right > 100) right = 100; else if (right < -100) right = -100;
227 
228  long motleft = (left * leftgain) >> 8;
229  long motright = (right * rightgain) >> 8;
230 
231  if (motleft > 0) {
232  digitalWrite(LIN1PIN, LOW);
233  digitalWrite(LIN2PIN, HIGH);
234  analogWrite(LPWMPIN, motleft);
235  } else if (motleft < 0) {
236  digitalWrite(LIN1PIN, HIGH);
237  digitalWrite(LIN2PIN, LOW);
238  analogWrite(LPWMPIN, -motleft);
239  } else {
240  digitalWrite(LIN1PIN, LOW);
241  digitalWrite(LIN2PIN, LOW);
242  }
243 
244  if (motright > 0) {
245  digitalWrite(RIN1PIN, LOW);
246  digitalWrite(RIN2PIN, HIGH);
247  analogWrite(RPWMPIN, motright);
248  } else if (motright < 0) {
249  digitalWrite(RIN1PIN, HIGH);
250  digitalWrite(RIN2PIN, LOW);
251  analogWrite(RPWMPIN, -motright);
252  } else {
253  digitalWrite(RIN1PIN, LOW);
254  digitalWrite(RIN2PIN, LOW);
255  }
256 }
257 
#define LEDPIN
#define RPWMPIN
double aout
ServoPD tiltservo(300, 100, TILTZERO, TILTRANGE)
#define LIN2PIN
void rawset(long rawval)
long rightgain
#define PANRANGE
#define LIN1PIN
#define INLEN
double aset
#define SERIAL
long leftgain
void motor(long left, long right)
ServoPD panservo(400, 200, PANZERO, PANRANGE)
#define PANPIN
ServoPD(long Kp, long Kd, long zero, long range, long scalebits=8)
#define PANZERO
double win
double ain
long rawget()
void update(long targetpos)
type_with_N_bits< unsigned char, 8 >::type byte
int ledstate
int TARGW
#define RIN1PIN
#define TILTPIN
void setup()
#define TILTRANGE
double steer
double wset
double speed
void loop()
#define TILTZERO
double wout
#define LPWMPIN
void attach(int pin, int pos)
#define RIN2PIN
void reset(long targetpos)

Hopping from one target to the next

We modify the code slightly to handle multiple targets. When we receive messages from JeVois about multiple targets, we keep track of the current ArUco ID that is tracked, and the next one that is being detected. Once the currently tracked one is close to us, we switch to tracking the next one. See the slightly modified logic in the loop() function of the modified code:

1 // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
2 //
3 // JeVois Smart Embedded Machine Vision Toolkit - Copyright (C) 2017 by Laurent Itti, the University of Southern
4 // California (USC), and iLab at USC. See http://iLab.usc.edu and http://jevois.org for information about this project.
5 //
6 // This file is part of the JeVois Smart Embedded Machine Vision Toolkit. This program is free software; you can
7 // redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software
8 // Foundation, version 2. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
9 // without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public
10 // License for more details. You should have received a copy of the GNU General Public License along with this program;
11 // if not, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
12 //
13 // Contact information: Laurent Itti - 3641 Watt Way, HNB-07A - Los Angeles, CA 90089-2520 - USA.
14 // Tel: +1 213 740 3527 - itti@pollux.usc.edu - http://iLab.usc.edu - http://jevois.org
15 // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
16 
17 // You need to install the Arduino PID Library by Brett Beauregard
18 #include <PID_v1.h>
19 #include <Servo.h>
20 
21 // Pins setup: Use RXLED(17) on Arduino pro micro 32u4 which does not have LED(13)
22 #define LPWMPIN 3
23 #define LIN1PIN 2
24 #define LIN2PIN 21
25 #define RPWMPIN 5
26 #define RIN1PIN 20
27 #define RIN2PIN 4
28 #define LEDPIN 17
29 #define PANPIN 9
30 #define TILTPIN 10
31 
32 // Serial port to use for JeVois:
33 // On chips with USB (e.g., 32u4), that usually is Serial1. On chips without USB, use Serial.
34 #define SERIAL Serial1
35 
36 // Serial port for debugging (optional):
37 #define DBGSERIAL Serial
38 
39 // Buffer for received serial port bytes:
40 #define INLEN 128
41 char instr[INLEN + 1];
42 
43 // Gain values to compensate for unbalanced motors: Do not exceed 655. Give a lower value to the motor that is faster.
44 // E.g., if your car vires to the left when both motors are 100% forward, your right motor is a bit faster, so decrease
45 // rightgain to compensate.
46 long leftgain = 655;
47 long rightgain = 655;
48 
49 // Desired target width in standardized JeVois coordinates (a value of 2000 would occupy the whole field of view):
50 int TARGW = 300;
51 
52 // Create a PID for target width, and one for steering angle:
53 double wset, win, wout, aset, ain, aout;
54 PID wpid(&win, &wout, &wset, 1.0, 0.01, 0.01, DIRECT);
55 PID apid(&ain, &aout, &aset, 0.5, 0.05, 0.02, DIRECT);
56 
57 // current speed and steering:
58 double speed = 0;
59 double steer = 0;
60 int steerdelay = 0; // Delay in steering when re-aquiring a target to avoid steering jerk
61 
62 // Pan and tilt servos zero values and +/- angular range, in degrees:
63 #define PANZERO 90
64 #define PANRANGE 60
65 #define TILTZERO 70
66 #define TILTRANGE 40
67 
68 // Handoff between several targets:
69 int currid = -1, currw = 0, currt = 0, nextid = -1, nextw = 0, nextt = 0, doneid = -1;
70 #define HANDOFFW ((TARGW * 5) / 10)
71 int t = 0;
72 
73 // ###################################################################################################
74 // Simple PD servo controller
75 class ServoPD
76 {
77 public:
78  ServoPD(long Kp, long Kd, long zero, long range, long scalebits = 8) :
79  itsKp(Kp), itsKd(Kd), itsPos(zero << scalebits),
80  itsPrevTarget(zero << scalebits), itsZero(zero << scalebits),
81  itsRange(range << scalebits), itsScaleBits(scalebits)
82  { }
83 
84  void attach(int pin, int pos)
85  {
86  itsServo.attach(pin);
87  itsPos = (pos << itsScaleBits);
88  itsServo.write(pos);
89  }
90 
91  long get() const
92  {
93  return (itsPos >> itsScaleBits);
94  }
95 
96  void update(long targetpos)
97  {
98  targetpos <<= itsScaleBits;
99  long diff = itsKp * targetpos + itsKd * (targetpos - itsPrevTarget);
100  itsPos += (diff >> 16);
101  itsPos = constrain(itsPos, itsZero - itsRange, itsZero + itsRange);
102  itsServo.write(itsPos >> itsScaleBits);
103  itsPrevTarget = targetpos;
104  }
105 
106  void reset(long targetpos)
107  {
108  targetpos <<= itsScaleBits;
109  itsPos = constrain(itsPos, itsZero - itsRange, itsZero + itsRange);
110  itsPrevTarget = targetpos;
111  }
112 
113  long rawget()
114  {
115  return itsPos >> itsScaleBits;
116  }
117 
118  void rawset(long rawval)
119  {
120  itsPos = rawval << itsScaleBits;
121  itsServo.write(rawval);
122  }
123 
124  private:
125  Servo itsServo;
126  long itsKp, itsKd, itsPos, itsPrevTarget, itsZero, itsRange, itsScaleBits;
127 };
128 
129 // Create one servo PD controler for camera pan and another for camera tilt:
130 //ServoPD panservo(400, 200, PANZERO, PANRANGE);
131 ServoPD panservo(100, 50, PANZERO, PANRANGE);
132 ServoPD tiltservo(300, 100, TILTZERO, TILTRANGE);
133 
134 // ###################################################################################################
135 void setup()
136 {
137  SERIAL.setTimeout(50);
138  SERIAL.begin(115200);
139  SERIAL.setTimeout(50);
140  pinMode(LPWMPIN, OUTPUT);
141  pinMode(LIN1PIN, OUTPUT);
142  pinMode(LIN2PIN, OUTPUT);
143  pinMode(RPWMPIN, OUTPUT);
144  pinMode(RIN1PIN, OUTPUT);
145  pinMode(RIN2PIN, OUTPUT);
146  pinMode(LEDPIN, OUTPUT);
147  pinMode(PANPIN, OUTPUT);
148  pinMode(TILTPIN, OUTPUT);
149  motor(0, 0);
150 
151  panservo.attach(PANPIN, PANZERO);
152  tiltservo.attach(TILTPIN, TILTZERO);
153 
154  win = TARGW; wset = TARGW; wout = 0;
155  wpid.SetMode(AUTOMATIC);
156  wpid.SetSampleTime(10);
157  wpid.SetOutputLimits(-120, 120);
158 
159  ain = 0; aset = 0; aout = 0;
160  apid.SetMode(AUTOMATIC);
161  apid.SetSampleTime(10);
162  apid.SetOutputLimits(-50, 50);
163 }
164 
165 // ###################################################################################################
166 void loop()
167 {
168  byte len = SERIAL.readBytesUntil('\n', instr, INLEN);
169  instr[len] = 0;
170 
171  char * tok = strtok(instr, " \r\n");
172  int state = 0; int id, targx, targy, targw, targh;
173 
174  while (tok)
175  {
176  // State machine:
177  // 0: start parsing
178  // 1: N2 command, parse id
179  // 2: N2 command, parse targx
180  // 3: N2 command, parse targy
181  // 4: N2 command, parse targw
182  // 5: N2 command, parse targh
183  // 6: N2 command complete
184  // 1000: unknown command
185  switch (state)
186  {
187  case 0: if (strcmp(tok, "N2") == 0) state = 1; else state = 1000; break;
188  case 1: id = atoi(&tok[1]); state = 2; break; // ignore prefix
189  case 2: targx = atoi(tok); state = 3; break;
190  case 3: targy = atoi(tok); state = 4; break;
191  case 4: targw = atoi(tok); state = 5; break;
192  case 5: targh = atoi(tok); state = 6; break;
193  default: break; // Skip any additional tokens
194  }
195  tok = strtok(0, " \r\n");
196  }
197 
198  // If a complete new N2 command was received, act:
199  if (state == 6)
200  {
201  // Update our steering delay after re-aquisition of previously lost target:
202  if (steerdelay) steerdelay -= 1;
203 
204  // Decide whether we should track this target or another (larger) one:
205  int trackit = 0;
206 
207  // If we are not tracking anything, track this one:
208  if (currid == -1)
209  { currid = id; currw = targw; currt = t; trackit = 1; } // New target, track it
210  else
211  {
212  if (id == doneid)
213  { trackit = 0; } // Ignore this one as we have already tracked it all the way
214  else if (id == currid)
215  { currw = targw; currt = t; trackit = 1; } // Keep tracking current target
216  else if (nextid == -1 && targw < currw)
217  { nextid = id; nextw = targw; nextt = t; trackit = 0; } // Next target detected, do not track it yet
218  else if (id == nextid && currw > HANDOFFW)
219  { doneid = currid; currid = nextid; nextid = -1; currw = targw; currt = t; trackit = 1; } // handoff
220  }
221 
222  if (trackit)
223  {
224  // Actuate the pan/tilt head to track the target:
225  panservo.update(-targx);
226  tiltservo.update(targy);
227 
228  // Move the car forward/backward to track the derired target width:
229  win = targw; wpid.Compute(); speed = wout;
230 
231  // Steer the car to zero out the camera's pan angle:
232  ain = panservo.get() - PANZERO; apid.Compute(); steer = aout * (1.0 + fabs(speed) * 0.05);
233  }
234  }
235  else
236  {
237  // Slow down if we lost track:
238  speed *= 0.95;
239  steer *= 0.8;
240  steerdelay = 1;
241  }
242 
243  // Actuate the motors:
244  if (steerdelay) steer = 0.0; // delay steering a bit after target re-acquisition to avoid steering jerk
245  motor(speed + steer, speed - steer);
246 
247  // Invalidate previously detected targets that we have not seen in a while:
248  if (t - currt > 30) { currid = -1; nextid = -1; doneid = -1; }
249  if (t - nextt > 30) nextid = -1;
250 
251  // Blink the LED on every loop:
252  digitalWrite(LEDPIN, t & 1);
253  t += 1;
254 }
255 
256 // ###################################################################################################
257 // Actuate the motors: Values in [-100 .. 100]; positive = forward, negative = backward, 0 = brake
258 void motor(long left, long right)
259 {
260  if (left > 100) left = 100; else if (left < -100) left = -100;
261  if (right > 100) right = 100; else if (right < -100) right = -100;
262 
263  long motleft = (left * leftgain) >> 8;
264  long motright = (right * rightgain) >> 8;
265 
266  if (motleft > 0) {
267  digitalWrite(LIN1PIN, LOW);
268  digitalWrite(LIN2PIN, HIGH);
269  analogWrite(LPWMPIN, motleft);
270  } else if (motleft < 0) {
271  digitalWrite(LIN1PIN, HIGH);
272  digitalWrite(LIN2PIN, LOW);
273  analogWrite(LPWMPIN, -motleft);
274  } else {
275  digitalWrite(LIN1PIN, LOW);
276  digitalWrite(LIN2PIN, LOW);
277  }
278 
279  if (motright > 0) {
280  digitalWrite(RIN1PIN, LOW);
281  digitalWrite(RIN2PIN, HIGH);
282  analogWrite(RPWMPIN, motright);
283  } else if (motright < 0) {
284  digitalWrite(RIN1PIN, HIGH);
285  digitalWrite(RIN2PIN, LOW);
286  analogWrite(RPWMPIN, -motright);
287  } else {
288  digitalWrite(RIN1PIN, LOW);
289  digitalWrite(RIN2PIN, LOW);
290  }
291 }
292 
#define LIN1PIN
long rightgain
int currt
#define PANRANGE
#define SERIAL
int currw
void motor(long left, long right)
long leftgain
#define RIN2PIN
double wset
void rawset(long rawval)
#define LPWMPIN
int TARGW
#define RPWMPIN
#define RIN1PIN
void setup()
double win
int currid
ServoPD(long Kp, long Kd, long zero, long range, long scalebits=8)
void loop()
#define TILTRANGE
double wout
long rawget()
#define INLEN
double ain
void update(long targetpos)
type_with_N_bits< unsigned char, 8 >::type byte
ServoPD panservo(100, 50, PANZERO, PANRANGE)
#define TILTZERO
#define HANDOFFW
double aout
#define LEDPIN
double aset
#define PANZERO
int nextt
ServoPD tiltservo(300, 100, TILTZERO, TILTRANGE)
int nextw
int t
#define PANPIN
#define LIN2PIN
int nextid
int steerdelay
void attach(int pin, int pos)
int doneid
void reset(long targetpos)
#define TILTPIN