JeVois Tutorials  1.17
JeVois Smart Embedded Machine Vision Tutorials
Share this page:
ArduinoPanTiltTutorial.C
Go to the documentation of this file.
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;
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 }
tiltgain
long tiltgain
Definition: ArduinoPanTiltTutorial.C:25
pangain
long pangain
Definition: ArduinoPanTiltTutorial.C:24
SCALE
#define SCALE
Definition: ArduinoPanTiltTutorial.C:23
INLEN
#define INLEN
Definition: ArduinoPanTiltTutorial.C:29
panval
long panval
Definition: ArduinoPanTiltTutorial.C:26
instr
char instr[INLEN+1]
Definition: ArduinoPanTiltTutorial.C:30
tiltservo
Servo tiltservo
Definition: ArduinoPanTiltTutorial.C:14
loop
void loop()
Definition: ArduinoPanTiltTutorial.C:46
tiltval
long tiltval
Definition: ArduinoPanTiltTutorial.C:27
panservo
Servo panservo
Definition: ArduinoPanTiltTutorial.C:12
LEDPIN
#define LEDPIN
Definition: ArduinoPanTiltTutorial.C:8
PANZERO
#define PANZERO
Definition: ArduinoPanTiltTutorial.C:17
TILTZERO
#define TILTZERO
Definition: ArduinoPanTiltTutorial.C:18
TILTPIN
#define TILTPIN
Definition: ArduinoPanTiltTutorial.C:15
PANPIN
#define PANPIN
Definition: ArduinoPanTiltTutorial.C:13
SERIAL
#define SERIAL
Definition: ArduinoPanTiltTutorial.C:10
setup
void setup()
Definition: ArduinoPanTiltTutorial.C:31