JeVois Tutorials  1.22
JeVois Smart Embedded Machine Vision Tutorials
 
Share this page:
Loading...
Searching...
No Matches
JeVoisRobotCar1.C
Go to the documentation of this file.
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
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:
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)
ServoPD(long Kp, long Kd, long zero, long range, long scalebits=8)
void rawset(long rawval)