JeVois Tutorials  1.21
JeVois Smart Embedded Machine Vision Tutorials
 
Share this page:
Loading...
Searching...
No Matches
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:

  • How to design and build a simple autonomous vehicle with vision processing
  • How to program a simple visually-guided behavior on an Arduino

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

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

  • Assemble the car chassis per the provided instructions
  • Assemble the pan/tilt head per the provided instructions
  • Use the following Arduino connections:
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:

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
41char 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.
46long leftgain = 655;
47long rightgain = 655;
48
49// Desired target width in standardized JeVois coordinates (a value of 2000 would occupy the whole field of view):
50int TARGW = 300;
51
52// Create a PID for target width, and one for steering angle:
53double wset, win, wout, aset, ain, aout;
54PID wpid(&win, &wout, &wset, 1.0, 0.01, 0.01, DIRECT);
55PID apid(&ain, &aout, &aset, 0.5, 0.05, 0.02, DIRECT);
56
57// current speed and steering:
58double speed = 0;
59double steer = 0;
60
61int 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
71class ServoPD
72{
73public:
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:
126ServoPD panservo(400, 200, PANZERO, PANRANGE);
128
129// ###################################################################################################
130void 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
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// ###################################################################################################
161void 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:
215
216 // Blink the LED on every loop:
217 digitalWrite(LEDPIN, ledstate);
219}
220
221// ###################################################################################################
222// Actuate the motors: Values in [-100 .. 100]; positive = forward, negative = backward, 0 = brake
223void 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 TILTPIN
#define RIN1PIN
double win
#define TILTZERO
#define RIN2PIN
double ain
#define LEDPIN
void setup()
long rightgain
#define INLEN
long leftgain
int TARGW
void motor(long left, long right)
ServoPD tiltservo(300, 100, TILTZERO, TILTRANGE)
double speed
#define PANRANGE
#define TILTRANGE
double wset
double wout
#define LIN1PIN
#define PANPIN
ServoPD panservo(400, 200, PANZERO, PANRANGE)
#define SERIAL
char instr[INLEN+1]
#define LIN2PIN
#define LPWMPIN
#define PANZERO
double aset
#define RPWMPIN
double aout
int ledstate
double steer
void loop()
void update(long targetpos)
long rawget()
void attach(int pin, int pos)
long get() const
void reset(long targetpos)
void rawset(long rawval)

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
41char 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.
46long leftgain = 655;
47long rightgain = 655;
48
49// Desired target width in standardized JeVois coordinates (a value of 2000 would occupy the whole field of view):
50int TARGW = 300;
51
52// Create a PID for target width, and one for steering angle:
53double wset, win, wout, aset, ain, aout;
54PID wpid(&win, &wout, &wset, 1.0, 0.01, 0.01, DIRECT);
55PID apid(&ain, &aout, &aset, 0.5, 0.05, 0.02, DIRECT);
56
57// current speed and steering:
58double speed = 0;
59double steer = 0;
60int 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:
69int currid = -1, currw = 0, currt = 0, nextid = -1, nextw = 0, nextt = 0, doneid = -1;
70#define HANDOFFW ((TARGW * 5) / 10)
71int t = 0;
72
73// ###################################################################################################
74// Simple PD servo controller
75class ServoPD
76{
77public:
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);
133
134// ###################################################################################################
135void 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
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// ###################################################################################################
166void 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
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
258void 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 TILTPIN
#define RIN1PIN
#define TILTZERO
#define RIN2PIN
int currt
#define HANDOFFW
#define LEDPIN
int nextt
int currid
void setup()
long rightgain
#define INLEN
long leftgain
int nextid
int TARGW
void motor(long left, long right)
ServoPD tiltservo(300, 100, TILTZERO, TILTRANGE)
double speed
#define PANRANGE
#define TILTRANGE
double wset
ServoPD panservo(100, 50, PANZERO, PANRANGE)
double wout
#define LIN1PIN
#define PANPIN
int currw
#define SERIAL
double ain
char instr[INLEN+1]
int t
int steerdelay
#define LIN2PIN
double win
int nextw
#define LPWMPIN
#define PANZERO
double aset
int doneid
#define RPWMPIN
double aout
double steer
void loop()