-
Notifications
You must be signed in to change notification settings - Fork 0
/
4_darkpaw.cpp
2189 lines (1994 loc) · 72.6 KB
/
4_darkpaw.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
#include <iostream> /* for standard I/O in C++ */
#include <cstdio> /* for printf, cstdio in C++ */
#include <cstdint> /* for uint64 definition */
#include <cstdlib> /* for exit() definition */
#define _BSD_SOURCE /* this one makes time.h to work properly */
#include <sys/time.h> /* for clock_gettime */
#include <cmath> /* for mathematical funtions, cmath in C++ */
#include <pthread.h> /* for threading out loud*/
#include <pigpio.h> /* for handling the GPIO */
#include <csignal> /* for catching exceptions e.g. control-C, csignal in C++ */
#include <unistd.h> /* this one is to make usleep() work */
#include "mongoose.h" /* This one is for the web server */
#include <opencv2/core/core.hpp> /*this one and the ones that follow are the opencv stuff */
#include <opencv2/core/core_c.h>
#include <opencv2/core/utility.hpp>
#include <opencv2/core/persistence.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/background_segm.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/objdetect/objdetect.hpp>
#include <opencv2/imgcodecs/imgcodecs.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/tracking/tracker.hpp>
#include <opencv2/opencv_modules.hpp>
#include <opencv2/datasets/track_alov.hpp>
using namespace std;
using namespace cv;
using namespace cv::datasets;
/*The following structure(s) define all the inputs from the sensorial parts and interaction
with servos, they are meant to act as global variables for the threads*/
typedef struct
{
double distance; /*distance to the obstacle from the ultrasounds sensor*/
int ultrimpct; /*near frontal impact to the wall, from the ultrasounds */
int block; /* this is to avoid the robot from getting stuck in obstacles not detected by the sensors */
int global_position; /*this one is to store the legs global position from 1 to 8 */
int MPU6050;
float acce_x; /* Accelerometer X axis */
float acce_y; /* Accelerometer y axis */
float acce_z; /* Accelerometer z axis */
float gyro_x; /* Gyroscope X axis */
float gyro_y; /* Gyroscope y axis */
float gyro_z; /* Gyroscope z axis */
float camera_x; /* This is the x axis of the target coming from the camera */
int seek; /* This is the flag to start tracking */
int detect; /* This is the flag to start detecting */
int enable_walking; /* This just enables the robot capacity to move */
int left; /* Tell the robot to turn left */
int right; /* Tell the robot to turn right */
int backwards; /* Tell the robot to go backwards */
int forward; /* Tell the robot to go forward */
FILE *f;
} PARAM;
typedef struct
{
int FLB_pulse_old;
int FLM_pulse_old;
int FLE_pulse_old;
int HLB_pulse_old;
int HLM_pulse_old;
int HLE_pulse_old;
int FRB_pulse_old;
int FRM_pulse_old;
int FRE_pulse_old;
int HRB_pulse_old;
int HRM_pulse_old;
int HRE_pulse_old;
int servos; /*this one is the PCA9685 handler returned by I2Copen() */
int forward;
int PID_start;
float PID_out;
float PID_left;
float PID_right;
float PID_measurement[20]; /* For angle rolling average */
} SERVOS_PARAMETERS;
/* Define globally accessible variables no mutex */
volatile PARAM parameters;
/* Define globally accessible variables no mutex for exclusive use of the servos*/
volatile SERVOS_PARAMETERS parameters_servo;
/* Buffer for jpeg streaming*/
static std::vector<uchar> outbuf;
/* This global variable is used to interrupt the infite while loops with signal()
this is important as if not used and trying to stop the program with Ctrl-c,
the robot might go full speed against the wall (it happened)*/
static volatile int interrupt = 1;
/* the next function is the signal() function handler, is critical to
avoid damage to the robot*/
void inthandler(int signum)
{
interrupt = 0;
printf("Caught signal %d, coming out ...\n", signum);
}
void inthandler1(int signum)
{
interrupt = 0;
usleep(1500000);
gpioTerminate();
printf("Caught signal %d, coming out ...\n", signum);
exit(EXIT_FAILURE);
}
/* Web server functions below */
// HTTP request handler function. It implements the following endpoints:
// /video - hangs forever, returns MJPEG video stream
// all other URI - serves web_root/ directory
static void cb(struct mg_connection *c, int ev, void *ev_data, void *fn_data)
{
char *test = (char*) malloc(20);
/*if (mg_http_get_var(&hm->query, "stop", result, sizeof(result))>0) <-for GET mg_http_get_var(&hm->body, "test", test, 5); <- for POST
{printf("%s\n",result);} */
if (ev == MG_EV_HTTP_MSG) {
struct mg_http_message *hm = (struct mg_http_message *) ev_data; //(struct mg_http_message *) ev_data;
if (mg_http_match_uri(hm, "/video")) {
c->label[0] = 'S'; // Mark that connection as live streamer
mg_printf(
c, "%s",
"HTTP/1.0 200 OK\r\n"
"Cache-Control: no-cache\r\n"
"Pragma: no-cache\r\nExpires: Thu, 01 Dec 1994 16:00:00 GMT\r\n"
"Content-Type: multipart/x-mixed-replace; boundary=--foo\r\n\r\n");
}
else {
mg_http_get_var(&hm->body, "startseeking", test, 20);
if (!strcmp (test, "STARTSEEK"))
{
parameters.enable_walking = 1;
printf("SEEKING\n");
}
mg_http_get_var(&hm->body, "stopseeking", test, 20);
if (!strcmp (test, "STOPSEEK"))
{
parameters.enable_walking = 0;
printf("STOP SEEKING\n");
}
mg_http_get_var(&hm->body, "resettracker", test, 20);
if (!strcmp (test, "RESETTRACKER"))
{
parameters.seek = 0;
printf("TRACKER RESET\n");
}
mg_http_get_var(&hm->body, "turnleft", test, 20);
if (!strcmp (test, "TURNLEFT"))
{
parameters.left = 1;
parameters.enable_walking = 1;
parameters.detect = 0;
parameters.seek = 0;
printf("TURN LEFT\n");
}
mg_http_get_var(&hm->body, "turnright", test, 20);
if (!strcmp (test, "TURNRIGHT"))
{
parameters.right = 1;
parameters.enable_walking = 1;
parameters.detect = 0;
parameters.seek = 0;
printf("TURN RIGHT\n");
}
mg_http_get_var(&hm->body, "backwards", test, 20);
if (!strcmp (test, "BACKWARDS"))
{
parameters.backwards = 1;
parameters.enable_walking = 1;
parameters.detect = 0;
parameters.seek = 0;
printf("GO BACKWARDS\n");
}
mg_http_get_var(&hm->body, "forward", test, 20);
if (!strcmp (test, "FORWARD"))
{
parameters.forward = 1;
parameters.enable_walking = 1;
parameters.detect = 0;
parameters.seek = 0;
printf("GO FORWARD\n");
}
mg_http_get_var(&hm->body, "test", test, 20);
if (!strcmp (test, "uno"))
{printf("IT WORKS\n");}
struct mg_http_serve_opts opts = {.root_dir = "web_root"};
mg_http_serve_dir(c, hm, &opts);
}
}
}
static void broadcast_mjpeg_frame(struct mg_mgr *mgr)
{
if( outbuf.size()<4 ||
(outbuf[0]!=0xff && outbuf[0]!=0xd8) ||
(outbuf[outbuf.size()-2]!=0xff && outbuf[outbuf.size()-1]!=0xd9))
{
usleep(10000);
}
uchar *data = outbuf.data();
size_t size = outbuf.size();
struct mg_connection *c;
for (c = mgr->conns; c != NULL; c = c->next) {
if (c->label[0] != 'S') continue; // Skip non-stream connections
if (outbuf.data() == NULL || size == 0) continue; // Skip on buffer read error f (outbuf.data() == NULL || size == 0) continue;
mg_printf(c,
"--foo\r\nContent-Type: image/jpeg\r\n"
"Content-Length: %lu\r\n\r\n",
(unsigned long) size);
mg_send(c, data, size); // mg_send(c, &outbuf[0], size) mg_send(c, outbuf.data(), size);
mg_send(c, "\r\n", 2);
}
}
static void timer_callback(void *arg)
{
struct mg_mgr *mgr = (struct mg_mgr *) arg;
broadcast_mjpeg_frame(mgr);
}
/* PID regulator thread */
void *PID(void *arg)
{
/* Controller gains */
const float Kp = 3.5;
const float Ki = 0.5;
const float Kd = 0.25;
/* Sample time (in seconds) */
const float T = 0.1;
/* Derivative filter tau (in seconds) */
const float Tau = 0.02;
/* Controller "memory" */
float prevError = 0; /* Required for integrator */
float prevIntegral = 0; /* Required for integrator */
float prevDerivative = 0; /* Required for differentiator */
float proportional = 0;
float integral = 0;
float derivative = 0;
/* Controller intput */
float measurement[20] = {0};
/* Controller output */
float out = 0;
/* Controller setpoint */
const float setpoint = 320;
/* Controller error */
float error = 0;
/* Flags */
int left_flag = 0;
while(interrupt)
{
while(parameters_servo.PID_start && interrupt)
{
measurement[0] = measurement[0]/20;
measurement[0] = parameters.camera_x;
error = setpoint - measurement[0];
if (error>0)
{
left_flag = 1;
}
else
{
left_flag = 0;
}
/* Proportional */
proportional = Kp * error;
/* Integral */
integral = prevIntegral + 0.5 * Ki * T * (error + prevError);
/* Derivative */
derivative = (2.0*Kd*(error-prevError)+(2.0*Tau-T)*prevDerivative)/(2.0*Tau+T);
/* Saving for next iteration */
prevDerivative = derivative;
prevIntegral = integral;
prevError = error;
out = fabs(proportional + integral + derivative);
out = 1-out/320;
if (out < 0.50)
{
out = 0.50;
}
if (out > 1)
{
out = 1;
}
parameters_servo.PID_out = out;
if ((-20 > error) || (error > 20))
{
if (left_flag == 1)
{
parameters_servo.PID_right = out;
parameters_servo.PID_left = 1;
}
else
{
parameters_servo.PID_left = out;
parameters_servo.PID_right = 1;
}
}
else
{
parameters_servo.PID_left = 1;
parameters_servo.PID_right = 1;
}
usleep(100000);
}
}
pthread_exit(NULL);
}
void *Book_keeping(void *arg)
{
char s[100];
time_t now;
struct tm *t;
FILE *f;
f = fopen("record.csv", "w+");
if (f == NULL)
{
printf("Error opening file! Quitting Book_keeping thread\n");
interrupt = 0;
//exit(1);
}
parameters.f = f;
while(interrupt)
{
time(&now);
t = localtime(&now);
strftime(s, 100, "%H:%M:%S", t);
fprintf(f,"%s, %f, %f, %f, %f, %f, %f, %f, %f\n",s ,parameters.distance, parameters_servo.PID_out, parameters.acce_x,
parameters.acce_y, parameters.acce_z, parameters.gyro_x, parameters.gyro_y, parameters.gyro_z);
usleep(250000);
}
fclose(f);
pthread_exit(NULL);
}
/* This function below is for the ultrasounds sensor */
void *Distance(void *arg)
{
const int TRIGGER = 11; /*BCM pin 11, sending ultrasound cry*/
const int ECHO = 8; /*BCM pin 8, receiving ultrasound*/
gpioSetMode(ECHO, PI_INPUT);
gpioSetMode(TRIGGER, PI_OUTPUT);
struct timeval t1, t2, t3;
double time_interval,distance, distbuff;
int count;
while (interrupt)
{
time_interval = 0;
distance = 0;
gettimeofday(&t3, NULL);
gpioWrite(TRIGGER, 1);
usleep(15);
gpioWrite(TRIGGER, 0);
while (!gpioRead(ECHO))
{
gettimeofday(&t1, NULL);
gettimeofday(&t2, NULL);
time_interval = (t2.tv_sec-t3.tv_sec) * 1000000;
time_interval = (time_interval + (t2.tv_usec-t2.tv_usec))/1000000;
if (time_interval > 0.05)
break;
}
while (gpioRead(ECHO))
{
gettimeofday(&t2, NULL);
if (time_interval > 0.05)
break;
}
if (time_interval < 0.05)
{
time_interval = (t2.tv_sec-t1.tv_sec) * 1000000;
time_interval = (time_interval + (t2.tv_usec-t1.tv_usec))/1000000;
distance = time_interval*17150;
parameters.distance = distance;
if ((abs(distance - distbuff)) < 2)
{
count++;
}
if (count>7)
{
parameters.block = 1;
count = 0;
}
else
{
parameters.block = 0;
}
distbuff = distance;
if(distance<25)
{
parameters.ultrimpct = 1;
}
else
{
parameters.ultrimpct = 0;
}
}
usleep(250000);
}
pthread_exit(NULL);
}
/* Next thread will read the gyroscope & accelerometer data MPU6050 */
void *MPU6050_DATA(void *arg)
{
//const float GRAVITIY_MS2 = 9.80665;
/* Scale Modifiers */
//const float ACCEL_SCALE_MODIFIER_2G = 16384.0;
const float ACCEL_SCALE_MODIFIER_4G = 8192.0;
//const float ACCEL_SCALE_MODIFIER_8G = 4096.0;
//const float ACCEL_SCALE_MODIFIER_16G = 2048.0;
const float GYRO_SCALE_MODIFIER_250DEG = 131.0;
//const float GYRO_SCALE_MODIFIER_500DEG = 65.5;
//const float GYRO_SCALE_MODIFIER_1000DEG = 32.8;
//const float GYRO_SCALE_MODIFIER_2000DEG = 16.4;
/* Pre-defined ranges */
//const int ACCEL_RANGE_2G = 0x00;
const int ACCEL_RANGE_4G = 0x08;
//const int ACCEL_RANGE_8G = 0x10;
//const int ACCEL_RANGE_16G = 0x18;
const int GYRO_RANGE_250DEG = 0x00;
//const int GYRO_RANGE_500DEG = 0x08;
//const int GYRO_RANGE_1000DEG = 0x10;
//const int GYRO_RANGE_2000DEG = 0x18;
/* MPU-6050 Registers */
const int PWR_MGMT_1 = 0x6B;
//const int PWR_MGMT_2 = 0x6C;
const int ACCEL_XOUT0 = 0x3B;
const int ACCEL_YOUT0 = 0x3D;
const int ACCEL_ZOUT0 = 0x3F;
//const int TEMP_OUT0 = 0x41;
const int GYRO_XOUT0 = 0x43;
const int GYRO_YOUT0 = 0x45;
const int GYRO_ZOUT0 = 0x47;
const int ACCEL_CONFIG = 0x1C;
const int GYRO_CONFIG = 0x1B;
int MPU6050 = 0;
int acce_x_H = 0;
int acce_x_L = 0;
float acce_x = 0;
int acce_y_H = 0;
int acce_y_L = 0;
float acce_y = 0;
int acce_z_H = 0;
int acce_z_L = 0;
float acce_z = 0;
int gyro_x_H = 0;
int gyro_x_L = 0;
float gyro_x = 0;
int gyro_y_H = 0;
int gyro_y_L = 0;
float gyro_y = 0;
int gyro_z_H = 0;
int gyro_z_L = 0;
float gyro_z = 0;
int count_20 = 0;
/* Opening the conenction to the I2C slave */
MPU6050 = i2cOpen(1,0x68,0);
if (MPU6050 >= 0)
{
/* PI connection to I2C slave 0x68 OK*/
printf("Open I2C to slave 0x68 OK. Return code: %d\n", MPU6050);
printf("MPU6050 number handler: %d\n", MPU6050);
}
else
{
/* No PI connection to I2C slave 0x68 */
printf("Open I2C to slave 0x68 failed. Quitting MPU6050 thread Error code: %d\n", MPU6050);
interrupt = 0;
//exit(MPU6050);
}
parameters.MPU6050 = MPU6050;
/* Wake up the MPU-6050 since it starts in sleep mode */
i2cWriteByteData(MPU6050, PWR_MGMT_1, 0x00);
usleep(1000);
/* Now set up the accelerator range to 4G */
i2cWriteByteData(MPU6050, ACCEL_CONFIG, ACCEL_RANGE_4G);
usleep(1000);
/* Now set up the gyroscope range to 500 deg/second */
i2cWriteByteData(MPU6050, GYRO_CONFIG, GYRO_RANGE_250DEG);
usleep(1000);
while (interrupt)
{
/* Reading accelerometer values */
acce_x_H = i2cReadByteData(MPU6050, ACCEL_XOUT0); /* getting the H register 15:8 */
acce_x_L = i2cReadByteData(MPU6050, ACCEL_XOUT0+1); /* getting the H register 7:0 */
acce_x = (acce_x_H << 8) + acce_x_L;
if (acce_x >= 0x8000)
{
acce_x = -(65535 - acce_x) + 1;
}
acce_x = acce_x/ACCEL_SCALE_MODIFIER_4G;
parameters.acce_x = acce_x;
acce_y_H = i2cReadByteData(MPU6050, ACCEL_YOUT0); /* getting the H register 15:8 */
acce_y_L = i2cReadByteData(MPU6050, ACCEL_YOUT0+1); /* getting the H register 7:0 */
acce_y = (acce_y_H << 8) + acce_y_L;
if (acce_y >= 0x8000)
{
acce_y = -(65535 - acce_y) + 1;
}
acce_y = acce_y/ACCEL_SCALE_MODIFIER_4G;
parameters.acce_y = acce_y;
acce_z_H = i2cReadByteData(MPU6050, ACCEL_ZOUT0); /* getting the H register 15:8 */
acce_z_L = i2cReadByteData(MPU6050, ACCEL_ZOUT0+1); /* getting the H register 7:0 */
acce_z = (acce_z_H << 8) + acce_z_L;
if (acce_z >= 0x8000)
{
acce_z = -(65535 - acce_z) + 1;
}
acce_z = acce_z/ACCEL_SCALE_MODIFIER_4G;
parameters.acce_z = acce_z;
/* Reading gyroscope values */
gyro_x_H = i2cReadByteData(MPU6050, GYRO_XOUT0); /* getting the H register 15:8 */
gyro_x_L = i2cReadByteData(MPU6050, GYRO_XOUT0+1); /* getting the H register 7:0 */
gyro_x = (gyro_x_H << 8) + gyro_x_L;
if (gyro_x >= 0x8000)
{
gyro_x = -(65535 - gyro_x) + 1;
}
gyro_x = gyro_x/GYRO_SCALE_MODIFIER_250DEG;
parameters.gyro_x = gyro_x;
gyro_y_H = i2cReadByteData(MPU6050, GYRO_YOUT0); /* getting the H register 15:8 */
gyro_y_L = i2cReadByteData(MPU6050, GYRO_YOUT0+1); /* getting the H register 7:0 */
gyro_y = (gyro_y_H << 8) + gyro_y_L;
if (gyro_y >= 0x8000)
{
gyro_y = -(65535 - gyro_y) + 1;
}
gyro_y = gyro_y/GYRO_SCALE_MODIFIER_250DEG;
parameters.gyro_y = gyro_y;
gyro_z_H = i2cReadByteData(MPU6050, GYRO_ZOUT0); /* getting the H register 15:8 */
gyro_z_L = i2cReadByteData(MPU6050, GYRO_ZOUT0+1); /* getting the H register 7:0 */
gyro_z = (gyro_z_H << 8) + gyro_z_L;
if (gyro_z >= 0x8000)
{
gyro_z = -(65535 - gyro_z) + 1;
}
gyro_z = gyro_z/GYRO_SCALE_MODIFIER_250DEG;
gyro_z = gyro_z - 8.8; /*this is to correct an 8.6 offset detected on this axis */
parameters.gyro_z = gyro_z;
parameters_servo.PID_measurement[count_20] = gyro_z;
count_20++;
if (count_20>=30)
{
count_20 = 0;
}
usleep(100000);
}
i2cClose(parameters.MPU6050);
pthread_exit(NULL);
}
/* Next thread will manage the opencv stuff */
void *Camera(void *arg)
{
clock_t before = 0;
clock_t after = 0;
Mat frame, framegray;
Mat fgMaskMOG2; /* Foreground mask used by MOG2 method */
vector<vector<Point> > contours;
vector<Point> approx;
bool ok = false;
int detection = 1;
Rect2d mr;
Point centre;
/* Parameters to the encoder for jpeg stream */
std::vector<unsigned char> buffer;
vector<int> params_stream(2);
params_stream[0] = IMWRITE_JPEG_QUALITY;
params_stream[1] = 80; /* JPEG quality (1...100) */
Ptr<BackgroundSubtractor> pMOG2; /* MOG2 Background subtractor */
pMOG2 = createBackgroundSubtractorMOG2(100,15,true); /* Create MOG2 Background Subtractor object */
TrackerCSRT::Params params = TrackerCSRT::Params(); /* Creating parameters for the tracker so we are able to change threshold */
/* Full list of parameters */
params.use_channel_weights = true;
params.use_segmentation = true;
params.use_hog = true;
params.use_color_names = true;
params.use_gray = false; /* orginally true */
params.use_rgb = true; /* originally false */
params.window_function = "hann"; /* originally= hann, but others are possible: Window function: "hann", "cheb", "kaiser"*/
params.kaiser_alpha = 3.75f;
params.cheb_attenuation = 45;
params.padding = 3.0f;
params.template_size = 200;
params.gsl_sigma = 1.0f;
params.hog_orientations = 9;
params.hog_clip = 0.2f;
params.num_hog_channels_used = 18;
params.filter_lr = 0.02f;
params.weights_lr = 0.02f;
params.admm_iterations = 6; // originally 4
params.number_of_scales = 33;
params.scale_sigma_factor = 0.250f;
params.scale_model_max_area = 512.0f;
params.scale_lr = 0.025f;
params.scale_step = 1.020f;
params.histogram_bins = 16;
params.background_ratio = 2; /* Default value =2 */
params.histogram_lr = 0.04f;
params.psr_threshold = 0.07f; /* Default value= 0.035 CSRT Tracker parameter to make it more sensible to false positives */
Ptr<Tracker> tracker; /* Create Pointer to tracker object */
//tracker = TrackerCSRT::create(params); /* Create the tracker object */
int contours_chosen = 0;
/*--- INITIALIZE VIDEOCAPTURE */
VideoCapture cap;
cap.set(CAP_PROP_FRAME_WIDTH,640); /*set camera resolution */
cap.set(CAP_PROP_FRAME_HEIGHT,480); /* set camera resolution */
cap.set(CAP_PROP_BUFFERSIZE, 10); /* To store only the last 10 frames */
int deviceID = 0; // 0 = open default camera
int apiID = cv::CAP_ANY; // 0 = autodetect default API
// open selected camera using selected API
cap.open(deviceID, apiID);
// check if we succeeded
if (!cap.isOpened())
{
cerr << "ERROR! Unable to open camera, exiting thread\n";
pthread_exit(NULL);
}
while(interrupt)
{
detection = 1;
parameters.seek = 0;
before = clock(); /* allow time to settle the movement detector when start frame capture camera*/
while (detection && interrupt)
{
parameters.seek = 0;
/* wait for a new frame from camera and store it into 'frame' */
cap.read(frame);
/* check if we succeeded */
if (frame.empty())
{
printf("ERROR! blank frame grabbed\n");
break;
}
/* Put frame into buffer */
imencode(".jpg", frame, buffer, params_stream);
outbuf.swap(buffer);
/* colour to gray conversion */
cvtColor(frame, framegray, COLOR_RGB2GRAY); //COLOR_BGR2GRAY
GaussianBlur(framegray, framegray, Size(21, 21), 0, 0);
pMOG2->apply(framegray, fgMaskMOG2, 0.2); /* Update the MOG2 background model based
on the current frame, 0=background model not updated, 1=background model is completely reinitialized from the
last frame, negative=automatic, used=0.0035 */
dilate(fgMaskMOG2, fgMaskMOG2, getStructuringElement(MORPH_RECT, Size(7, 7)), Point(-1, -1),2,BORDER_DEFAULT);
findContours(fgMaskMOG2, contours, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);
/* test each contour for bigger area on screen) */
for( size_t i = 0; i < contours.size(); i++ )
{
contours_chosen = 0;
if (i>0)
{
if (contourArea(contours[i]) > contourArea(contours[i-1]))
{
contours_chosen = i;
}
}
}
after = clock();
if ((contourArea(contours[contours_chosen]))>2000 && ((double)(after-before)/(double)CLOCKS_PER_SEC)>10 && parameters.detect)
{
mr = boundingRect(contours[contours_chosen]); //mr= boundingRect(approx);
if (mr.x>=0 && mr.y>=0 && mr.width<640 && mr.height<480) /* To check if the rectangle is out of screen (long negative),if not it will THROW EXCEPTION:
error(-211) see: https://github.com/opencv/opencv/issues/7573 */
{
detection = 0;
mr = mr & Rect2d(0, 0, 640, 480); /* To avoid exception error: (-215:Assertion failed) */
tracker = TrackerCSRT::create(params); /* Create the tracker object */
tracker->init(frame, mr);
parameters_servo.PID_start = 1;
parameters.seek = 1;
}
}
}
//destroyWindow("FG Mask MOG 2");
while (parameters.seek && interrupt)
{
/* wait for a new frame from camera and store it into 'frame' */
cap.read(frame);
/* check if we succeeded */
if (frame.empty())
{
printf("ERROR! blank frame grabbed\n");
break;
}
ok = tracker->update(frame, mr);
//printf("OK: %d\n",ok);
if (ok)
{
rectangle(frame, mr, Scalar( 255, 0, 0 ), 2, 1 );
putText(frame, "TRACKING", Point(20, 20), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 0)); /* this one is the top-left corner */
float scale = 1.0;
centre.x = cvRound((mr.x + mr.width*0.5)*scale);
centre.y = cvRound((mr.y + mr.height*0.5)*scale);
parameters.camera_x = centre.x;
}
else
{
detection = 1;
parameters.seek = 0;
parameters_servo.PID_start = 0;
}
/* Put frame into buffer */
imencode(".jpg", frame, buffer, params_stream);
outbuf.swap(buffer);
}
tracker.release();
}
pMOG2.release();
cap.release();
pthread_exit(NULL);
}
/* Pigpio library initialization */
void Init_Pigpio(void)
{
int Init = 0;
Init = gpioInitialise();
if (Init < 0)
{
/* pigpio initialisation failed */
printf("Pigpio initialisation failed. Finishing Program Error code: %d\n", Init);
interrupt = 0;
}
else
{
/* pigpio initialised okay*/
printf("Pigpio initialisation OK. Return code: %d\n", Init);
}
}
/* Next function is to create a connection to and initialize the PCA9685 drive */
int Restart_PCA9685(void)
{
int oldmode;
int newmode;
int servos;
servos = i2cOpen(1,0x40,0);
if (servos >= 0)
{
/* PI connection to I2C slave 40 OK*/
printf("Open I2C to slave 0x40 OK. Return code: %d\n", servos);
printf("PCA9685 number handler: %d\n", servos);
}
else
{
/* No PI connection to I2C slave 40 */
printf("Open I2C to slave 0x40 failed. Quitting Servos thread Error code: %d\n", servos);
interrupt = 0;
}
oldmode = i2cReadByteData(servos, 0x00); /* getting current mode */
newmode = (oldmode & 0xEF); /* wake up definition */
i2cWriteByteData(servos, 0x00, newmode); /* wake up in case */
usleep(5000);
oldmode = i2cReadByteData(servos, 0x00); /* getting current mode */
i2cWriteByteData(servos, 0x00, oldmode | 0x80); /* restart */
return(servos);
}
/*Setting up the servo drive here (frequency) */
void Init_PCA9685(int servos)
{
float freq;
int oldmode;
int newmode;
/* Setting the PCA9685 frequency, must be 50Hz for the SG-90 servos */
freq = (25000000/(4096*50)) - 0.5; /* now 25*10^6 is 25Mhz of the internal clock, 4096 is 12 bit resolution and 50hz is the wanted frequency setup) */
freq = (int) freq;
/* now there is a whole sequence to set up the frequency */
oldmode = i2cReadByteData(servos, 0x00); /* getting current mode */
newmode = (oldmode & 0x7F) | 0x10; /* sleep mode definition */
i2cWriteByteData(servos, 0x00, newmode); /* going to sleep now */
i2cWriteByteData(servos, 0xFE, freq); /* setting up the frequency now */
i2cWriteByteData(servos, 0x00, oldmode); /* coming back to the old mode */
usleep(5000);
i2cWriteByteData(servos, 0x00, oldmode | 0x80); /* final step on frequency set up */
}
/* Next function will put all hte servos to its central position */
void Centre(int servos_handler)
{
int i;
float pulse_1;
int pulse;
int servos;
servos = servos_handler;
/* These ones are to calculate the central postion common to all of them */
pulse_1 = 1500; /* 1500 should be the centered position, 1000 is up and 2000 is down */
pulse_1 = (pulse_1*4096)/20000; /* this ends up being 307 so maybe more handy to use that number directly */
pulse = (int) pulse_1;
for(i=0;i<12;i++)
{
i2cWriteByteData(servos, (4*i)+6, 0x00);
i2cWriteByteData(servos, (4*i)+7, 0x00);
i2cWriteByteData(servos, (4*i)+8, pulse & 0xFF);
i2cWriteByteData(servos, (4*i)+9, pulse >> 8);
usleep(5000);
}
parameters_servo.FLB_pulse_old = 307;
parameters_servo.FLM_pulse_old = 307;
parameters_servo.FLE_pulse_old = 307;
parameters_servo.HLB_pulse_old = 307;
parameters_servo.HLM_pulse_old = 307;
parameters_servo.HLE_pulse_old = 307;
parameters_servo.FRB_pulse_old = 307;
parameters_servo.FRM_pulse_old = 307;
parameters_servo.FRE_pulse_old = 307;
parameters_servo.HRB_pulse_old = 307;
parameters_servo.HRM_pulse_old = 307;
parameters_servo.HRE_pulse_old = 307;
}
/* Next function moves the robot forward */
void Forward(int servos_handler)
{
int i;
int FLB_pulse_old = parameters_servo.FLB_pulse_old;
int FLM_pulse_old = parameters_servo.FLM_pulse_old;
int FLE_pulse_old = parameters_servo.FLE_pulse_old;
int HLB_pulse_old = parameters_servo.HLB_pulse_old;
int HLM_pulse_old = parameters_servo.HLM_pulse_old;
int HLE_pulse_old = parameters_servo.HLE_pulse_old;
int FRB_pulse_old = parameters_servo.FRB_pulse_old;
int FRM_pulse_old = parameters_servo.FRM_pulse_old;
int FRE_pulse_old = parameters_servo.FRE_pulse_old;
int HRB_pulse_old = parameters_servo.HRB_pulse_old;
int HRM_pulse_old = parameters_servo.HRM_pulse_old;
int HRE_pulse_old = parameters_servo.HRE_pulse_old;
float FLB_pulse_now;
float FLM_pulse_now;
float FLE_pulse_now;
float HLB_pulse_now;
float HLM_pulse_now;
float HLE_pulse_now;
float FRB_pulse_now;
float FRM_pulse_now;
float FRE_pulse_now;
float HRB_pulse_now;
float HRM_pulse_now;
float HRE_pulse_now;
int global_position;
int servos;
float FLB_pulse;
float FLM_pulse;
float FLE_pulse;
float FRB_pulse;
float FRM_pulse;
float FRE_pulse;
float HLB_pulse;
float HLM_pulse;
float HLE_pulse;
float HRB_pulse;
float HRM_pulse;
float HRE_pulse;
int FLB_direction = 1;
int FLM_direction = -1;
int FLE_direction = -1;
int FRB_direction = -1;
int FRM_direction = 1;
int FRE_direction = 1;
int HLB_direction = -1;
int HLM_direction = 1;
int HLE_direction = 1;
int HRB_direction = 1;
int HRM_direction = -1;
int HRE_direction = -1;
int wiggle_h = 120;
int wiggle_v = 200;
int wiggle_middle = 30;
servos = servos_handler;
/* Going from position 1 to 8 to walk forward*/
for(global_position=parameters.global_position;global_position<9;global_position++)
{
/* Calculate the pulse to send to each servomotor and determine each leg position*/
parameters.global_position=global_position;
switch (global_position)
{
case 1:
FLB_pulse = 307 + wiggle_middle*FLB_direction*parameters_servo.PID_right;
FLM_pulse = 307 + wiggle_v*FLM_direction;
FLE_pulse = 307 + wiggle_v*FLE_direction;
HLB_pulse = 307 + (-wiggle_middle + (wiggle_h*5/3 - wiggle_h))*HLB_direction*parameters_servo.PID_right;
HLM_pulse = 307;
HLE_pulse = 307;
FRB_pulse = 307 + (wiggle_middle + (wiggle_h*3/3 - wiggle_h))*FRB_direction*parameters_servo.PID_left;
FRM_pulse = 307;
FRE_pulse = 307;
HRB_pulse = 307 + (-wiggle_middle + (wiggle_h*1/3 - wiggle_h))*HRB_direction*parameters_servo.PID_left;
HRM_pulse = 307;
HRE_pulse = 307;
break;
case 2:
FLB_pulse = 307 + (wiggle_middle + wiggle_h)*FLB_direction*parameters_servo.PID_right;
FLM_pulse = 307;
FLE_pulse = 307 ;
HLB_pulse = 307 + (-wiggle_middle + (wiggle_h*(4)/3 - wiggle_h))*HLB_direction*parameters_servo.PID_right;
HLM_pulse = 307;
HLE_pulse = 307;
FRB_pulse = 307 + (wiggle_middle + (wiggle_h*(2)/3 - wiggle_h))*FRB_direction*parameters_servo.PID_left;
FRM_pulse = 307;
FRE_pulse = 307;
HRB_pulse = 307 + (-wiggle_middle + (wiggle_h*(0)/3 - wiggle_h))*HRB_direction*parameters_servo.PID_left;
HRM_pulse = 307;
HRE_pulse = 307;
break;
case 3: