JeVois Tutorials  1.22
JeVois Smart Embedded Machine Vision Tutorials
 
Share this page:
Loading...
Searching...
No Matches
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:
13#define PANPIN 3
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
24long pangain = 100;
25long tiltgain = 100;
28// Buffer for received serial port bytes:
29#define INLEN 128
30char instr[INLEN + 1];
31void 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}
46void 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}
#define TILTPIN
#define SCALE
#define TILTZERO
#define LEDPIN
void setup()
#define INLEN
long tiltgain
long panval
#define PANPIN
#define SERIAL
Servo tiltservo
char instr[INLEN+1]
long tiltval
#define PANZERO
long pangain
Servo panservo
void loop()