myCobot 280 Face Tracking Robot – Python, MediaPipe & Arduino

This project transforms the myCobot 280 into a smooth real-time 2D face-tracking robot using Python, MediaPipe, OpenCV, and Arduino.

Nov 5, 2026

•

8 views

•

0 respects

•

GPL3+

Embedded

Monitoring

Robots

Devices & Components

myCobot 280 M5Stack 2023 - 6 DOF Collaborative Robot

Camera Flange 2.0 Version for myCobot

Hardware & Tools

Soldering Iron Kit

Software & Tools

Arduino IDE

Python 3

Project description

Code

Arduino Code

cpp

...

1// by mircemk October, 2026
2
3#include <M5Stack.h>
4#include <MyCobotBasic.h>
5
6MyCobotBasic myCobot;
7
8
9// =====================================================
10// VERSION
11// =====================================================
12// TEST 8H
13// J1 = Horizontal tracking
14// J4 = Vertical tracking
15// Added graphical HOR / VERT position bars
16// =====================================================
17
18
19// =====================================================
20// TRACKING SETTINGS
21// =====================================================
22
23#define J1_STEP               1.4f
24#define J4_STEP               1.4f
25
26// Horizontal tracking with J1
27#define J1_MIN              -60.0f
28#define J1_MAX               60.0f
29
30// Vertical +/-30 deg around J4 = 90 deg
31#define J4_MIN               60.0f
32#define J4_MAX              120.0f
33
34#define JOG_SPEED             100
35#define HOME_SPEED             60
36
37#define JOINT_INTERVAL_MS      40
38
39
40// =====================================================
41// TRACKING HOME POSITION
42// =====================================================
43
44#define J1_HOME      0
45#define J2_HOME      0
46#define J3_HOME    -90
47#define J4_HOME     90
48#define J5_HOME      0
49#define J6_HOME    -45
50
51
52// =====================================================
53// SOUND
54// =====================================================
55
56#define SPEAKER_PIN   25
57#define DAC_VOL_HI   160
58#define DAC_VOL_LO     0
59
60bool soundEnabled = true;
61
62
63// =====================================================
64// CURRENT POSITION
65// =====================================================
66
67float j1Position = J1_HOME;
68float j4Position = J4_HOME;
69
70unsigned long lastJ1MoveTime = 0;
71unsigned long lastJ4MoveTime = 0;
72
73
74// =====================================================
75// STATUS
76// =====================================================
77
78String currentState = "READY";
79String lastTrackingStatus = "WAITING";
80
81bool manualMoveActive = false;
82
83
84// =====================================================
85// GRAPHICAL POSITION BARS
86// =====================================================
87
88// -----------------------------------------------------
89// VERTICAL BAR - LEFT SIDE
90// -----------------------------------------------------
91
92// -----------------------------------------------------
93// VERTICAL BAR - LEFT SIDE
94// -----------------------------------------------------
95
96#define VBAR_X       8
97#define VBAR_Y      10
98#define VBAR_W      12
99#define VBAR_H     204
100
101
102// -----------------------------------------------------
103// HORIZONTAL BAR - BOTTOM
104// -----------------------------------------------------
105
106#define HBAR_X      58
107#define HBAR_Y     222
108#define HBAR_W     245
109#define HBAR_H      12
110
111
112// =====================================================
113// 5x5 RGB LED
114// =====================================================
115
116void setStatusLED(int r, int g, int b)
117{
118  myCobot.setLEDRGB(r, g, b);
119}
120
121
122void setBlueLED()
123{
124  setStatusLED(0, 0, 255);
125}
126
127
128void setGreenLED()
129{
130  setStatusLED(0, 255, 0);
131}
132
133
134void setRedLED()
135{
136  setStatusLED(255, 0, 0);
137}
138
139
140void setYellowLED()
141{
142  setStatusLED(255, 255, 0);
143}
144
145
146// =====================================================
147// SPEAKER
148// =====================================================
149
150void initSpeaker()
151{
152  pinMode(SPEAKER_PIN, OUTPUT);
153
154  dacWrite(
155    SPEAKER_PIN,
156    DAC_VOL_LO
157  );
158}
159
160
161void stopTone()
162{
163  dacWrite(
164    SPEAKER_PIN,
165    DAC_VOL_LO
166  );
167}
168
169
170void playTone(
171  int freq,
172  int duration_ms
173)
174{
175  if (!soundEnabled)
176  {
177    return;
178  }
179
180  if (
181    freq <= 0
182    ||
183    duration_ms <= 0
184  )
185  {
186    return;
187  }
188
189
190  const uint32_t total_us =
191    (uint32_t)duration_ms * 1000UL;
192
193
194  const uint32_t half_period_us =
195    500000UL / (uint32_t)freq;
196
197
198  if (half_period_us == 0)
199  {
200    return;
201  }
202
203
204  const uint32_t startTime =
205    micros();
206
207
208  while (
209    micros() - startTime
210    <
211    total_us
212  )
213  {
214    dacWrite(
215      SPEAKER_PIN,
216      DAC_VOL_HI
217    );
218
219    delayMicroseconds(
220      half_period_us
221    );
222
223
224    dacWrite(
225      SPEAKER_PIN,
226      DAC_VOL_LO
227    );
228
229    delayMicroseconds(
230      half_period_us
231    );
232
233
234    yield();
235  }
236
237
238  stopTone();
239}
240
241
242// =====================================================
243// SOUND EFFECTS
244// =====================================================
245
246// HOME sound from original RPS project
247void homeSound()
248{
249  playTone(
250    700,
251    60
252  );
253}
254
255
256// CENTER / LOCKED
257void centerSound()
258{
259  playTone(
260    1800,
261    80
262  );
263}
264
265
266// REAL FACE LOST
267void noFaceSound()
268{
269  playTone(
270    320,
271    450
272  );
273}
274
275
276// =====================================================
277// STATIC BAR FRAMES
278// =====================================================
279
280void drawBarFrames()
281{
282  // ---------------------------------------------------
283  // VERTICAL BAR
284  // ---------------------------------------------------
285
286  M5.Lcd.drawRect(
287    VBAR_X,
288    VBAR_Y,
289    VBAR_W,
290    VBAR_H,
291    WHITE
292  );
293
294
295  int verticalCenter =
296    VBAR_Y + VBAR_H / 2;
297
298
299  M5.Lcd.drawFastHLine(
300    VBAR_X,
301    verticalCenter,
302    VBAR_W,
303    WHITE
304  );
305
306
307  // ---------------------------------------------------
308  // HORIZONTAL BAR
309  // ---------------------------------------------------
310
311  M5.Lcd.drawRect(
312    HBAR_X,
313    HBAR_Y,
314    HBAR_W,
315    HBAR_H,
316    WHITE
317  );
318
319
320  int horizontalCenter =
321    HBAR_X + HBAR_W / 2;
322
323
324  M5.Lcd.drawFastVLine(
325    horizontalCenter,
326    HBAR_Y,
327    HBAR_H,
328    WHITE
329  );
330}
331
332
333// =====================================================
334// HORIZONTAL POSITION BAR
335// J1 = -60 ... 0 ... +60
336// =====================================================
337
338void updateHorizontalBar()
339{
340  // Clear inside only.
341  M5.Lcd.fillRect(
342    HBAR_X + 1,
343    HBAR_Y + 1,
344    HBAR_W - 2,
345    HBAR_H - 2,
346    BLACK
347  );
348
349
350  int centerX =
351    HBAR_X + HBAR_W / 2;
352
353
354  int insideHeight =
355    HBAR_H - 2;
356
357
358  // Available width from center
359  // to one end of the bar.
360  int halfWidth =
361    (HBAR_W - 3) / 2;
362
363
364  if (j1Position > 0.0f)
365  {
366    float ratio =
367      j1Position / J1_MAX;
368
369
370    if (ratio > 1.0f)
371    {
372      ratio = 1.0f;
373    }
374
375
376    int fillWidth =
377      (int)(
378        ratio * halfWidth
379      );
380
381
382    if (fillWidth > 0)
383    {
384      M5.Lcd.fillRect(
385        centerX + 1,
386        HBAR_Y + 1,
387        fillWidth,
388        insideHeight,
389        YELLOW
390      );
391    }
392  }
393
394
395  else if (j1Position < 0.0f)
396  {
397    float ratio =
398      (-j1Position) / (-J1_MIN);
399
400
401    if (ratio > 1.0f)
402    {
403      ratio = 1.0f;
404    }
405
406
407    int fillWidth =
408      (int)(
409        ratio * halfWidth
410      );
411
412
413    if (fillWidth > 0)
414    {
415      M5.Lcd.fillRect(
416        centerX - fillWidth,
417        HBAR_Y + 1,
418        fillWidth,
419        insideHeight,
420        YELLOW
421      );
422    }
423  }
424
425
426  // Redraw center zero line.
427  M5.Lcd.drawFastVLine(
428    centerX,
429    HBAR_Y,
430    HBAR_H,
431    WHITE
432  );
433
434
435  // Redraw frame.
436  M5.Lcd.drawRect(
437    HBAR_X,
438    HBAR_Y,
439    HBAR_W,
440    HBAR_H,
441    WHITE
442  );
443}
444
445
446// =====================================================
447// VERTICAL POSITION BAR
448// J4 relative position = -30 ... 0 ... +30
449// =====================================================
450
451void updateVerticalBar()
452{
453  // Clear inside only.
454  M5.Lcd.fillRect(
455    VBAR_X + 1,
456    VBAR_Y + 1,
457    VBAR_W - 2,
458    VBAR_H - 2,
459    BLACK
460  );
461
462
463  int centerY =
464    VBAR_Y + VBAR_H / 2;
465
466
467  int insideWidth =
468    VBAR_W - 2;
469
470
471  int halfHeight =
472    (VBAR_H - 3) / 2;
473
474
475  float verticalPosition =
476    j4Position - J4_HOME;
477
478
479  // ---------------------------------------------------
480  // POSITIVE = UP
481  // ---------------------------------------------------
482
483  if (verticalPosition > 0.0f)
484  {
485    float maxPositive =
486      J4_MAX - J4_HOME;
487
488
489    float ratio =
490      verticalPosition /
491      maxPositive;
492
493
494    if (ratio > 1.0f)
495    {
496      ratio = 1.0f;
497    }
498
499
500    int fillHeight =
501      (int)(
502        ratio * halfHeight
503      );
504
505
506    if (fillHeight > 0)
507    {
508      M5.Lcd.fillRect(
509        VBAR_X + 1,
510        centerY - fillHeight,
511        insideWidth,
512        fillHeight,
513        RED
514      );
515    }
516  }
517
518
519  // ---------------------------------------------------
520  // NEGATIVE = DOWN
521  // ---------------------------------------------------
522
523  else if (verticalPosition < 0.0f)
524  {
525    float maxNegative =
526      J4_HOME - J4_MIN;
527
528
529    float ratio =
530      (-verticalPosition) /
531      maxNegative;
532
533
534    if (ratio > 1.0f)
535    {
536      ratio = 1.0f;
537    }
538
539
540    int fillHeight =
541      (int)(
542        ratio * halfHeight
543      );
544
545
546    if (fillHeight > 0)
547    {
548      M5.Lcd.fillRect(
549        VBAR_X + 1,
550        centerY + 1,
551        insideWidth,
552        fillHeight,
553        RED
554      );
555    }
556  }
557
558
559  // Redraw center zero line.
560  M5.Lcd.drawFastHLine(
561    VBAR_X,
562    centerY,
563    VBAR_W,
564    WHITE
565  );
566
567
568  // Redraw frame.
569  M5.Lcd.drawRect(
570    VBAR_X,
571    VBAR_Y,
572    VBAR_W,
573    VBAR_H,
574    WHITE
575  );
576}
577
578
579// =====================================================
580// UPDATE BOTH POSITION BARS
581// =====================================================
582
583void updatePositionBars()
584{
585  updateHorizontalBar();
586  updateVerticalBar();
587}
588
589
590// =====================================================
591// DISPLAY
592// =====================================================
593
594void showScreen()
595{
596  M5.Lcd.fillScreen(
597    BLACK
598  );
599
600
601  // ---------------------------------------------------
602  // TITLE
603  // ---------------------------------------------------
604
605  M5.Lcd.setTextColor(
606    CYAN
607  );
608
609  M5.Lcd.setTextSize(
610    3
611  );
612
613  M5.Lcd.setCursor(
614    45,
615    10
616  );
617
618  M5.Lcd.println(
619    "2D TRACKING"
620  );
621
622
623  // ---------------------------------------------------
624  // STATUS
625  // ---------------------------------------------------
626
627  M5.Lcd.setTextSize(
628    2
629  );
630
631  M5.Lcd.setTextColor(
632    WHITE
633  );
634
635  M5.Lcd.setCursor(
636    32,
637    55
638  );
639
640  M5.Lcd.print(
641    "STATUS: "
642  );
643
644
645  M5.Lcd.setTextColor(
646    YELLOW
647  );
648
649  M5.Lcd.println(
650    currentState
651  );
652
653
654  // ---------------------------------------------------
655  // HOR
656  // ---------------------------------------------------
657
658  M5.Lcd.setTextColor(
659    WHITE
660  );
661
662  M5.Lcd.setCursor(
663    32,
664    95
665  );
666
667  M5.Lcd.print(
668    "HOR:"
669  );
670
671
672  M5.Lcd.setTextColor(
673    GREEN
674  );
675
676  M5.Lcd.setCursor(
677    105,
678    95
679  );
680
681
682  if (j1Position >= 0)
683  {
684    M5.Lcd.print("+");
685  }
686
687
688  M5.Lcd.print(
689    j1Position,
690    1
691  );
692
693  M5.Lcd.print(
694    " deg"
695  );
696
697
698  // ---------------------------------------------------
699  // VERT
700  // ---------------------------------------------------
701
702  float verticalPosition =
703    j4Position - J4_HOME;
704
705
706  M5.Lcd.setTextColor(
707    WHITE
708  );
709
710  M5.Lcd.setCursor(
711    32,
712    130
713  );
714
715  M5.Lcd.print(
716    "VERT:"
717  );
718
719
720  M5.Lcd.setTextColor(
721    GREEN
722  );
723
724  M5.Lcd.setCursor(
725    105,
726    130
727  );
728
729
730  if (verticalPosition >= 0)
731  {
732    M5.Lcd.print("+");
733  }
734
735
736  M5.Lcd.print(
737    verticalPosition,
738    1
739  );
740
741  M5.Lcd.print(
742    " deg"
743  );
744
745
746  // ---------------------------------------------------
747  // BUTTON A
748  // ---------------------------------------------------
749
750  M5.Lcd.setTextColor(
751    WHITE
752  );
753
754  M5.Lcd.setCursor(
755    32,
756    170
757  );
758
759  M5.Lcd.println(
760    "A: CENTER (J1/J4)"
761  );
762
763
764  // ---------------------------------------------------
765  // BUTTON B
766  // ---------------------------------------------------
767
768  M5.Lcd.setCursor(
769    32,
770    200
771  );
772
773  M5.Lcd.print(
774    "B: HOME"
775  );
776
777
778  // ---------------------------------------------------
779  // BUTTON C
780  // ---------------------------------------------------
781
782  M5.Lcd.setCursor(
783    130,
784    200
785  );
786
787  M5.Lcd.print(
788    "C: SOUND "
789  );
790
791
792  if (soundEnabled)
793  {
794    M5.Lcd.setTextColor(
795      GREEN
796    );
797
798    M5.Lcd.print(
799      "ON"
800    );
801  }
802
803  else
804  {
805    M5.Lcd.setTextColor(
806      RED
807    );
808
809    M5.Lcd.print(
810      "OFF"
811    );
812  }
813
814
815  // ---------------------------------------------------
816  // POSITION BAR FRAMES
817  // ---------------------------------------------------
818
819  drawBarFrames();
820
821  updatePositionBars();
822}
823
824
825// =====================================================
826// UPDATE DISPLAY
827// =====================================================
828
829void updateDisplay()
830{
831  // ---------------------------------------------------
832  // STATUS
833  // ---------------------------------------------------
834
835  M5.Lcd.fillRect(
836    115,
837    45,
838    200,
839    35,
840    BLACK
841  );
842
843
844  M5.Lcd.setTextColor(
845    YELLOW
846  );
847
848  M5.Lcd.setTextSize(
849    2
850  );
851
852  M5.Lcd.setCursor(
853    115,
854    55
855  );
856
857  M5.Lcd.print(
858    currentState
859  );
860
861
862  // ---------------------------------------------------
863  // HOR
864  // ---------------------------------------------------
865
866  M5.Lcd.fillRect(
867    105,
868    85,
869    180,
870    35,
871    BLACK
872  );
873
874
875  M5.Lcd.setTextColor(
876    GREEN
877  );
878
879  M5.Lcd.setCursor(
880    105,
881    95
882  );
883
884
885  if (j1Position >= 0)
886  {
887    M5.Lcd.print("+");
888  }
889
890
891  M5.Lcd.print(
892    j1Position,
893    1
894  );
895
896  M5.Lcd.print(
897    " deg"
898  );
899
900
901  // ---------------------------------------------------
902  // VERT
903  // ---------------------------------------------------
904
905  float verticalPosition =
906    j4Position - J4_HOME;
907
908
909  M5.Lcd.fillRect(
910    105,
911    120,
912    180,
913    35,
914    BLACK
915  );
916
917
918  M5.Lcd.setTextColor(
919    GREEN
920  );
921
922  M5.Lcd.setCursor(
923    105,
924    130
925  );
926
927
928  if (verticalPosition >= 0)
929  {
930    M5.Lcd.print("+");
931  }
932
933
934  M5.Lcd.print(
935    verticalPosition,
936    1
937  );
938
939  M5.Lcd.print(
940    " deg"
941  );
942
943
944  // ---------------------------------------------------
945  // SOUND
946  // ---------------------------------------------------
947
948  M5.Lcd.fillRect(
949    238,
950    190,
951    80,
952    30,
953    BLACK
954  );
955
956
957  M5.Lcd.setCursor(
958    238,
959    200
960  );
961
962
963  if (soundEnabled)
964  {
965    M5.Lcd.setTextColor(
966      GREEN
967    );
968
969    M5.Lcd.print(
970      "ON"
971    );
972  }
973
974  else
975  {
976    M5.Lcd.setTextColor(
977      RED
978    );
979
980    M5.Lcd.print(
981      "OFF"
982    );
983  }
984
985
986  // ---------------------------------------------------
987  // GRAPHICAL BARS
988  // ---------------------------------------------------
989
990  updatePositionBars();
991}
992
993
994// =====================================================
995// JOG_INCREMENT
996// =====================================================
997
998void jogIncrement(
999  byte joint,
1000  float increment,
1001  byte speed
1002)
1003{
1004  int16_t incrementInt =
1005    (int16_t)(
1006      increment * 100.0f
1007    );
1008
1009
1010  byte angleHigh =
1011    (byte)(
1012      (incrementInt >> 8)
1013      &
1014      0xFF
1015    );
1016
1017
1018  byte angleLow =
1019    (byte)(
1020      incrementInt
1021      &
1022      0xFF
1023    );
1024
1025
1026  Serial2.write(
1027    0xFE
1028  );
1029
1030  Serial2.write(
1031    0xFE
1032  );
1033
1034  Serial2.write(
1035    0x06
1036  );
1037
1038  Serial2.write(
1039    0x33
1040  );
1041
1042  Serial2.write(
1043    joint
1044  );
1045
1046  Serial2.write(
1047    angleHigh
1048  );
1049
1050  Serial2.write(
1051    angleLow
1052  );
1053
1054  Serial2.write(
1055    speed
1056  );
1057
1058  Serial2.write(
1059    0xFA
1060  );
1061}
1062
1063
1064// =====================================================
1065// LEFT
1066// J1 POSITIVE
1067// =====================================================
1068
1069void stepLeft()
1070{
1071  if (
1072    millis() - lastJ1MoveTime
1073    <
1074    JOINT_INTERVAL_MS
1075  )
1076  {
1077    return;
1078  }
1079
1080
1081  float newPosition =
1082    j1Position + J1_STEP;
1083
1084
1085  if (
1086    newPosition > J1_MAX
1087  )
1088  {
1089    newPosition =
1090      J1_MAX;
1091  }
1092
1093
1094  if (
1095    j1Position >= J1_MAX
1096  )
1097  {
1098    return;
1099  }
1100
1101
1102  float increment =
1103    newPosition - j1Position;
1104
1105
1106  jogIncrement(
1107    1,
1108    increment,
1109    JOG_SPEED
1110  );
1111
1112
1113  j1Position =
1114    newPosition;
1115
1116
1117  lastJ1MoveTime =
1118    millis();
1119
1120
1121  updateDisplay();
1122}
1123
1124
1125// =====================================================
1126// RIGHT
1127// J1 NEGATIVE
1128// =====================================================
1129
1130void stepRight()
1131{
1132  if (
1133    millis() - lastJ1MoveTime
1134    <
1135    JOINT_INTERVAL_MS
1136  )
1137  {
1138    return;
1139  }
1140
1141
1142  float newPosition =
1143    j1Position - J1_STEP;
1144
1145
1146  if (
1147    newPosition < J1_MIN
1148  )
1149  {
1150    newPosition =
1151      J1_MIN;
1152  }
1153
1154
1155  if (
1156    j1Position <= J1_MIN
1157  )
1158  {
1159    return;
1160  }
1161
1162
1163  float increment =
1164    newPosition - j1Position;
1165
1166
1167  jogIncrement(
1168    1,
1169    increment,
1170    JOG_SPEED
1171  );
1172
1173
1174  j1Position =
1175    newPosition;
1176
1177
1178  lastJ1MoveTime =
1179    millis();
1180
1181
1182  updateDisplay();
1183}
1184
1185
1186// =====================================================
1187// UP
1188// J4 POSITIVE
1189// =====================================================
1190
1191void stepUp()
1192{
1193  if (
1194    millis() - lastJ4MoveTime
1195    <
1196    JOINT_INTERVAL_MS
1197  )
1198  {
1199    return;
1200  }
1201
1202
1203  float newPosition =
1204    j4Position + J4_STEP;
1205
1206
1207  if (
1208    newPosition > J4_MAX
1209  )
1210  {
1211    newPosition =
1212      J4_MAX;
1213  }
1214
1215
1216  if (
1217    j4Position >= J4_MAX
1218  )
1219  {
1220    return;
1221  }
1222
1223
1224  float increment =
1225    newPosition - j4Position;
1226
1227
1228  jogIncrement(
1229    4,
1230    increment,
1231    JOG_SPEED
1232  );
1233
1234
1235  j4Position =
1236    newPosition;
1237
1238
1239  lastJ4MoveTime =
1240    millis();
1241
1242
1243  updateDisplay();
1244}
1245
1246
1247// =====================================================
1248// DOWN
1249// J4 NEGATIVE
1250// =====================================================
1251
1252void stepDown()
1253{
1254  if (
1255    millis() - lastJ4MoveTime
1256    <
1257    JOINT_INTERVAL_MS
1258  )
1259  {
1260    return;
1261  }
1262
1263
1264  float newPosition =
1265    j4Position - J4_STEP;
1266
1267
1268  if (
1269    newPosition < J4_MIN
1270  )
1271  {
1272    newPosition =
1273      J4_MIN;
1274  }
1275
1276
1277  if (
1278    j4Position <= J4_MIN
1279  )
1280  {
1281    return;
1282  }
1283
1284
1285  float increment =
1286    newPosition - j4Position;
1287
1288
1289  jogIncrement(
1290    4,
1291    increment,
1292    JOG_SPEED
1293  );
1294
1295
1296  j4Position =
1297    newPosition;
1298
1299
1300  lastJ4MoveTime =
1301    millis();
1302
1303
1304  updateDisplay();
1305}
1306
1307
1308// =====================================================
1309// SERIAL HANDSHAKE
1310// =====================================================
1311
1312void clearPythonInput()
1313{
1314  while (
1315    Serial.available() > 0
1316  )
1317  {
1318    Serial.read();
1319  }
1320}
1321
1322
1323bool waitForPausedAck()
1324{
1325  unsigned long startTime =
1326    millis();
1327
1328
1329  while (
1330    millis() - startTime
1331    <
1332    600
1333  )
1334  {
1335    if (
1336      Serial.available() > 0
1337    )
1338    {
1339      String message =
1340        Serial.readStringUntil('\n');
1341
1342
1343      message.trim();
1344
1345      message.toUpperCase();
1346
1347
1348      if (
1349        message == "PAUSED_ACK"
1350      )
1351      {
1352        return true;
1353      }
1354    }
1355
1356
1357    delay(1);
1358  }
1359
1360
1361  return false;
1362}
1363
1364
1365void pausePythonTracking()
1366{
1367  clearPythonInput();
1368
1369
1370  Serial.println(
1371    "PAUSE_TRACKING"
1372  );
1373
1374
1375  Serial.flush();
1376
1377
1378  waitForPausedAck();
1379
1380
1381  clearPythonInput();
1382}
1383
1384
1385void resumePythonWaitingForFace()
1386{
1387  Serial.println(
1388    "RESUME_WAIT_FACE"
1389  );
1390
1391
1392  Serial.flush();
1393}
1394
1395
1396// =====================================================
1397// A = CENTER
1398// J1 -> 0
1399// J4 -> 90
1400// =====================================================
1401
1402void centerTracking()
1403{
1404  manualMoveActive =
1405    true;
1406
1407
1408  pausePythonTracking();
1409
1410
1411  currentState =
1412    "CENTERING";
1413
1414
1415  updateDisplay();
1416
1417
1418  // ---------------------------------------------------
1419  // J1 -> CENTER
1420  // ---------------------------------------------------
1421
1422  myCobot.writeAngle(
1423    1,
1424    J1_HOME,
1425    HOME_SPEED
1426  );
1427
1428
1429  delay(800);
1430
1431
1432  // ---------------------------------------------------
1433  // J4 -> CENTER
1434  // ---------------------------------------------------
1435
1436  myCobot.writeAngle(
1437    4,
1438    J4_HOME,
1439    HOME_SPEED
1440  );
1441
1442
1443  delay(800);
1444
1445
1446  // ---------------------------------------------------
1447  // SOFTWARE POSITION RESET
1448  // ---------------------------------------------------
1449
1450  j1Position =
1451    J1_HOME;
1452
1453
1454  j4Position =
1455    J4_HOME;
1456
1457
1458  lastJ1MoveTime =
1459    millis();
1460
1461
1462  lastJ4MoveTime =
1463    millis();
1464
1465
1466  // ---------------------------------------------------
1467  // FINAL CENTER STATE
1468  // ---------------------------------------------------
1469
1470  currentState =
1471    "CENTER";
1472
1473
1474  lastTrackingStatus =
1475    "WAITING";
1476
1477
1478  // LED after motor movement.
1479  setBlueLED();
1480
1481
1482  updateDisplay();
1483
1484
1485  centerSound();
1486
1487
1488  manualMoveActive =
1489    false;
1490
1491
1492  resumePythonWaitingForFace();
1493}
1494
1495
1496// =====================================================
1497// B = FULL HOME
1498// =====================================================
1499
1500void moveToHome()
1501{
1502  manualMoveActive =
1503    true;
1504
1505
1506  pausePythonTracking();
1507
1508
1509  currentState =
1510    "MOVING HOME";
1511
1512
1513  updateDisplay();
1514
1515
1516  // ---------------------------------------------------
1517  // TRACKING HOME POSITION
1518  // ---------------------------------------------------
1519
1520  myCobot.writeAngle(
1521    1,
1522    J1_HOME,
1523    HOME_SPEED
1524  );
1525
1526  delay(600);
1527
1528
1529  myCobot.writeAngle(
1530    2,
1531    J2_HOME,
1532    HOME_SPEED
1533  );
1534
1535  delay(600);
1536
1537
1538  myCobot.writeAngle(
1539    3,
1540    J3_HOME,
1541    HOME_SPEED
1542  );
1543
1544  delay(800);
1545
1546
1547  myCobot.writeAngle(
1548    4,
1549    J4_HOME,
1550    HOME_SPEED
1551  );
1552
1553  delay(700);
1554
1555
1556  myCobot.writeAngle(
1557    5,
1558    J5_HOME,
1559    HOME_SPEED
1560  );
1561
1562  delay(600);
1563
1564
1565  myCobot.writeAngle(
1566    6,
1567    J6_HOME,
1568    HOME_SPEED
1569  );
1570
1571  delay(800);
1572
1573
1574  // ---------------------------------------------------
1575  // SOFTWARE POSITION RESET
1576  // ---------------------------------------------------
1577
1578  j1Position =
1579    J1_HOME;
1580
1581
1582  j4Position =
1583    J4_HOME;
1584
1585
1586  lastJ1MoveTime =
1587    millis();
1588
1589
1590  lastJ4MoveTime =
1591    millis();
1592
1593
1594  // ---------------------------------------------------
1595  // FINAL HOME STATE
1596  // ---------------------------------------------------
1597
1598  currentState =
1599    "HOME";
1600
1601
1602  lastTrackingStatus =
1603    "WAITING";
1604
1605
1606  setBlueLED();
1607
1608
1609  updateDisplay();
1610
1611
1612  homeSound();
1613
1614
1615  manualMoveActive =
1616    false;
1617
1618
1619  resumePythonWaitingForFace();
1620}
1621
1622
1623// =====================================================
1624// PYTHON STATUS
1625// =====================================================
1626
1627void setTrackingStatus(
1628  String newStatus
1629)
1630{
1631  if (
1632    manualMoveActive
1633  )
1634  {
1635    return;
1636  }
1637
1638
1639  if (
1640    newStatus ==
1641    lastTrackingStatus
1642  )
1643  {
1644    return;
1645  }
1646
1647
1648  lastTrackingStatus =
1649    newStatus;
1650
1651
1652  // ---------------------------------------------------
1653  // TRACKING
1654  // ---------------------------------------------------
1655
1656  if (
1657    newStatus == "TRACKING"
1658  )
1659  {
1660    currentState =
1661      "TRACKING";
1662
1663
1664    setGreenLED();
1665  }
1666
1667
1668  // ---------------------------------------------------
1669  // LOCKED
1670  // ---------------------------------------------------
1671
1672  else if (
1673    newStatus == "LOCKED"
1674  )
1675  {
1676    currentState =
1677      "LOCKED";
1678
1679
1680    setRedLED();
1681
1682
1683    centerSound();
1684  }
1685
1686
1687  // ---------------------------------------------------
1688  // REAL NO FACE
1689  // ---------------------------------------------------
1690
1691  else if (
1692    newStatus == "NOFACE"
1693  )
1694  {
1695    currentState =
1696      "NO FACE";
1697
1698
1699    setYellowLED();
1700
1701
1702    noFaceSound();
1703  }
1704
1705
1706  updateDisplay();
1707}
1708
1709
1710// =====================================================
1711// C = SOUND ON / OFF
1712// =====================================================
1713
1714void toggleSound()
1715{
1716  if (
1717    soundEnabled
1718  )
1719  {
1720    playTone(
1721      600,
1722      70
1723    );
1724
1725
1726    soundEnabled =
1727      false;
1728  }
1729
1730
1731  else
1732  {
1733    soundEnabled =
1734      true;
1735
1736
1737    playTone(
1738      1200,
1739      80
1740    );
1741  }
1742
1743
1744  updateDisplay();
1745}
1746
1747
1748// =====================================================
1749// SERIAL COMMAND PROCESSING
1750// =====================================================
1751
1752void processCommand(
1753  String command
1754)
1755{
1756  command.trim();
1757
1758  command.toUpperCase();
1759
1760
1761  if (
1762    manualMoveActive
1763  )
1764  {
1765    return;
1766  }
1767
1768
1769  if (
1770    command == "LEFT"
1771  )
1772  {
1773    stepLeft();
1774  }
1775
1776
1777  else if (
1778    command == "RIGHT"
1779  )
1780  {
1781    stepRight();
1782  }
1783
1784
1785  else if (
1786    command == "UP"
1787  )
1788  {
1789    stepUp();
1790  }
1791
1792
1793  else if (
1794    command == "DOWN"
1795  )
1796  {
1797    stepDown();
1798  }
1799
1800
1801  else if (
1802    command == "TRACKING"
1803  )
1804  {
1805    setTrackingStatus(
1806      "TRACKING"
1807    );
1808  }
1809
1810
1811  else if (
1812    command == "LOCKED"
1813  )
1814  {
1815    setTrackingStatus(
1816      "LOCKED"
1817    );
1818  }
1819
1820
1821  else if (
1822    command == "NOFACE"
1823  )
1824  {
1825    setTrackingStatus(
1826      "NOFACE"
1827    );
1828  }
1829}
1830
1831
1832// =====================================================
1833// SETUP
1834// =====================================================
1835
1836void setup()
1837{
1838  M5.begin();
1839
1840
1841  Serial.begin(
1842    115200
1843  );
1844
1845
1846  Serial.setTimeout(
1847    20
1848  );
1849
1850
1851  myCobot.setup();
1852
1853
1854  myCobot.powerOn();
1855
1856
1857  initSpeaker();
1858
1859
1860  delay(500);
1861
1862
1863  currentState =
1864    "READY";
1865
1866
1867  lastTrackingStatus =
1868    "WAITING";
1869
1870
1871  setBlueLED();
1872
1873
1874  showScreen();
1875}
1876
1877
1878// =====================================================
1879// LOOP
1880// =====================================================
1881
1882void loop()
1883{
1884  M5.update();
1885
1886
1887  // ---------------------------------------------------
1888  // A = CENTER
1889  // ---------------------------------------------------
1890
1891  if (
1892    M5.BtnA.wasPressed()
1893  )
1894  {
1895    centerTracking();
1896  }
1897
1898
1899  // ---------------------------------------------------
1900  // B = HOME
1901  // ---------------------------------------------------
1902
1903  if (
1904    M5.BtnB.wasPressed()
1905  )
1906  {
1907    moveToHome();
1908  }
1909
1910
1911  // ---------------------------------------------------
1912  // C = SOUND
1913  // ---------------------------------------------------
1914
1915  if (
1916    M5.BtnC.wasPressed()
1917  )
1918  {
1919    toggleSound();
1920  }
1921
1922
1923  // ---------------------------------------------------
1924  // PYTHON COMMANDS
1925  // ---------------------------------------------------
1926
1927  if (
1928    !manualMoveActive
1929    &&
1930    Serial.available() > 0
1931  )
1932  {
1933    String command =
1934      Serial.readStringUntil('\n');
1935
1936
1937    processCommand(
1938      command
1939    );
1940  }
1941
1942
1943  delay(1);
1944}

// by mircemk October, 2026

#include <M5Stack.h>
#include <MyCobotBasic.h>

MyCobotBasic myCobot;


// =====================================================
// VERSION
// =====================================================
// TEST 8H
// J1 = Horizontal tracking
// J4 = Vertical tracking
// Added graphical HOR / VERT position bars
// =====================================================


// =====================================================
// TRACKING SETTINGS
// =====================================================

#define J1_STEP               1.4f
#define J4_STEP               1.4f

// Horizontal tracking with J1
#define J1_MIN              -60.0f
#define J1_MAX               60.0f

// Vertical +/-30 deg around J4 = 90 deg
#define J4_MIN               60.0f
#define J4_MAX              120.0f

#define JOG_SPEED             100
#define HOME_SPEED             60

#define JOINT_INTERVAL_MS      40


// =====================================================
// TRACKING HOME POSITION
// =====================================================

#define J1_HOME      0
#define J2_HOME      0
#define J3_HOME    -90
#define J4_HOME     90
#define J5_HOME      0
#define J6_HOME    -45


// =====================================================
// SOUND
// =====================================================

#define SPEAKER_PIN   25
#define DAC_VOL_HI   160
#define DAC_VOL_LO     0

bool soundEnabled = true;


// =====================================================
// CURRENT POSITION
// =====================================================

float j1Position = J1_HOME;
float j4Position = J4_HOME;

unsigned long lastJ1MoveTime = 0;
unsigned long lastJ4MoveTime = 0;


// =====================================================
// STATUS
// =====================================================

String currentState = "READY";
String lastTrackingStatus = "WAITING";

bool manualMoveActive = false;


// =====================================================
// GRAPHICAL POSITION BARS
// =====================================================

// -----------------------------------------------------
// VERTICAL BAR - LEFT SIDE
// -----------------------------------------------------

// -----------------------------------------------------
// VERTICAL BAR - LEFT SIDE
// -----------------------------------------------------

#define VBAR_X       8
#define VBAR_Y      10
#define VBAR_W      12
#define VBAR_H     204


// -----------------------------------------------------
// HORIZONTAL BAR - BOTTOM
// -----------------------------------------------------

#define HBAR_X      58
#define HBAR_Y     222
#define HBAR_W     245
#define HBAR_H      12


// =====================================================
// 5x5 RGB LED
// =====================================================

void setStatusLED(int r, int g, int b)
{
  myCobot.setLEDRGB(r, g, b);
}


void setBlueLED()
{
  setStatusLED(0, 0, 255);
}


void setGreenLED()
{
  setStatusLED(0, 255, 0);
}


void setRedLED()
{
  setStatusLED(255, 0, 0);
}


void setYellowLED()
{
  setStatusLED(255, 255, 0);
}


// =====================================================
// SPEAKER
// =====================================================

void initSpeaker()
{
  pinMode(SPEAKER_PIN, OUTPUT);

  dacWrite(
    SPEAKER_PIN,
    DAC_VOL_LO
  );
}


void stopTone()
{
  dacWrite(
    SPEAKER_PIN,
    DAC_VOL_LO
  );
}


void playTone(
  int freq,
  int duration_ms
)
{
  if (!soundEnabled)
  {
    return;
  }

  if (
    freq <= 0
    ||
    duration_ms <= 0
  )
  {
    return;
  }


  const uint32_t total_us =
    (uint32_t)duration_ms * 1000UL;


  const uint32_t half_period_us =
    500000UL / (uint32_t)freq;


  if (half_period_us == 0)
  {
    return;
  }


  const uint32_t startTime =
    micros();


  while (
    micros() - startTime
    <
    total_us
  )
  {
    dacWrite(
      SPEAKER_PIN,
      DAC_VOL_HI
    );

    delayMicroseconds(
      half_period_us
    );


    dacWrite(
      SPEAKER_PIN,
      DAC_VOL_LO
    );

    delayMicroseconds(
      half_period_us
    );


    yield();
  }


  stopTone();
}


// =====================================================
// SOUND EFFECTS
// =====================================================

// HOME sound from original RPS project
void homeSound()
{
  playTone(
    700,
    60
  );
}


// CENTER / LOCKED
void centerSound()
{
  playTone(
    1800,
    80
  );
}


// REAL FACE LOST
void noFaceSound()
{
  playTone(
    320,
    450
  );
}


// =====================================================
// STATIC BAR FRAMES
// =====================================================

void drawBarFrames()
{
  // ---------------------------------------------------
  // VERTICAL BAR
  // ---------------------------------------------------

  M5.Lcd.drawRect(
    VBAR_X,
    VBAR_Y,
    VBAR_W,
    VBAR_H,
    WHITE
  );


  int verticalCenter =
    VBAR_Y + VBAR_H / 2;


  M5.Lcd.drawFastHLine(
    VBAR_X,
    verticalCenter,
    VBAR_W,
    WHITE
  );


  // ---------------------------------------------------
  // HORIZONTAL BAR
  // ---------------------------------------------------

  M5.Lcd.drawRect(
    HBAR_X,
    HBAR_Y,
    HBAR_W,
    HBAR_H,
    WHITE
  );


  int horizontalCenter =
    HBAR_X + HBAR_W / 2;


  M5.Lcd.drawFastVLine(
    horizontalCenter,
    HBAR_Y,
    HBAR_H,
    WHITE
  );
}


// =====================================================
// HORIZONTAL POSITION BAR
// J1 = -60 ... 0 ... +60
// =====================================================

void updateHorizontalBar()
{
  // Clear inside only.
  M5.Lcd.fillRect(
    HBAR_X + 1,
    HBAR_Y + 1,
    HBAR_W - 2,
    HBAR_H - 2,
    BLACK
  );


  int centerX =
    HBAR_X + HBAR_W / 2;


  int insideHeight =
    HBAR_H - 2;


  // Available width from center
  // to one end of the bar.
  int halfWidth =
    (HBAR_W - 3) / 2;


  if (j1Position > 0.0f)
  {
    float ratio =
      j1Position / J1_MAX;


    if (ratio > 1.0f)
    {
      ratio = 1.0f;
    }


    int fillWidth =
      (int)(
        ratio * halfWidth
      );


    if (fillWidth > 0)
    {
      M5.Lcd.fillRect(
        centerX + 1,
        HBAR_Y + 1,
        fillWidth,
        insideHeight,
        YELLOW
      );
    }
  }


  else if (j1Position < 0.0f)
  {
    float ratio =
      (-j1Position) / (-J1_MIN);


    if (ratio > 1.0f)
    {
      ratio = 1.0f;
    }


    int fillWidth =
      (int)(
        ratio * halfWidth
      );


    if (fillWidth > 0)
    {
      M5.Lcd.fillRect(
        centerX - fillWidth,
        HBAR_Y + 1,
        fillWidth,
        insideHeight,
        YELLOW
      );
    }
  }


  // Redraw center zero line.
  M5.Lcd.drawFastVLine(
    centerX,
    HBAR_Y,
    HBAR_H,
    WHITE
  );


  // Redraw frame.
  M5.Lcd.drawRect(
    HBAR_X,
    HBAR_Y,
    HBAR_W,
    HBAR_H,
    WHITE
  );
}


// =====================================================
// VERTICAL POSITION BAR
// J4 relative position = -30 ... 0 ... +30
// =====================================================

void updateVerticalBar()
{
  // Clear inside only.
  M5.Lcd.fillRect(
    VBAR_X + 1,
    VBAR_Y + 1,
    VBAR_W - 2,
    VBAR_H - 2,
    BLACK
  );


  int centerY =
    VBAR_Y + VBAR_H / 2;


  int insideWidth =
    VBAR_W - 2;


  int halfHeight =
    (VBAR_H - 3) / 2;


  float verticalPosition =
    j4Position - J4_HOME;


  // ---------------------------------------------------
  // POSITIVE = UP
  // ---------------------------------------------------

  if (verticalPosition > 0.0f)
  {
    float maxPositive =
      J4_MAX - J4_HOME;


    float ratio =
      verticalPosition /
      maxPositive;


    if (ratio > 1.0f)
    {
      ratio = 1.0f;
    }


    int fillHeight =
      (int)(
        ratio * halfHeight
      );


    if (fillHeight > 0)
    {
      M5.Lcd.fillRect(
        VBAR_X + 1,
        centerY - fillHeight,
        insideWidth,
        fillHeight,
        RED
      );
    }
  }


  // ---------------------------------------------------
  // NEGATIVE = DOWN
  // ---------------------------------------------------

  else if (verticalPosition < 0.0f)
  {
    float maxNegative =
      J4_HOME - J4_MIN;


    float ratio =
      (-verticalPosition) /
      maxNegative;


    if (ratio > 1.0f)
    {
      ratio = 1.0f;
    }


    int fillHeight =
      (int)(
        ratio * halfHeight
      );


    if (fillHeight > 0)
    {
      M5.Lcd.fillRect(
        VBAR_X + 1,
        centerY + 1,
        insideWidth,
        fillHeight,
        RED
      );
    }
  }


  // Redraw center zero line.
  M5.Lcd.drawFastHLine(
    VBAR_X,
    centerY,
    VBAR_W,
    WHITE
  );


  // Redraw frame.
  M5.Lcd.drawRect(
    VBAR_X,
    VBAR_Y,
    VBAR_W,
    VBAR_H,
    WHITE
  );
}


// =====================================================
// UPDATE BOTH POSITION BARS
// =====================================================

void updatePositionBars()
{
  updateHorizontalBar();
  updateVerticalBar();
}


// =====================================================
// DISPLAY
// =====================================================

void showScreen()
{
  M5.Lcd.fillScreen(
    BLACK
  );


  // ---------------------------------------------------
  // TITLE
  // ---------------------------------------------------

  M5.Lcd.setTextColor(
    CYAN
  );

  M5.Lcd.setTextSize(
    3
  );

  M5.Lcd.setCursor(
    45,
    10
  );

  M5.Lcd.println(
    "2D TRACKING"
  );


  // ---------------------------------------------------
  // STATUS
  // ---------------------------------------------------

  M5.Lcd.setTextSize(
    2
  );

  M5.Lcd.setTextColor(
    WHITE
  );

  M5.Lcd.setCursor(
    32,
    55
  );

  M5.Lcd.print(
    "STATUS: "
  );


  M5.Lcd.setTextColor(
    YELLOW
  );

  M5.Lcd.println(
    currentState
  );


  // ---------------------------------------------------
  // HOR
  // ---------------------------------------------------

  M5.Lcd.setTextColor(
    WHITE
  );

  M5.Lcd.setCursor(
    32,
    95
  );

  M5.Lcd.print(
    "HOR:"
  );


  M5.Lcd.setTextColor(
    GREEN
  );

  M5.Lcd.setCursor(
    105,
    95
  );


  if (j1Position >= 0)
  {
    M5.Lcd.print("+");
  }


  M5.Lcd.print(
    j1Position,
    1
  );

  M5.Lcd.print(
    " deg"
  );


  // ---------------------------------------------------
  // VERT
  // ---------------------------------------------------

  float verticalPosition =
    j4Position - J4_HOME;


  M5.Lcd.setTextColor(
    WHITE
  );

  M5.Lcd.setCursor(
    32,
    130
  );

  M5.Lcd.print(
    "VERT:"
  );


  M5.Lcd.setTextColor(
    GREEN
  );

  M5.Lcd.setCursor(
    105,
    130
  );


  if (verticalPosition >= 0)
  {
    M5.Lcd.print("+");
  }


  M5.Lcd.print(
    verticalPosition,
    1
  );

  M5.Lcd.print(
    " deg"
  );


  // ---------------------------------------------------
  // BUTTON A
  // ---------------------------------------------------

  M5.Lcd.setTextColor(
    WHITE
  );

  M5.Lcd.setCursor(
    32,
    170
  );

  M5.Lcd.println(
    "A: CENTER (J1/J4)"
  );


  // ---------------------------------------------------
  // BUTTON B
  // ---------------------------------------------------

  M5.Lcd.setCursor(
    32,
    200
  );

  M5.Lcd.print(
    "B: HOME"
  );


  // ---------------------------------------------------
  // BUTTON C
  // ---------------------------------------------------

  M5.Lcd.setCursor(
    130,
    200
  );

  M5.Lcd.print(
    "C: SOUND "
  );


  if (soundEnabled)
  {
    M5.Lcd.setTextColor(
      GREEN
    );

    M5.Lcd.print(
      "ON"
    );
  }

  else
  {
    M5.Lcd.setTextColor(
      RED
    );

    M5.Lcd.print(
      "OFF"
    );
  }


  // ---------------------------------------------------
  // POSITION BAR FRAMES
  // ---------------------------------------------------

  drawBarFrames();

  updatePositionBars();
}


// =====================================================
// UPDATE DISPLAY
// =====================================================

void updateDisplay()
{
  // ---------------------------------------------------
  // STATUS
  // ---------------------------------------------------

  M5.Lcd.fillRect(
    115,
    45,
    200,
    35,
    BLACK
  );


  M5.Lcd.setTextColor(
    YELLOW
  );

  M5.Lcd.setTextSize(
    2
  );

  M5.Lcd.setCursor(
    115,
    55
  );

  M5.Lcd.print(
    currentState
  );


  // ---------------------------------------------------
  // HOR
  // ---------------------------------------------------

  M5.Lcd.fillRect(
    105,
    85,
    180,
    35,
    BLACK
  );


  M5.Lcd.setTextColor(
    GREEN
  );

  M5.Lcd.setCursor(
    105,
    95
  );


  if (j1Position >= 0)
  {
    M5.Lcd.print("+");
  }


  M5.Lcd.print(
    j1Position,
    1
  );

  M5.Lcd.print(
    " deg"
  );


  // ---------------------------------------------------
  // VERT
  // ---------------------------------------------------

  float verticalPosition =
    j4Position - J4_HOME;


  M5.Lcd.fillRect(
    105,
    120,
    180,
    35,
    BLACK
  );


  M5.Lcd.setTextColor(
    GREEN
  );

  M5.Lcd.setCursor(
    105,
    130
  );


  if (verticalPosition >= 0)
  {
    M5.Lcd.print("+");
  }


  M5.Lcd.print(
    verticalPosition,
    1
  );

  M5.Lcd.print(
    " deg"
  );


  // ---------------------------------------------------
  // SOUND
  // ---------------------------------------------------

  M5.Lcd.fillRect(
    238,
    190,
    80,
    30,
    BLACK
  );


  M5.Lcd.setCursor(
    238,
    200
  );


  if (soundEnabled)
  {
    M5.Lcd.setTextColor(
      GREEN
    );

    M5.Lcd.print(
      "ON"
    );
  }

  else
  {
    M5.Lcd.setTextColor(
      RED
    );

    M5.Lcd.print(
      "OFF"
    );
  }


  // ---------------------------------------------------
  // GRAPHICAL BARS
  // ---------------------------------------------------

  updatePositionBars();
}


// =====================================================
// JOG_INCREMENT
// =====================================================

void jogIncrement(
  byte joint,
  float increment,
  byte speed
)
{
  int16_t incrementInt =
    (int16_t)(
      increment * 100.0f
    );


  byte angleHigh =
    (byte)(
      (incrementInt >> 8)
      &
      0xFF
    );


  byte angleLow =
    (byte)(
      incrementInt
      &
      0xFF
    );


  Serial2.write(
    0xFE
  );

  Serial2.write(
    0xFE
  );

  Serial2.write(
    0x06
  );

  Serial2.write(
    0x33
  );

  Serial2.write(
    joint
  );

  Serial2.write(
    angleHigh
  );

  Serial2.write(
    angleLow
  );

  Serial2.write(
    speed
  );

  Serial2.write(
    0xFA
  );
}


// =====================================================
// LEFT
// J1 POSITIVE
// =====================================================

void stepLeft()
{
  if (
    millis() - lastJ1MoveTime
    <
    JOINT_INTERVAL_MS
  )
  {
    return;
  }


  float newPosition =
    j1Position + J1_STEP;


  if (
    newPosition > J1_MAX
  )
  {
    newPosition =
      J1_MAX;
  }


  if (
    j1Position >= J1_MAX
  )
  {
    return;
  }


  float increment =
    newPosition - j1Position;


  jogIncrement(
    1,
    increment,
    JOG_SPEED
  );


  j1Position =
    newPosition;


  lastJ1MoveTime =
    millis();


  updateDisplay();
}


// =====================================================
// RIGHT
// J1 NEGATIVE
// =====================================================

void stepRight()
{
  if (
    millis() - lastJ1MoveTime
    <
    JOINT_INTERVAL_MS
  )
  {
    return;
  }


  float newPosition =
    j1Position - J1_STEP;


  if (
    newPosition < J1_MIN
  )
  {
    newPosition =
      J1_MIN;
  }


  if (
    j1Position <= J1_MIN
  )
  {
    return;
  }


  float increment =
    newPosition - j1Position;


  jogIncrement(
    1,
    increment,
    JOG_SPEED
  );


  j1Position =
    newPosition;


  lastJ1MoveTime =
    millis();


  updateDisplay();
}


// =====================================================
// UP
// J4 POSITIVE
// =====================================================

void stepUp()
{
  if (
    millis() - lastJ4MoveTime
    <
    JOINT_INTERVAL_MS
  )
  {
    return;
  }


  float newPosition =
    j4Position + J4_STEP;


  if (
    newPosition > J4_MAX
  )
  {
    newPosition =
      J4_MAX;
  }


  if (
    j4Position >= J4_MAX
  )
  {
    return;
  }


  float increment =
    newPosition - j4Position;


  jogIncrement(
    4,
    increment,
    JOG_SPEED
  );


  j4Position =
    newPosition;


  lastJ4MoveTime =
    millis();


  updateDisplay();
}


// =====================================================
// DOWN
// J4 NEGATIVE
// =====================================================

void stepDown()
{
  if (
    millis() - lastJ4MoveTime
    <
    JOINT_INTERVAL_MS
  )
  {
    return;
  }


  float newPosition =
    j4Position - J4_STEP;


  if (
    newPosition < J4_MIN
  )
  {
    newPosition =
      J4_MIN;
  }


  if (
    j4Position <= J4_MIN
  )
  {
    return;
  }


  float increment =
    newPosition - j4Position;


  jogIncrement(
    4,
    increment,
    JOG_SPEED
  );


  j4Position =
    newPosition;


  lastJ4MoveTime =
    millis();


  updateDisplay();
}


// =====================================================
// SERIAL HANDSHAKE
// =====================================================

void clearPythonInput()
{
  while (
    Serial.available() > 0
  )
  {
    Serial.read();
  }
}


bool waitForPausedAck()
{
  unsigned long startTime =
    millis();


  while (
    millis() - startTime
    <
    600
  )
  {
    if (
      Serial.available() > 0
    )
    {
      String message =
        Serial.readStringUntil('\n');


      message.trim();

      message.toUpperCase();


      if (
        message == "PAUSED_ACK"
      )
      {
        return true;
      }
    }


    delay(1);
  }


  return false;
}


void pausePythonTracking()
{
  clearPythonInput();


  Serial.println(
    "PAUSE_TRACKING"
  );


  Serial.flush();


  waitForPausedAck();


  clearPythonInput();
}


void resumePythonWaitingForFace()
{
  Serial.println(
    "RESUME_WAIT_FACE"
  );


  Serial.flush();
}


// =====================================================
// A = CENTER
// J1 -> 0
// J4 -> 90
// =====================================================

void centerTracking()
{
  manualMoveActive =
    true;


  pausePythonTracking();


  currentState =
    "CENTERING";


  updateDisplay();


  // ---------------------------------------------------
  // J1 -> CENTER
  // ---------------------------------------------------

  myCobot.writeAngle(
    1,
    J1_HOME,
    HOME_SPEED
  );


  delay(800);


  // ---------------------------------------------------
  // J4 -> CENTER
  // ---------------------------------------------------

  myCobot.writeAngle(
    4,
    J4_HOME,
    HOME_SPEED
  );


  delay(800);


  // ---------------------------------------------------
  // SOFTWARE POSITION RESET
  // ---------------------------------------------------

  j1Position =
    J1_HOME;


  j4Position =
    J4_HOME;


  lastJ1MoveTime =
    millis();


  lastJ4MoveTime =
    millis();


  // ---------------------------------------------------
  // FINAL CENTER STATE
  // ---------------------------------------------------

  currentState =
    "CENTER";


  lastTrackingStatus =
    "WAITING";


  // LED after motor movement.
  setBlueLED();


  updateDisplay();


  centerSound();


  manualMoveActive =
    false;


  resumePythonWaitingForFace();
}


// =====================================================
// B = FULL HOME
// =====================================================

void moveToHome()
{
  manualMoveActive =
    true;


  pausePythonTracking();


  currentState =
    "MOVING HOME";


  updateDisplay();


  // ---------------------------------------------------
  // TRACKING HOME POSITION
  // ---------------------------------------------------

  myCobot.writeAngle(
    1,
    J1_HOME,
    HOME_SPEED
  );

  delay(600);


  myCobot.writeAngle(
    2,
    J2_HOME,
    HOME_SPEED
  );

  delay(600);


  myCobot.writeAngle(
    3,
    J3_HOME,
    HOME_SPEED
  );

  delay(800);


  myCobot.writeAngle(
    4,
    J4_HOME,
    HOME_SPEED
  );

  delay(700);


  myCobot.writeAngle(
    5,
    J5_HOME,
    HOME_SPEED
  );

  delay(600);


  myCobot.writeAngle(
    6,
    J6_HOME,
    HOME_SPEED
  );

  delay(800);


  // ---------------------------------------------------
  // SOFTWARE POSITION RESET
  // ---------------------------------------------------

  j1Position =
    J1_HOME;


  j4Position =
    J4_HOME;


  lastJ1MoveTime =
    millis();


  lastJ4MoveTime =
    millis();


  // ---------------------------------------------------
  // FINAL HOME STATE
  // ---------------------------------------------------

  currentState =
    "HOME";


  lastTrackingStatus =
    "WAITING";


  setBlueLED();


  updateDisplay();


  homeSound();


  manualMoveActive =
    false;


  resumePythonWaitingForFace();
}


// =====================================================
// PYTHON STATUS
// =====================================================

void setTrackingStatus(
  String newStatus
)
{
  if (
    manualMoveActive
  )
  {
    return;
  }


  if (
    newStatus ==
    lastTrackingStatus
  )
  {
    return;
  }


  lastTrackingStatus =
    newStatus;


  // ---------------------------------------------------
  // TRACKING
  // ---------------------------------------------------

  if (
    newStatus == "TRACKING"
  )
  {
    currentState =
      "TRACKING";


    setGreenLED();
  }


  // ---------------------------------------------------
  // LOCKED
  // ---------------------------------------------------

  else if (
    newStatus == "LOCKED"
  )
  {
    currentState =
      "LOCKED";


    setRedLED();


    centerSound();
  }


  // ---------------------------------------------------
  // REAL NO FACE
  // ---------------------------------------------------

  else if (
    newStatus == "NOFACE"
  )
  {
    currentState =
      "NO FACE";


    setYellowLED();


    noFaceSound();
  }


  updateDisplay();
}


// =====================================================
// C = SOUND ON / OFF
// =====================================================

void toggleSound()
{
  if (
    soundEnabled
  )
  {
    playTone(
      600,
      70
    );


    soundEnabled =
      false;
  }


  else
  {
    soundEnabled =
      true;


    playTone(
      1200,
      80
    );
  }


  updateDisplay();
}


// =====================================================
// SERIAL COMMAND PROCESSING
// =====================================================

void processCommand(
  String command
)
{
  command.trim();

  command.toUpperCase();


  if (
    manualMoveActive
  )
  {
    return;
  }


  if (
    command == "LEFT"
  )
  {
    stepLeft();
  }


  else if (
    command == "RIGHT"
  )
  {
    stepRight();
  }


  else if (
    command == "UP"
  )
  {
    stepUp();
  }


  else if (
    command == "DOWN"
  )
  {
    stepDown();
  }


  else if (
    command == "TRACKING"
  )
  {
    setTrackingStatus(
      "TRACKING"
    );
  }


  else if (
    command == "LOCKED"
  )
  {
    setTrackingStatus(
      "LOCKED"
    );
  }


  else if (
    command == "NOFACE"
  )
  {
    setTrackingStatus(
      "NOFACE"
    );
  }
}


// =====================================================
// SETUP
// =====================================================

void setup()
{
  M5.begin();


  Serial.begin(
    115200
  );


  Serial.setTimeout(
    20
  );


  myCobot.setup();


  myCobot.powerOn();


  initSpeaker();


  delay(500);


  currentState =
    "READY";


  lastTrackingStatus =
    "WAITING";


  setBlueLED();


  showScreen();
}


// =====================================================
// LOOP
// =====================================================

void loop()
{
  M5.update();


  // ---------------------------------------------------
  // A = CENTER
  // ---------------------------------------------------

  if (
    M5.BtnA.wasPressed()
  )
  {
    centerTracking();
  }


  // ---------------------------------------------------
  // B = HOME
  // ---------------------------------------------------

  if (
    M5.BtnB.wasPressed()
  )
  {
    moveToHome();
  }


  // ---------------------------------------------------
  // C = SOUND
  // ---------------------------------------------------

  if (
    M5.BtnC.wasPressed()
  )
  {
    toggleSound();
  }


  // ---------------------------------------------------
  // PYTHON COMMANDS
  // ---------------------------------------------------

  if (
    !manualMoveActive
    &&
    Serial.available() > 0
  )
  {
    String command =
      Serial.readStringUntil('\n');


    processCommand(
      command
    );
  }


  delay(1);
}

Downloadable files

Final Codes ZIP

...

Final Codes.zip

Comments

Only logged in users can leave comments

mircemk

0 Followers

•

0 Projects

Intro