1+ /*
2+ ============================
3+
4+ SERVO CAM Arduino Client
5+ v 0.9.2 | 2023.03.22
6+
7+ (c) 2023, servocam.org
8+ https://servocam.org
9+ https://github.com/servo-cam
10+ info@servocam.org
11+
12+ ============================
13+ */
14+
15+ #include < Servo.h>
16+
17+ // --------- BEGIN CONFIG -----------
18+
19+ #define DEBUG false // debug mode (sends status to serial after every command)
20+
21+ #define PIN_SERVO_X 10 // servo X (horizontal) PWM pin
22+ #define PIN_SERVO_Y 11 // servo Y (vertical) PWM pin
23+ #define PIN_ACTION_1 2 // action #1 (A1) DIGITAL pin
24+ #define PIN_ACTION_2 4 // action #2 (A2) DIGITAL pin
25+ #define PIN_ACTION_3 7 // action #3 (A3) DIGITAL pin
26+ #define PIN_ACTION_4 8 // action #4 (B4) DIGITAL pin
27+ #define PIN_ACTION_5 12 // action #5 (B5) DIGITAL pin
28+ #define PIN_ACTION_6 13 // action #6 (B6) DIGITAL pin
29+
30+ #define SERVO_X_MIN 0 // servo X min angle
31+ #define SERVO_X_MAX 180 // servo X max angle
32+ #define SERVO_Y_MIN 0 // servo Y min angle
33+ #define SERVO_Y_MAX 180 // servo Y max angle
34+
35+ #define SERVO_X_LIMIT_MIN 0 // servo X min allowed movement angle
36+ #define SERVO_X_LIMIT_MAX 180 // servo X max allowed movement angle
37+ #define SERVO_Y_LIMIT_MIN 0 // servo Y min allowed movement angle
38+ #define SERVO_Y_LIMIT_MAX 180 // servo Y max allowed movement angle
39+
40+ #define SERVO_X_PULSE_MIN 771 // servo X min pulse
41+ #define SERVO_X_PULSE_MAX 2193 // servo X max pulse
42+ #define SERVO_Y_PULSE_MIN 771 // servo Y min pulse
43+ #define SERVO_Y_PULSE_MAX 2193 // servo Y max pulse
44+
45+ #define SERVO_X_INITIAL_ANGLE 90 // servo X start angle
46+ #define SERVO_Y_INITIAL_ANGLE 90 // servo Y start angle
47+
48+ #define SERIAL_TIMEOUT 4 // serial port timeout
49+ #define SERIAL_BAUD_RATE 9600 // serial port baud rate
50+
51+ // --------- END OF CONFIG -----------
52+
53+
54+ bool actions[6 ] = {}; // action states
55+ int pins[6 ] = {}; // action pins
56+ int counter = 0 ; // detected objects counter
57+ int x = 0 ; // servo X current angle
58+ int y = 0 ; // servo Y current angle
59+
60+ char *cmd[9 ]; // extracted commands
61+ char *ptr = NULL ; // str cmd pointer
62+
63+ Servo servo_x; // servo X
64+ Servo servo_y; // servo Y
65+ String buffer; // serial data buffer
66+
67+ void setup ()
68+ {
69+ Serial.begin (SERIAL_BAUD_RATE); // begin serial
70+ Serial.setTimeout (SERIAL_TIMEOUT); // set minimal timeout
71+
72+ // setup action pins
73+ pins[0 ] = PIN_ACTION_1; // action #1 (A1) pin
74+ pins[1 ] = PIN_ACTION_2; // action #2 (A2) pin
75+ pins[2 ] = PIN_ACTION_3; // action #3 (A3) pin
76+ pins[3 ] = PIN_ACTION_4; // action #4 (B4) pin
77+ pins[4 ] = PIN_ACTION_5; // action #5 (B5) pin
78+ pins[5 ] = PIN_ACTION_6; // action #6 (B6) pin
79+
80+ x = SERVO_X_INITIAL_ANGLE; // start servo X with default position
81+ y = SERVO_Y_INITIAL_ANGLE; // start servo Y with default position
82+
83+ // setup servo
84+ servo_x.write (x); // set initial servo X position
85+ servo_y.write (y); // set initial servo Y position
86+ servo_x.attach (PIN_SERVO_X, SERVO_X_PULSE_MIN, SERVO_X_PULSE_MAX); // attach servo X
87+ servo_y.attach (PIN_SERVO_Y, SERVO_Y_PULSE_MIN, SERVO_Y_PULSE_MAX); // attach servo Y
88+
89+ // initialize action pins with LOW state
90+ for (int i = 0 ; i < 6 ; i++) {
91+ pinMode (pins[i], OUTPUT); // set pin to OUTPUT mode
92+ actions[i] = false ; // store disabled state
93+ digitalWrite (pins[i], LOW); // set state = LOW
94+ }
95+ }
96+
97+ // define status message here, must returns String
98+ String get_status () {
99+ return " RECV: "
100+ }
101+
102+ void loop ()
103+ {
104+ buffer = " " // clear
105+
106+ // read serial buffer
107+ if (Serial.available () > 0 ) {
108+ buffer = Serial.readStringUntil (' \n ' // \n must be the command terminator
109+ if (buffer == " \n " " "
110+ return ; // abort if empty command
111+ }
112+ // send parsed status message if command == 0 received
113+ if (buffer == " 0"
114+ Serial.println (get_status () + " " // <---------- STATUS SEND
115+ return ;
116+ }
117+ } else {
118+ return ; // abort if no command
119+ }
120+
121+ // parse buffer
122+ if (buffer.length () > 0 ) {
123+ byte index = 0 ;
124+ byte ary[buffer.length () + 1 ];
125+ buffer.toCharArray (ary, buffer.length () + 1 );
126+ ptr = strtok (ary, " ," // command delimiter
127+ while (ptr != NULL ) {
128+ cmd[index ] = ptr;
129+ index ++;
130+ ptr = strtok (NULL , " ,"
131+ }
132+
133+ String tmp_cmd; // tmp string
134+
135+ // get servo positions
136+ if (index > 0 ) {
137+ tmp_cmd = String (cmd[0 ]); // servo X angle
138+ x = tmp_cmd.toInt (); // to integer
139+ }
140+ if (index > 1 ) {
141+ tmp_cmd = String (cmd[1 ]); // servo Y angle
142+ y = tmp_cmd.toInt (); // to integer
143+ }
144+
145+ // fix min/max angle
146+ if (x < SERVO_X_MIN) {
147+ x = SERVO_X_MIN;
148+ } else if (x > SERVO_X_MAX) {
149+ x = SERVO_X_MAX;
150+ }
151+ if (y < SERVO_Y_MIN) {
152+ y = SERVO_Y_MIN;
153+ } else if (y > SERVO_Y_MAX) {
154+ y = SERVO_Y_MAX;
155+ }
156+
157+ // fix min/max allowed movement angle
158+ if (x < SERVO_X_LIMIT_MIN) {
159+ x = SERVO_X_LIMIT_MIN;
160+ } else if (x > SERVO_X_LIMIT_MAX) {
161+ x = SERVO_X_LIMIT_MAX;
162+ }
163+ if (y < SERVO_Y_LIMIT_MIN) {
164+ y = SERVO_Y_LIMIT_MIN;
165+ } else if (y > SERVO_Y_LIMIT_MAX) {
166+ y = SERVO_Y_LIMIT_MAX;
167+ }
168+
169+ // move servo position
170+ servo_x.write (x); // move servo X
171+ servo_y.write (y); // move servo Y
172+
173+ // prepare rest of commands
174+ if (index > 2 ) {
175+ tmp_cmd = String (cmd[2 ]); // detected objects count
176+ if (tmp_cmd != " "
177+ counter = tmp_cmd.toInt (); // to integer
178+ }
179+ }
180+
181+ // get action states
182+ for (int i = 0 ; i < 6 ; i++) {
183+ int j = i + 3 ;
184+ if (index > j) {
185+ tmp_cmd = String (cmd[j]);
186+ if (tmp_cmd != " "
187+ actions[i] = (tmp_cmd.toInt () != 0 ); // to bool
188+ }
189+ }
190+ }
191+
192+ // apply action commands to pins
193+ for (int i = 0 ; i < 6 ; i++) {
194+ if (actions[i] == true ) {
195+ digitalWrite (pins[i], HIGH); // enable action
196+ } else {
197+ digitalWrite (pins[i], LOW); // disable action
198+ }
199+ }
200+ }
201+
202+ // debug current position and command
203+ if (DEBUG == true ) {
204+ Serial.print (" CMD: "
205+ Serial.println (buffer);
206+ Serial.print (" POS: "
207+ Serial.println (servo_x.read ());
208+ Serial.println (servo_y.read ());
209+ Serial.println ();
210+ }
211+
212+ buffer = " " // clear serial buffer
213+ }
0 commit comments