Projet Robot self-balancing Elegoo Trumbller arduino nano

Invité · 01/03/2023, 18h38

Bonjour

Je suis nouveau sur ce forum, je travaille sur un robot self-balancing (auto-équilibré) de la marque Elegoo. Le projet consiste à le faire avancer et quand il rencontre un mur via les capteurs ultrason de faire un virage à droite de 90° , de continuer à avancer pendant 3 secondes et s'arrête tout en restant en équilibre. Puis plus tard dans le projet de communiquer via une carte wifi avec d'autres robots. Le problème est que je n'arrive pas à comprendre comment fonctionne le programme Arduino officiel de Elegoo, je vous demande donc un peu d'aide pour m'expliquer comment fonctionne le programme. Je vous mets la documentation du robot en pdf et le programme en dossier zip.ELEGOO-TumbllerV1.1-Self-Balancing-Car-Tutorial-main.zip

**Vincent PETIT** · 01/03/2023, 19h30

Bonsoir,
Il y a très peu de chance que quelqu'un ouvre ton fichier zip, postes plutôt ton programme entre les balises adaptées [CODE][/CODE] ou en cliquant sur le bouton # dans l'éditeur de message

Invité · 02/03/2023, 19h32

Ok merci beaucoup

Donc voici le programme principale:

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

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
/*
 * @Description: In User Settings Edit
 * @Author: your name
 * @Date: 2019-09-12 14:51:36
 * @LastEditTime: 2019-10-11 16:39:57
 * @LastEditors: Please set LastEditors
 */
#include <Arduino.h>
#include "PinChangeInt.h"
#include "Pins.h"
#include "mode.h"
#include "Command.h"
#include "BalanceCar.h"
#include "Rgb.h"
#include "Ultrasonic.h"
#include "voltage.h"
 
unsigned long start_prev_time = 0;
boolean carInitialize_en = true;
 
void functionMode()
{
  switch (function_mode)
  {
  case IDLE:
    break;
  case IRREMOTE:
    break;
  case OBSTACLE:
    obstacleAvoidanceMode();
    break;
  case FOLLOW:
    followMode();
    break;
  case BLUETOOTH:
    break;
  case FOLLOW2:
    followMode2();
    break;
  default:
    break;
  }
}
 
void setMotionState()
{
  switch (motion_mode)
  {
  case FORWARD:
    switch (function_mode)
    {
    case FOLLOW:
      setting_car_speed = 20;
      setting_turn_speed = 0;
      break;
    case FOLLOW2:
      setting_car_speed = 20;
      setting_turn_speed = 0;
      break;
    case BLUETOOTH:
      setting_car_speed = 80;
      break;
    case IRREMOTE:
      setting_car_speed = 80;
      setting_turn_speed = 0;
      break;
    default:
      setting_car_speed = 40;
      setting_turn_speed = 0;
      break;
    }
    break;
  case BACKWARD:
    switch (function_mode)
    {
    case FOLLOW:
      setting_car_speed = -20;
      setting_turn_speed = 0;
      break;
    case FOLLOW2:
      setting_car_speed = -20;
      setting_turn_speed = 0;
      break;
    case BLUETOOTH:
      setting_car_speed = -80;
      break;
    case IRREMOTE:
      setting_car_speed = -80;
      setting_turn_speed = 0;
      break;
    default:
      setting_car_speed = -40;
      setting_turn_speed = 0;
      break;
    }
    break;
  case TURNLEFT:
    switch (function_mode)
    {
    case FOLLOW:
      setting_car_speed = 0;
      setting_turn_speed = 50;
      break;
    case FOLLOW2:
      setting_car_speed = 0;
      setting_turn_speed = 50;
      break;
    case BLUETOOTH:
      setting_turn_speed = 80;
      break;
    case IRREMOTE:
      setting_car_speed = 0;
      setting_turn_speed = 80;
      break;
    default:
      setting_car_speed = 0;
      setting_turn_speed = 50;
      break;
    }
    break;
  case TURNRIGHT:
    switch (function_mode)
    {
    case FOLLOW:
      setting_car_speed = 0;
      setting_turn_speed = -50;
      break;
    case FOLLOW2:
      setting_car_speed = 0;
      setting_turn_speed = -50;
      break;
    case BLUETOOTH:
      setting_turn_speed = -80;
      break;
    case IRREMOTE:
      setting_car_speed = 0;
      setting_turn_speed = -80;
      break;
    default:
      setting_car_speed = 0;
      setting_turn_speed = -50;
      break;
    }
    break;
  case STANDBY:
    setting_car_speed = 0;
    setting_turn_speed = 0;
    break;
  case STOP:
    if (millis() - start_prev_time > 1000)
    {
      function_mode = IDLE;
      if (balance_angle_min <= kalmanfilter_angle && kalmanfilter_angle <= balance_angle_max)
      {
        motion_mode = STANDBY;
        rgb.lightOff();
      }
    }
    break;
  case START:
    if (millis() - start_prev_time > 2000)
    {
      if (balance_angle_min <= kalmanfilter_angle && kalmanfilter_angle <= balance_angle_max)
      {
        car_speed_integeral = 0;
        setting_car_speed = 0;
        motion_mode = STANDBY;
        rgb.lightOff();
      }
      else
      {
        motion_mode = STOP;
        carStop();
        rgb.brightRedColor();
      }
    }
    break;
  default:
    break;
  }
}
 
void keyEventHandle()
{
  if (key_value != '\0')
  {
    key_flag = key_value;
 
    switch (key_value)
    {
    case 's':
      rgb.lightOff();
      motion_mode = STANDBY;
      break;
    case 'f':
      rgb.flashBlueColorFront();
      motion_mode = FORWARD;
      break;
    case 'b':
      rgb.flashBlueColorback();
      motion_mode = BACKWARD;
      break;
    case 'l':
      rgb.flashBlueColorLeft();
      motion_mode = TURNLEFT;
      break;
    case 'i':
      rgb.flashBlueColorRight();
      motion_mode = TURNRIGHT;
      break;
    case '1':
      function_mode = FOLLOW;
      follow_flag = 0;
      follow_prev_time = millis();
      break;
    case '2':
      function_mode = OBSTACLE;
      obstacle_avoidance_flag = 0;
      obstacle_avoidance_prev_time = millis();
      break;
    case '3':
    rgb_loop:
      key_value = '\0';
      rgb.flag++;
      if (rgb.flag > 6)
      {
        rgb.flag = 1;
      }
      switch (rgb.flag)
      {
      case 0:
        break;
      case 1:
        if (rgb.theaterChaseRainbow(50) && key_value == '3')
          goto rgb_loop;
        break;
      case 2:
        if (rgb.rainbowCycle(20) && key_value == '3')
          goto rgb_loop;
        break;
      case 3:
        if (rgb.theaterChase(127, 127, 127, 50) && key_value == '3')
          goto rgb_loop;
        break;
      case 4:
        if (rgb.rainbow(20) && key_value == '3')
          goto rgb_loop;
        break;
      case 5:
        if (rgb.whiteOverRainbow(20, 30, 4) && key_value == '3')
          goto rgb_loop;
        break;
      case 6:
        if (rgb.rainbowFade2White(3, 50, 50) && key_value == '3')
          goto rgb_loop;
        break;
        break;
      default:
        break;
      }
      break;
    case '4':
      function_mode = IDLE;
      motion_mode = STOP;
      carBack(110);
      delay((kalmanfilter_angle - 30) * (kalmanfilter_angle - 30) / 8);
      carStop();
      start_prev_time = millis();
      rgb.brightRedColor();
      break;
    case '5':
      if (millis() - start_prev_time > 500 && kalmanfilter_angle >= balance_angle_min)
      {
        start_prev_time = millis();
        motion_mode = START;
      }
      motion_mode = START;
      break;
    case '6':
      rgb.brightness = 50;
      rgb.setBrightness(rgb.brightness);
      rgb.show();
      break;
    case '7':
      rgb.brightRedColor();
      rgb.brightness -= 25;
      if (rgb.brightness <= 0)
      {
        rgb.brightness = 0;
      }
      rgb.setBrightness(rgb.brightness);
      rgb.show();
      break;
    case '8':
      rgb.brightRedColor();
      rgb.brightness += 25;
      if (rgb.brightness >= 255)
      {
        rgb.brightness = 255;
      }
      rgb.setBrightness(rgb.brightness);
      rgb.show();
      break;
    case '9':
      rgb.brightness = 0;
      rgb.setBrightness(rgb.brightness);
      rgb.show();
      break;
    case '0':
      function_mode = FOLLOW2;
      follow_flag = 0;
      follow_prev_time = millis();
      break;
    case '*':
      break;
    case '#':
      break;
    default:
      break;
    }
    if (key_flag == key_value)
    {
      key_value = '\0';
    }
  }
}
 
void setup()
{
 
  Serial.begin(9600);
  ultrasonicInit();
  keyInit();
  rgb.initialize();
  voltageInit();
  start_prev_time = millis();
  carInitialize();
}
void loop()
{
  getKeyValue();
  getBluetoothData();
  keyEventHandle();
  getDistance();
  voltageMeasure();
  setMotionState();
  functionMode();
  checkObstacle();
  rgb.blink(100);
  static unsigned long print_time;
  if (millis() - print_time > 100)
  {
    print_time = millis();
    Serial.println(kalmanfilter.angle);
  }
  static unsigned long start_time;
  if (millis() - start_time < 10)
  {
    function_mode = IDLE;
    motion_mode = STOP;
    carStop();
  }
  if (millis() - start_time == 2000) // Enter the pendulum, the car balances...
  {
    key_value = '5';
  }
}

Invité · 02/03/2023, 20h00

Et voilât les sous programme :

Adafruit_NeoPixel.cpp :

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

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
1001
1002
1003
1004
1005
1006
1007
1008
1009
1010
1011
1012
1013
1014
1015
1016
1017
1018
1019
1020
1021
1022
1023
1024
1025
1026
1027
1028
1029
1030
1031
1032
1033
1034
1035
1036
1037
1038
1039
1040
1041
1042
1043
1044
1045
1046
1047
1048
1049
1050
1051
1052
1053
1054
1055
1056
1057
1058
1059
1060
1061
1062
1063
1064
1065
1066
1067
1068
1069
1070
1071
1072
1073
1074
1075
1076
1077
1078
1079
1080
1081
1082
1083
1084
1085
1086
1087
1088
1089
1090
1091
1092
1093
1094
1095
1096
1097
1098
1099
1100
1101
1102
1103
1104
1105
1106
1107
1108
1109
1110
1111
1112
1113
1114
1115
1116
1117
1118
1119
1120
1121
1122
1123
1124
1125
1126
1127
1128
1129
1130
1131
1132
1133
1134
1135
1136
1137
1138
1139
1140
1141
1142
1143
1144
1145
1146
1147
1148
1149
1150
1151
1152
1153
1154
1155
1156
1157
1158
1159
1160
1161
1162
1163
1164
1165
1166
1167
1168
1169
1170
1171
1172
1173
1174
1175
1176
1177
1178
1179
1180
1181
1182
1183
1184
1185
1186
1187
1188
1189
1190
1191
1192
1193
1194
1195
1196
1197
1198
1199
1200
1201
1202
1203
1204
1205
1206
1207
1208
1209
1210
1211
1212
1213
1214
1215
1216
1217
1218
1219
1220
1221
1222
1223
1224
1225
1226
1227
1228
1229
1230
1231
1232
1233
1234
1235
1236
1237
1238
1239
1240
1241
1242
1243
1244
1245
1246
1247
1248
1249
1250
1251
1252
1253
1254
1255
1256
1257
1258
1259
1260
1261
1262
1263
1264
1265
1266
1267
1268
1269
1270
1271
1272
1273
1274
1275
1276
1277
1278
1279
1280
1281
1282
1283
1284
1285
1286
1287
1288
1289
1290
1291
1292
1293
1294
1295
1296
1297
1298
1299
1300
1301
1302
1303
1304
1305
1306
1307
1308
1309
1310
1311
1312
1313
1314
1315
1316
1317
1318
1319
1320
1321
1322
1323
1324
1325
1326
1327
1328
1329
1330
1331
1332
1333
1334
1335
1336
1337
1338
1339
1340
1341
1342
1343
1344
1345
1346
1347
1348
1349
1350
1351
1352
1353
1354
1355
1356
1357
1358
1359
1360
1361
1362
1363
1364
1365
1366
1367
1368
1369
1370
1371
1372
1373
1374
1375
1376
1377
1378
1379
1380
1381
1382
1383
1384
1385
1386
1387
1388
1389
1390
1391
1392
1393
1394
1395
1396
1397
1398
1399
1400
1401
1402
1403
1404
1405
1406
1407
1408
1409
1410
1411
1412
1413
1414
1415
1416
1417
1418
1419
1420
1421
1422
1423
1424
1425
1426
1427
1428
1429
1430
1431
1432
1433
1434
1435
1436
1437
1438
1439
1440
1441
1442
1443
1444
1445
1446
1447
1448
1449
1450
1451
1452
1453
1454
1455
1456
1457
1458
1459
1460
1461
1462
1463
1464
1465
*-------------------------------------------------------------------------
 *  Arduino library to control a wide variety of WS2811- and WS2812-based RGB
 *  LED devices such as Adafruit FLORA RGB Smart Pixels and NeoPixel strips.
 *  Currently handles 400 and 800 KHz bitstreams on 8, 12 and 16 MHz ATmega
 *  MCUs, with LEDs wired for various color orders.  8 MHz MCUs provide
 *  output on PORTB and PORTD, while 16 MHz chips can handle most output pins
 *  (possible exception with upper PORT registers on the Arduino Mega).
 *
 *  Written by Phil Burgess / Paint Your Dragon for Adafruit Industries,
 *  contributions by PJRC, Michael Miller and other members of the open
 *  source community.
 *
 *  Adafruit invests time and resources providing this open source code,
 *  please support Adafruit and open-source hardware by purchasing products
 *  from Adafruit!
 *
 *  -------------------------------------------------------------------------
 *  This file is part of the Adafruit NeoPixel library.
 *
 *  NeoPixel is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU Lesser General Public License as
 *  published by the Free Software Foundation, either version 3 of
 *  the License, or (at your option) any later version.
 *
 *  NeoPixel is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with NeoPixel.  If not, see
 *  <http://www.gnu.org/licenses/>.
 *  -------------------------------------------------------------------------*/
 
#include "Adafruit_NeoPixel.h"
// Constructor when length, pin and type are known at compile-time:
Adafruit_NeoPixel::Adafruit_NeoPixel(uint16_t n, uint8_t p, neoPixelType t) :
	begun(false), brightness(0), pixels(NULL), endTime(0)
{
	updateType(t);
	updateLength(n);
	setPin(p);
}
 
 
// via Michael Vogt/neophob: empty constructor is used when strand length
// isn't known at compile-time; situations where program config might be
// read from internal flash memory or an SD card, or arrive via serial
// command.  If using this constructor, MUST follow up with updateType(),
// updateLength(), etc. to establish the strand type, length and pin number!
Adafruit_NeoPixel::Adafruit_NeoPixel() :
#ifdef NEO_KHZ400
	is800KHz(true),
#endif
	begun(false), numLEDs(0), numBytes(0), pin(-1), brightness(0), pixels(NULL),
	rOffset(1), gOffset(0), bOffset(2), wOffset(1), endTime(0)
{
}
 
 
Adafruit_NeoPixel::~Adafruit_NeoPixel()
{
	if (pixels) {
		free(pixels);
	}
	if (pin >= 0) {
		pinMode(pin, INPUT);
	}
}
 
 
void Adafruit_NeoPixel::begin(void)
{
	if (pin >= 0) {
		pinMode(pin, OUTPUT);
		digitalWrite(pin, LOW);
	}
	begun = true;
}
 
 
void Adafruit_NeoPixel::updateLength(uint16_t n)
{
	if (pixels) {
		free(pixels); // Free existing data (if any)
	}
	// Allocate new data -- note: ALL PIXELS ARE CLEARED
	numBytes = n * ((wOffset == rOffset) ? 3 : 4);
	if ((pixels = (uint8_t *)malloc(numBytes))) {
		memset(pixels, 0, numBytes);
		numLEDs = n;
	} else {
		numLEDs = numBytes = 0;
	}
}
 
 
void Adafruit_NeoPixel::updateType(neoPixelType t)
{
	boolean oldThreeBytesPerPixel = (wOffset == rOffset);   // false if RGBW
 
	wOffset = (t >> 6) & 0b11;                              // See notes in header file
	rOffset = (t >> 4) & 0b11;                              // regarding R/G/B/W offsets
	gOffset = (t >> 2) & 0b11;
	bOffset = t       & 0b11;
#ifdef NEO_KHZ400
	is800KHz = (t < 256); // 400 KHz flag is 1<<8
#endif
 
	// If bytes-per-pixel has changed (and pixel data was previously
	// allocated), re-allocate to new size.  Will clear any data.
	if (pixels) {
		boolean newThreeBytesPerPixel = (wOffset == rOffset);
		if (newThreeBytesPerPixel != oldThreeBytesPerPixel) {
			updateLength(numLEDs);
		}
	}
}
 
 
#ifdef ESP8266
// ESP8266 show() is external to enforce ICACHE_RAM_ATTR execution
extern "C" void ICACHE_RAM_ATTR espShow(
	uint8_t pin, uint8_t *pixels, uint32_t numBytes, uint8_t type);
 
#endif // ESP8266
 
void Adafruit_NeoPixel::show(void)
{
	if (!pixels) {
		return;
	}
 
	// Data latch = 50+ microsecond pause in the output stream.  Rather than
	// put a delay at the end of the function, the ending time is noted and
	// the function will simply hold off (if needed) on issuing the
	// subsequent round of data until the latch time has elapsed.  This
	// allows the mainline code to start generating the next frame of data
	// rather than stalling for the latch.
	while (!canShow())
	{
	}
	// endTime is a private member (rather than global var) so that mutliple
	// instances on different pins can be quickly issued in succession (each
	// instance doesn't delay the next).
 
	// In order to make this code runtime-configurable to work with any pin,
	// SBI/CBI instructions are eschewed in favor of full PORT writes via the
	// OUT or ST instructions.  It relies on two facts: that peripheral
	// functions (such as PWM) take precedence on output pins, so our PORT-
	// wide writes won't interfere, and that interrupts are globally disabled
	// while data is being issued to the LEDs, so no other code will be
	// accessing the PORT.  The code takes an initial 'snapshot' of the PORT
	// state, computes 'pin high' and 'pin low' values, and writes these back
	// to the PORT register as needed.
 
	noInterrupts(); // Need 100% focus on instruction timing
 
 
#ifdef __AVR__
// AVR MCUs -- ATmega & ATtiny (no XMEGA) ---------------------------------
 
	volatile uint16_t
	    i = numBytes;       // Loop counter
	volatile uint8_t
	*ptr = pixels,          // Pointer to next byte
	    b = *ptr++,         // Current byte value
	    hi,                 // PORT w/output bit set high
	    lo;                 // PORT w/output bit set low
 
	// Hand-tuned assembly code issues data to the LED drivers at a specific
	// rate.  There's separate code for different CPU speeds (8, 12, 16 MHz)
	// for both the WS2811 (400 KHz) and WS2812 (800 KHz) drivers.  The
	// datastream timing for the LED drivers allows a little wiggle room each
	// way (listed in the datasheets), so the conditions for compiling each
	// case are set up for a range of frequencies rather than just the exact
	// 8, 12 or 16 MHz values, permitting use with some close-but-not-spot-on
	// devices (e.g. 16.5 MHz DigiSpark).  The ranges were arrived at based
	// on the datasheet figures and have not been extensively tested outside
	// the canonical 8/12/16 MHz speeds; there's no guarantee these will work
	// close to the extremes (or possibly they could be pushed further).
	// Keep in mind only one CPU speed case actually gets compiled; the
	// resulting program isn't as massive as it might look from source here.
 
// 8 MHz(ish) AVR ---------------------------------------------------------
#if (F_CPU >= 7400000UL) && (F_CPU <= 9500000UL)
#ifdef NEO_KHZ400 // 800 KHz check needed only if 400 KHz support enabled
	if (is800KHz) {
#endif
 
	volatile uint8_t n1, n2 = 0; // First, next bits out
 
	// Squeezing an 800 KHz stream out of an 8 MHz chip requires code
	// specific to each PORT register.  At present this is only written
	// to work with pins on PORTD or PORTB, the most likely use case --
	// this covers all the pins on the Adafruit Flora and the bulk of
	// digital pins on the Arduino Pro 8 MHz (keep in mind, this code
	// doesn't even get compiled for 16 MHz boards like the Uno, Mega,
	// Leonardo, etc., so don't bother extending this out of hand).
	// Additional PORTs could be added if you really need them, just
	// duplicate the else and loop and change the PORT.  Each add'l
	// PORT will require about 150(ish) bytes of program space.
 
	// 10 instruction clocks per bit: HHxxxxxLLL
	// OUT instructions:              ^ ^    ^   (T=0,2,7)
 
#ifdef PORTD // PORTD isn't present on ATtiny85, etc.
	if (port == &PORTD) {
		hi = PORTD |  pinMask;
		lo = PORTD & ~pinMask;
		n1 = lo;
		if (b & 0x80) {
			n1 = hi;
		}
 
		// Dirty trick: RJMPs proceeding to the next instruction are used
		// to delay two clock cycles in one instruction word (rather than
		// using two NOPs).  This was necessary in order to squeeze the
		// loop down to exactly 64 words -- the maximum possible for a
		// relative branch.
 
		asm volatile (
			"headD:"                   "\n\t"       // Clk  Pseudocode
			// Bit 7:
			"out  %[port] , %[hi]"    "\n\t"        // 1    PORT = hi
			"mov  %[n2]   , %[lo]"    "\n\t"        // 1    n2   = lo
			"out  %[port] , %[n1]"    "\n\t"        // 1    PORT = n1
			"rjmp .+0"                "\n\t"        // 2    nop nop
			"sbrc %[byte] , 6"        "\n\t"        // 1-2  if(b & 0x40)
			"mov %[n2]   , %[hi]"    "\n\t"         // 0-1   n2 = hi
			"out  %[port] , %[lo]"    "\n\t"        // 1    PORT = lo
			"rjmp .+0"                "\n\t"        // 2    nop nop
			// Bit 6:
			"out  %[port] , %[hi]"    "\n\t"        // 1    PORT = hi
			"mov  %[n1]   , %[lo]"    "\n\t"        // 1    n1   = lo
			"out  %[port] , %[n2]"    "\n\t"        // 1    PORT = n2
			"rjmp .+0"                "\n\t"        // 2    nop nop
			"sbrc %[byte] , 5"        "\n\t"        // 1-2  if(b & 0x20)
			"mov %[n1]   , %[hi]"    "\n\t"         // 0-1   n1 = hi
			"out  %[port] , %[lo]"    "\n\t"        // 1    PORT = lo
			"rjmp .+0"                "\n\t"        // 2    nop nop
			// Bit 5:
			"out  %[port] , %[hi]"    "\n\t"        // 1    PORT = hi
			"mov  %[n2]   , %[lo]"    "\n\t"        // 1    n2   = lo
			"out  %[port] , %[n1]"    "\n\t"        // 1    PORT = n1
			"rjmp .+0"                "\n\t"        // 2    nop nop
			"sbrc %[byte] , 4"        "\n\t"        // 1-2  if(b & 0x10)
			"mov %[n2]   , %[hi]"    "\n\t"         // 0-1   n2 = hi
			"out  %[port] , %[lo]"    "\n\t"        // 1    PORT = lo
			"rjmp .+0"                "\n\t"        // 2    nop nop
			// Bit 4:
			"out  %[port] , %[hi]"    "\n\t"        // 1    PORT = hi
			"mov  %[n1]   , %[lo]"    "\n\t"        // 1    n1   = lo
			"out  %[port] , %[n2]"    "\n\t"        // 1    PORT = n2
			"rjmp .+0"                "\n\t"        // 2    nop nop
			"sbrc %[byte] , 3"        "\n\t"        // 1-2  if(b & 0x08)
			"mov %[n1]   , %[hi]"    "\n\t"         // 0-1   n1 = hi
			"out  %[port] , %[lo]"    "\n\t"        // 1    PORT = lo
			"rjmp .+0"                "\n\t"        // 2    nop nop
			// Bit 3:
			"out  %[port] , %[hi]"    "\n\t"        // 1    PORT = hi
			"mov  %[n2]   , %[lo]"    "\n\t"        // 1    n2   = lo
			"out  %[port] , %[n1]"    "\n\t"        // 1    PORT = n1
			"rjmp .+0"                "\n\t"        // 2    nop nop
			"sbrc %[byte] , 2"        "\n\t"        // 1-2  if(b & 0x04)
			"mov %[n2]   , %[hi]"    "\n\t"         // 0-1   n2 = hi
			"out  %[port] , %[lo]"    "\n\t"        // 1    PORT = lo
			"rjmp .+0"                "\n\t"        // 2    nop nop
			// Bit 2:
			"out  %[port] , %[hi]"    "\n\t"        // 1    PORT = hi
			"mov  %[n1]   , %[lo]"    "\n\t"        // 1    n1   = lo
			"out  %[port] , %[n2]"    "\n\t"        // 1    PORT = n2
			"rjmp .+0"                "\n\t"        // 2    nop nop
			"sbrc %[byte] , 1"        "\n\t"        // 1-2  if(b & 0x02)
			"mov %[n1]   , %[hi]"    "\n\t"         // 0-1   n1 = hi
			"out  %[port] , %[lo]"    "\n\t"        // 1    PORT = lo
			"rjmp .+0"                "\n\t"        // 2    nop nop
			// Bit 1:
			"out  %[port] , %[hi]"    "\n\t"        // 1    PORT = hi
			"mov  %[n2]   , %[lo]"    "\n\t"        // 1    n2   = lo
			"out  %[port] , %[n1]"    "\n\t"        // 1    PORT = n1
			"rjmp .+0"                "\n\t"        // 2    nop nop
			"sbrc %[byte] , 0"        "\n\t"        // 1-2  if(b & 0x01)
			"mov %[n2]   , %[hi]"    "\n\t"         // 0-1   n2 = hi
			"out  %[port] , %[lo]"    "\n\t"        // 1    PORT = lo
			"sbiw %[count], 1"        "\n\t"        // 2    i-- (don't act on Z flag yet)
			// Bit 0:
			"out  %[port] , %[hi]"    "\n\t"        // 1    PORT = hi
			"mov  %[n1]   , %[lo]"    "\n\t"        // 1    n1   = lo
			"out  %[port] , %[n2]"    "\n\t"        // 1    PORT = n2
			"ld   %[byte] , %a[ptr]+" "\n\t"        // 2    b = *ptr++
			"sbrc %[byte] , 7"        "\n\t"        // 1-2  if(b & 0x80)
			"mov %[n1]   , %[hi]"    "\n\t"         // 0-1   n1 = hi
			"out  %[port] , %[lo]"    "\n\t"        // 1    PORT = lo
			"brne headD"              "\n"          // 2    while(i) (Z flag set above)
			: [byte]  "+r" (b),
			[n1]    "+r" (n1),
			[n2]    "+r" (n2),
			[count] "+w" (i)
			: [port]   "I" (_SFR_IO_ADDR(PORTD)),
			[ptr]    "e" (ptr),
			[hi]     "r" (hi),
			[lo]     "r" (lo));
	} else if (port == &PORTB) {
#endif // PORTD
 
	// Same as above, just switched to PORTB and stripped of comments.
	hi = PORTB |  pinMask;
	lo = PORTB & ~pinMask;
	n1 = lo;
	if (b & 0x80) {
		n1 = hi;
	}
 
	asm volatile (
		"headB:"                   "\n\t"
		"out  %[port] , %[hi]"    "\n\t"
		"mov  %[n2]   , %[lo]"    "\n\t"
		"out  %[port] , %[n1]"    "\n\t"
		"rjmp .+0"                "\n\t"
		"sbrc %[byte] , 6"        "\n\t"
		"mov %[n2]   , %[hi]"    "\n\t"
		"out  %[port] , %[lo]"    "\n\t"
		"rjmp .+0"                "\n\t"
		"out  %[port] , %[hi]"    "\n\t"
		"mov  %[n1]   , %[lo]"    "\n\t"
		"out  %[port] , %[n2]"    "\n\t"
		"rjmp .+0"                "\n\t"
		"sbrc %[byte] , 5"        "\n\t"
		"mov %[n1]   , %[hi]"    "\n\t"
		"out  %[port] , %[lo]"    "\n\t"
		"rjmp .+0"                "\n\t"
		"out  %[port] , %[hi]"    "\n\t"
		"mov  %[n2]   , %[lo]"    "\n\t"
		"out  %[port] , %[n1]"    "\n\t"
		"rjmp .+0"                "\n\t"
		"sbrc %[byte] , 4"        "\n\t"
		"mov %[n2]   , %[hi]"    "\n\t"
		"out  %[port] , %[lo]"    "\n\t"
		"rjmp .+0"                "\n\t"
		"out  %[port] , %[hi]"    "\n\t"
		"mov  %[n1]   , %[lo]"    "\n\t"
		"out  %[port] , %[n2]"    "\n\t"
		"rjmp .+0"                "\n\t"
		"sbrc %[byte] , 3"        "\n\t"
		"mov %[n1]   , %[hi]"    "\n\t"
		"out  %[port] , %[lo]"    "\n\t"
		"rjmp .+0"                "\n\t"
		"out  %[port] , %[hi]"    "\n\t"
		"mov  %[n2]   , %[lo]"    "\n\t"
		"out  %[port] , %[n1]"    "\n\t"
		"rjmp .+0"                "\n\t"
		"sbrc %[byte] , 2"        "\n\t"
		"mov %[n2]   , %[hi]"    "\n\t"
		"out  %[port] , %[lo]"    "\n\t"
		"rjmp .+0"                "\n\t"
		"out  %[port] , %[hi]"    "\n\t"
		"mov  %[n1]   , %[lo]"    "\n\t"
		"out  %[port] , %[n2]"    "\n\t"
		"rjmp .+0"                "\n\t"
		"sbrc %[byte] , 1"        "\n\t"
		"mov %[n1]   , %[hi]"    "\n\t"
		"out  %[port] , %[lo]"    "\n\t"
		"rjmp .+0"                "\n\t"
		"out  %[port] , %[hi]"    "\n\t"
		"mov  %[n2]   , %[lo]"    "\n\t"
		"out  %[port] , %[n1]"    "\n\t"
		"rjmp .+0"                "\n\t"
		"sbrc %[byte] , 0"        "\n\t"
		"mov %[n2]   , %[hi]"    "\n\t"
		"out  %[port] , %[lo]"    "\n\t"
		"sbiw %[count], 1"        "\n\t"
		"out  %[port] , %[hi]"    "\n\t"
		"mov  %[n1]   , %[lo]"    "\n\t"
		"out  %[port] , %[n2]"    "\n\t"
		"ld   %[byte] , %a[ptr]+" "\n\t"
		"sbrc %[byte] , 7"        "\n\t"
		"mov %[n1]   , %[hi]"    "\n\t"
		"out  %[port] , %[lo]"    "\n\t"
		"brne headB"              "\n"
		: [byte] "+r" (b), [n1] "+r" (n1), [n2] "+r" (n2), [count] "+w" (i)
		: [port] "I" (_SFR_IO_ADDR(PORTB)), [ptr] "e" (ptr), [hi] "r" (hi),
		[lo] "r" (lo));
 
#ifdef PORTD
}        // endif PORTB
#endif
 
#ifdef NEO_KHZ400
} else {   // end 800 KHz, do 400 KHz
	   // Timing is more relaxed; unrolling the inner loop for each bit is
	   // not necessary.  Still using the peculiar RJMPs as 2X NOPs, not out
	   // of need but just to trim the code size down a little.
	   // This 400-KHz-datastream-on-8-MHz-CPU code is not quite identical
	   // to the 800-on-16 code later -- the hi/lo timing between WS2811 and
	   // WS2812 is not simply a 2:1 scale!
 
	// 20 inst. clocks per bit: HHHHxxxxxxLLLLLLLLLL
	// ST instructions:         ^   ^     ^          (T=0,4,10)
 
	volatile uint8_t next, bit;
 
	hi = *port |  pinMask;
	lo = *port & ~pinMask;
	next = lo;
	bit = 8;
 
	asm volatile (
		"head20:"                  "\n\t"       // Clk  Pseudocode    (T =  0)
		"st   %a[port], %[hi]"    "\n\t"        // 2    PORT = hi     (T =  2)
		"sbrc %[byte] , 7"        "\n\t"        // 1-2  if(b & 128)
		"mov  %[next], %[hi]"    "\n\t"         // 0-1   next = hi    (T =  4)
		"st   %a[port], %[next]"  "\n\t"        // 2    PORT = next   (T =  6)
		"mov  %[next] , %[lo]"    "\n\t"        // 1    next = lo     (T =  7)
		"dec  %[bit]"             "\n\t"        // 1    bit--         (T =  8)
		"breq nextbyte20"         "\n\t"        // 1-2  if(bit == 0)
		"rol  %[byte]"            "\n\t"        // 1    b <<= 1       (T = 10)
		"st   %a[port], %[lo]"    "\n\t"        // 2    PORT = lo     (T = 12)
		"rjmp .+0"                "\n\t"        // 2    nop nop       (T = 14)
		"rjmp .+0"                "\n\t"        // 2    nop nop       (T = 16)
		"rjmp .+0"                "\n\t"        // 2    nop nop       (T = 18)
		"rjmp head20"             "\n\t"        // 2    -> head20 (next bit out)
		"nextbyte20:"              "\n\t"       //                    (T = 10)
		"st   %a[port], %[lo]"    "\n\t"        // 2    PORT = lo     (T = 12)
		"nop"                     "\n\t"        // 1    nop           (T = 13)
		"ldi  %[bit]  , 8"        "\n\t"        // 1    bit = 8       (T = 14)
		"ld   %[byte] , %a[ptr]+" "\n\t"        // 2    b = *ptr++    (T = 16)
		"sbiw %[count], 1"        "\n\t"        // 2    i--           (T = 18)
		"brne head20"             "\n"          // 2    if(i != 0) -> (next byte)
		: [port]  "+e" (port),
		[byte]  "+r" (b),
		[bit]   "+r" (bit),
		[next]  "+r" (next),
		[count] "+w" (i)
		: [hi]    "r" (hi),
		[lo]    "r" (lo),
		[ptr]   "e" (ptr));
}
#endif // NEO_KHZ400
 
// 12 MHz(ish) AVR --------------------------------------------------------
#elif (F_CPU >= 11100000UL) && (F_CPU <= 14300000UL)
#ifdef NEO_KHZ400 // 800 KHz check needed only if 400 KHz support enabled
	if (is800KHz) {
#endif
 
	// In the 12 MHz case, an optimized 800 KHz datastream (no dead time
	// between bytes) requires a PORT-specific loop similar to the 8 MHz
	// code (but a little more relaxed in this case).
 
	// 15 instruction clocks per bit: HHHHxxxxxxLLLLL
	// OUT instructions:              ^   ^     ^     (T=0,4,10)
 
	volatile uint8_t next;
 
#ifdef PORTD
	if (port == &PORTD) {
		hi = PORTD |  pinMask;
		lo = PORTD & ~pinMask;
		next = lo;
		if (b & 0x80) {
			next = hi;
		}
 
		// Don't "optimize" the OUT calls into the bitTime subroutine;
		// we're exploiting the RCALL and RET as 3- and 4-cycle NOPs!
		asm volatile (
			"headD:"                   "\n\t"       //        (T =  0)
			"out   %[port], %[hi]"    "\n\t"        //        (T =  1)
			"rcall bitTimeD"          "\n\t"        // Bit 7  (T = 15)
			"out   %[port], %[hi]"    "\n\t"
			"rcall bitTimeD"          "\n\t"        // Bit 6
			"out   %[port], %[hi]"    "\n\t"
			"rcall bitTimeD"          "\n\t"        // Bit 5
			"out   %[port], %[hi]"    "\n\t"
			"rcall bitTimeD"          "\n\t"        // Bit 4
			"out   %[port], %[hi]"    "\n\t"
			"rcall bitTimeD"          "\n\t"        // Bit 3
			"out   %[port], %[hi]"    "\n\t"
			"rcall bitTimeD"          "\n\t"        // Bit 2
			"out   %[port], %[hi]"    "\n\t"
			"rcall bitTimeD"          "\n\t"        // Bit 1
			// Bit 0:
			"out  %[port] , %[hi]"    "\n\t"        // 1    PORT = hi    (T =  1)
			"rjmp .+0"                "\n\t"        // 2    nop nop      (T =  3)
			"ld   %[byte] , %a[ptr]+" "\n\t"        // 2    b = *ptr++   (T =  5)
			"out  %[port] , %[next]"  "\n\t"        // 1    PORT = next  (T =  6)
			"mov  %[next] , %[lo]"    "\n\t"        // 1    next = lo    (T =  7)
			"sbrc %[byte] , 7"        "\n\t"        // 1-2  if(b & 0x80) (T =  8)
			"mov %[next] , %[hi]"    "\n\t"         // 0-1    next = hi  (T =  9)
			"nop"                     "\n\t"        // 1                 (T = 10)
			"out  %[port] , %[lo]"    "\n\t"        // 1    PORT = lo    (T = 11)
			"sbiw %[count], 1"        "\n\t"        // 2    i--          (T = 13)
			"brne headD"              "\n\t"        // 2    if(i != 0) -> (next byte)
			"rjmp doneD"             "\n\t"
			"bitTimeD:"               "\n\t"        //      nop nop nop     (T =  4)
			"out  %[port], %[next]"  "\n\t"         // 1    PORT = next     (T =  5)
			"mov  %[next], %[lo]"    "\n\t"         // 1    next = lo       (T =  6)
			"rol  %[byte]"           "\n\t"         // 1    b <<= 1         (T =  7)
			"sbrc %[byte], 7"        "\n\t"         // 1-2  if(b & 0x80)    (T =  8)
			"mov %[next], %[hi]"    "\n\t"          // 0-1   next = hi      (T =  9)
			"nop"                    "\n\t"         // 1                    (T = 10)
			"out  %[port], %[lo]"    "\n\t"         // 1    PORT = lo       (T = 11)
			"ret"                    "\n\t"         // 4    nop nop nop nop (T = 15)
			"doneD:"                 "\n"
			: [byte]  "+r" (b),
			[next]  "+r" (next),
			[count] "+w" (i)
			: [port]   "I" (_SFR_IO_ADDR(PORTD)),
			[ptr]    "e" (ptr),
			[hi]     "r" (hi),
			[lo]     "r" (lo));
	} else if (port == &PORTB) {
#endif // PORTD
 
	hi = PORTB |  pinMask;
	lo = PORTB & ~pinMask;
	next = lo;
	if (b & 0x80) {
		next = hi;
	}
 
	// Same as above, just set for PORTB & stripped of comments
	asm volatile (
		"headB:"                   "\n\t"
		"out   %[port], %[hi]"    "\n\t"
		"rcall bitTimeB"          "\n\t"
		"out   %[port], %[hi]"    "\n\t"
		"rcall bitTimeB"          "\n\t"
		"out   %[port], %[hi]"    "\n\t"
		"rcall bitTimeB"          "\n\t"
		"out   %[port], %[hi]"    "\n\t"
		"rcall bitTimeB"          "\n\t"
		"out   %[port], %[hi]"    "\n\t"
		"rcall bitTimeB"          "\n\t"
		"out   %[port], %[hi]"    "\n\t"
		"rcall bitTimeB"          "\n\t"
		"out   %[port], %[hi]"    "\n\t"
		"rcall bitTimeB"          "\n\t"
		"out  %[port] , %[hi]"    "\n\t"
		"rjmp .+0"                "\n\t"
		"ld   %[byte] , %a[ptr]+" "\n\t"
		"out  %[port] , %[next]"  "\n\t"
		"mov  %[next] , %[lo]"    "\n\t"
		"sbrc %[byte] , 7"        "\n\t"
		"mov %[next] , %[hi]"    "\n\t"
		"nop"                     "\n\t"
		"out  %[port] , %[lo]"    "\n\t"
		"sbiw %[count], 1"        "\n\t"
		"brne headB"              "\n\t"
		"rjmp doneB"             "\n\t"
		"bitTimeB:"               "\n\t"
		"out  %[port], %[next]"  "\n\t"
		"mov  %[next], %[lo]"    "\n\t"
		"rol  %[byte]"           "\n\t"
		"sbrc %[byte], 7"        "\n\t"
		"mov %[next], %[hi]"    "\n\t"
		"nop"                    "\n\t"
		"out  %[port], %[lo]"    "\n\t"
		"ret"                    "\n\t"
		"doneB:"                 "\n"
		: [byte] "+r" (b), [next] "+r" (next), [count] "+w" (i)
		: [port] "I" (_SFR_IO_ADDR(PORTB)), [ptr] "e" (ptr), [hi] "r" (hi),
		[lo] "r" (lo));
 
#ifdef PORTD
}
#endif
 
#ifdef NEO_KHZ400
} else {   // 400 KHz
	   // 30 instruction clocks per bit: HHHHHHxxxxxxxxxLLLLLLLLLLLLLLL
	   // ST instructions:               ^     ^        ^    (T=0,6,15)
 
	volatile uint8_t next, bit;
 
	hi = *port |  pinMask;
	lo = *port & ~pinMask;
	next = lo;
	bit = 8;
 
	asm volatile (
		"head30:"                  "\n\t"       // Clk  Pseudocode    (T =  0)
		"st   %a[port], %[hi]"    "\n\t"        // 2    PORT = hi     (T =  2)
		"sbrc %[byte] , 7"        "\n\t"        // 1-2  if(b & 128)
		"mov  %[next], %[hi]"    "\n\t"         // 0-1   next = hi    (T =  4)
		"rjmp .+0"                "\n\t"        // 2    nop nop       (T =  6)
		"st   %a[port], %[next]"  "\n\t"        // 2    PORT = next   (T =  8)
		"rjmp .+0"                "\n\t"        // 2    nop nop       (T = 10)
		"rjmp .+0"                "\n\t"        // 2    nop nop       (T = 12)
		"rjmp .+0"                "\n\t"        // 2    nop nop       (T = 14)
		"nop"                     "\n\t"        // 1    nop           (T = 15)
		"st   %a[port], %[lo]"    "\n\t"        // 2    PORT = lo     (T = 17)
		"rjmp .+0"                "\n\t"        // 2    nop nop       (T = 19)
		"dec  %[bit]"             "\n\t"        // 1    bit--         (T = 20)
		"breq nextbyte30"         "\n\t"        // 1-2  if(bit == 0)
		"rol  %[byte]"            "\n\t"        // 1    b <<= 1       (T = 22)
		"rjmp .+0"                "\n\t"        // 2    nop nop       (T = 24)
		"rjmp .+0"                "\n\t"        // 2    nop nop       (T = 26)
		"rjmp .+0"                "\n\t"        // 2    nop nop       (T = 28)
		"rjmp head30"             "\n\t"        // 2    -> head30 (next bit out)
		"nextbyte30:"              "\n\t"       //                    (T = 22)
		"nop"                     "\n\t"        // 1    nop           (T = 23)
		"ldi  %[bit]  , 8"        "\n\t"        // 1    bit = 8       (T = 24)
		"ld   %[byte] , %a[ptr]+" "\n\t"        // 2    b = *ptr++    (T = 26)
		"sbiw %[count], 1"        "\n\t"        // 2    i--           (T = 28)
		"brne head30"             "\n"          // 1-2  if(i != 0) -> (next byte)
		: [port]  "+e" (port),
		[byte]  "+r" (b),
		[bit]   "+r" (bit),
		[next]  "+r" (next),
		[count] "+w" (i)
		: [hi]     "r" (hi),
		[lo]     "r" (lo),
		[ptr]    "e" (ptr));
}
#endif // NEO_KHZ400
 
// 16 MHz(ish) AVR --------------------------------------------------------
#elif (F_CPU >= 15400000UL) && (F_CPU <= 19000000L)
#ifdef NEO_KHZ400 // 800 KHz check needed only if 400 KHz support enabled
	if (is800KHz) {
#endif
 
	// WS2811 and WS2812 have different hi/lo duty cycles; this is
	// similar but NOT an exact copy of the prior 400-on-8 code.
 
	// 20 inst. clocks per bit: HHHHHxxxxxxxxLLLLLLL
	// ST instructions:         ^   ^        ^       (T=0,5,13)
 
	volatile uint8_t next, bit;
 
	hi = *port |  pinMask;
	lo = *port & ~pinMask;
	next = lo;
	bit = 8;
 
	asm volatile (
		"head20:"                   "\n\t"      // Clk  Pseudocode    (T =  0)
		"st   %a[port],  %[hi]"    "\n\t"       // 2    PORT = hi     (T =  2)
		"sbrc %[byte],  7"         "\n\t"       // 1-2  if(b & 128)
		"mov  %[next], %[hi]"     "\n\t"        // 0-1   next = hi    (T =  4)
		"dec  %[bit]"              "\n\t"       // 1    bit--         (T =  5)
		"st   %a[port],  %[next]"  "\n\t"       // 2    PORT = next   (T =  7)
		"mov  %[next] ,  %[lo]"    "\n\t"       // 1    next = lo     (T =  8)
		"breq nextbyte20"          "\n\t"       // 1-2  if(bit == 0) (from dec above)
		"rol  %[byte]"             "\n\t"       // 1    b <<= 1       (T = 10)
		"rjmp .+0"                 "\n\t"       // 2    nop nop       (T = 12)
		"nop"                      "\n\t"       // 1    nop           (T = 13)
		"st   %a[port],  %[lo]"    "\n\t"       // 2    PORT = lo     (T = 15)
		"nop"                      "\n\t"       // 1    nop           (T = 16)
		"rjmp .+0"                 "\n\t"       // 2    nop nop       (T = 18)
		"rjmp head20"              "\n\t"       // 2    -> head20 (next bit out)
		"nextbyte20:"               "\n\t"      //                    (T = 10)
		"ldi  %[bit]  ,  8"        "\n\t"       // 1    bit = 8       (T = 11)
		"ld   %[byte] ,  %a[ptr]+" "\n\t"       // 2    b = *ptr++    (T = 13)
		"st   %a[port], %[lo]"     "\n\t"       // 2    PORT = lo     (T = 15)
		"nop"                      "\n\t"       // 1    nop           (T = 16)
		"sbiw %[count], 1"         "\n\t"       // 2    i--           (T = 18)
		"brne head20"             "\n"          // 2    if(i != 0) -> (next byte)
		: [port]  "+e" (port),
		[byte]  "+r" (b),
		[bit]   "+r" (bit),
		[next]  "+r" (next),
		[count] "+w" (i)
		: [ptr]    "e" (ptr),
		[hi]     "r" (hi),
		[lo]     "r" (lo));
 
#ifdef NEO_KHZ400
} else {   // 400 KHz
	   // The 400 KHz clock on 16 MHz MCU is the most 'relaxed' version.
 
	// 40 inst. clocks per bit: HHHHHHHHxxxxxxxxxxxxLLLLLLLLLLLLLLLLLLLL
	// ST instructions:         ^       ^           ^         (T=0,8,20)
 
	volatile uint8_t next, bit;
 
	hi = *port |  pinMask;
	lo = *port & ~pinMask;
	next = lo;
	bit = 8;
 
	asm volatile (
		"head40:"                  "\n\t"       // Clk  Pseudocode    (T =  0)
		"st   %a[port], %[hi]"    "\n\t"        // 2    PORT = hi     (T =  2)
		"sbrc %[byte] , 7"        "\n\t"        // 1-2  if(b & 128)
		"mov  %[next] , %[hi]"   "\n\t"         // 0-1   next = hi    (T =  4)
		"rjmp .+0"                "\n\t"        // 2    nop nop       (T =  6)
		"rjmp .+0"                "\n\t"        // 2    nop nop       (T =  8)
		"st   %a[port], %[next]"  "\n\t"        // 2    PORT = next   (T = 10)
		"rjmp .+0"                "\n\t"        // 2    nop nop       (T = 12)
		"rjmp .+0"                "\n\t"        // 2    nop nop       (T = 14)
		"rjmp .+0"                "\n\t"        // 2    nop nop       (T = 16)
		"rjmp .+0"                "\n\t"        // 2    nop nop       (T = 18)
		"rjmp .+0"                "\n\t"        // 2    nop nop       (T = 20)
		"st   %a[port], %[lo]"    "\n\t"        // 2    PORT = lo     (T = 22)
		"nop"                     "\n\t"        // 1    nop           (T = 23)
		"mov  %[next] , %[lo]"    "\n\t"        // 1    next = lo     (T = 24)
		"dec  %[bit]"             "\n\t"        // 1    bit--         (T = 25)
		"breq nextbyte40"         "\n\t"        // 1-2  if(bit == 0)
		"rol  %[byte]"            "\n\t"        // 1    b <<= 1       (T = 27)
		"nop"                     "\n\t"        // 1    nop           (T = 28)
		"rjmp .+0"                "\n\t"        // 2    nop nop       (T = 30)
		"rjmp .+0"                "\n\t"        // 2    nop nop       (T = 32)
		"rjmp .+0"                "\n\t"        // 2    nop nop       (T = 34)
		"rjmp .+0"                "\n\t"        // 2    nop nop       (T = 36)
		"rjmp .+0"                "\n\t"        // 2    nop nop       (T = 38)
		"rjmp head40"             "\n\t"        // 2    -> head40 (next bit out)
		"nextbyte40:"              "\n\t"       //                    (T = 27)
		"ldi  %[bit]  , 8"        "\n\t"        // 1    bit = 8       (T = 28)
		"ld   %[byte] , %a[ptr]+" "\n\t"        // 2    b = *ptr++    (T = 30)
		"rjmp .+0"                "\n\t"        // 2    nop nop       (T = 32)
		"st   %a[port], %[lo]"    "\n\t"        // 2    PORT = lo     (T = 34)
		"rjmp .+0"                "\n\t"        // 2    nop nop       (T = 36)
		"sbiw %[count], 1"        "\n\t"        // 2    i--           (T = 38)
		"brne head40"             "\n"          // 1-2  if(i != 0) -> (next byte)
		: [port]  "+e" (port),
		[byte]  "+r" (b),
		[bit]   "+r" (bit),
		[next]  "+r" (next),
		[count] "+w" (i)
		: [ptr]    "e" (ptr),
		[hi]     "r" (hi),
		[lo]     "r" (lo));
}
#endif // NEO_KHZ400
#else
#error "CPU SPEED NOT SUPPORTED"
#endif // end F_CPU ifdefs on __AVR__
 
// END AVR ----------------------------------------------------------------
#elif defined(__arm__)
// ARM MCUs -- Teensy 3.0, 3.1, LC, Arduino Due ---------------------------
 
#if defined(__MK20DX128__) || defined(__MK20DX256__) // Teensy 3.0 & 3.1
#define CYCLES_800_T0H		(F_CPU / 4000000)
#define CYCLES_800_T1H		(F_CPU / 1250000)
#define CYCLES_800		(F_CPU /  800000)
#define CYCLES_400_T0H		(F_CPU / 2000000)
#define CYCLES_400_T1H		(F_CPU /  833333)
#define CYCLES_400		(F_CPU /  400000)
 
	uint8_t *p = pixels,
	    *end = p + numBytes, pix, mask;
	volatile uint8_t *set = portSetRegister(pin),
	    *clr = portClearRegister(pin);
	uint32_t cyc;
 
	ARM_DEMCR |= ARM_DEMCR_TRCENA;
	ARM_DWT_CTRL |= ARM_DWT_CTRL_CYCCNTENA;
 
#ifdef NEO_KHZ400 // 800 KHz check needed only if 400 KHz support enabled
	if (is800KHz) {
#endif
	cyc = ARM_DWT_CYCCNT + CYCLES_800;
	while (p < end)
	{
		pix = *p++;
		for (mask = 0x80; mask; mask >>= 1)
		{
			while (ARM_DWT_CYCCNT - cyc < CYCLES_800)
			{
			}
			cyc = ARM_DWT_CYCCNT;
			*set = 1;
			if (pix & mask) {
				while (ARM_DWT_CYCCNT - cyc < CYCLES_800_T1H)
				{
				}
			} else {
				while (ARM_DWT_CYCCNT - cyc < CYCLES_800_T0H)
				{
				}
			}
			*clr = 1;
		}
	}
	while (ARM_DWT_CYCCNT - cyc < CYCLES_800)
	{
	}
#ifdef NEO_KHZ400
} else {   // 400 kHz bitstream
	cyc = ARM_DWT_CYCCNT + CYCLES_400;
	while (p < end)
	{
		pix = *p++;
		for (mask = 0x80; mask; mask >>= 1)
		{
			while (ARM_DWT_CYCCNT - cyc < CYCLES_400)
			{
			}
			cyc = ARM_DWT_CYCCNT;
			*set = 1;
			if (pix & mask) {
				while (ARM_DWT_CYCCNT - cyc < CYCLES_400_T1H)
				{
				}
			} else {
				while (ARM_DWT_CYCCNT - cyc < CYCLES_400_T0H)
				{
				}
			}
			*clr = 1;
		}
	}
	while (ARM_DWT_CYCCNT - cyc < CYCLES_400)
	{
	}
}
#endif // NEO_KHZ400
#elif defined(__MKL26Z64__) // Teensy-LC
#if F_CPU == 48000000
	uint8_t *p = pixels,
	    pix, count, dly,
	    bitmask = digitalPinToBitMask(pin);
	volatile uint8_t *reg = portSetRegister(pin);
	uint32_t num = numBytes;
	asm volatile (
		"L%=_begin:"                            "\n\t"
		"ldrb	%[pix], [%[p], #0]"       "\n\t"
		"lsl	%[pix], #24"           "\n\t"
		"movs	%[count], #7"             "\n\t"
		"L%=_loop:"                             "\n\t"
		"lsl	%[pix], #1"            "\n\t"
		"bcs	L%=_loop_one"          "\n\t"
		"L%=_loop_zero:"
		"strb	%[bitmask], [%[reg], #0]" "\n\t"
		"movs	%[dly], #4"               "\n\t"
		"L%=_loop_delay_T0H:"                   "\n\t"
		"sub	%[dly], #1"            "\n\t"
		"bne	L%=_loop_delay_T0H"    "\n\t"
		"strb	%[bitmask], [%[reg], #4]" "\n\t"
		"movs	%[dly], #13"              "\n\t"
		"L%=_loop_delay_T0L:"                   "\n\t"
		"sub	%[dly], #1"            "\n\t"
		"bne	L%=_loop_delay_T0L"    "\n\t"
		"b	L%=_next"        "\n\t"
		"L%=_loop_one:"
		"strb	%[bitmask], [%[reg], #0]" "\n\t"
		"movs	%[dly], #13"              "\n\t"
		"L%=_loop_delay_T1H:"                   "\n\t"
		"sub	%[dly], #1"            "\n\t"
		"bne	L%=_loop_delay_T1H"    "\n\t"
		"strb	%[bitmask], [%[reg], #4]" "\n\t"
		"movs	%[dly], #4"               "\n\t"
		"L%=_loop_delay_T1L:"                   "\n\t"
		"sub	%[dly], #1"            "\n\t"
		"bne	L%=_loop_delay_T1L"    "\n\t"
		"nop"                                   "\n\t"
		"L%=_next:"                             "\n\t"
		"sub	%[count], #1"          "\n\t"
		"bne	L%=_loop"              "\n\t"
		"lsl	%[pix], #1"            "\n\t"
		"bcs	L%=_last_one"          "\n\t"
		"L%=_last_zero:"
		"strb	%[bitmask], [%[reg], #0]" "\n\t"
		"movs	%[dly], #4"               "\n\t"
		"L%=_last_delay_T0H:"                   "\n\t"
		"sub	%[dly], #1"            "\n\t"
		"bne	L%=_last_delay_T0H"    "\n\t"
		"strb	%[bitmask], [%[reg], #4]" "\n\t"
		"movs	%[dly], #10"              "\n\t"
		"L%=_last_delay_T0L:"                   "\n\t"
		"sub	%[dly], #1"            "\n\t"
		"bne	L%=_last_delay_T0L"    "\n\t"
		"b	L%=_repeat"      "\n\t"
		"L%=_last_one:"
		"strb	%[bitmask], [%[reg], #0]" "\n\t"
		"movs	%[dly], #13"              "\n\t"
		"L%=_last_delay_T1H:"                   "\n\t"
		"sub	%[dly], #1"            "\n\t"
		"bne	L%=_last_delay_T1H"    "\n\t"
		"strb	%[bitmask], [%[reg], #4]" "\n\t"
		"movs	%[dly], #1"               "\n\t"
		"L%=_last_delay_T1L:"                   "\n\t"
		"sub	%[dly], #1"            "\n\t"
		"bne	L%=_last_delay_T1L"    "\n\t"
		"nop"                                   "\n\t"
		"L%=_repeat:"                           "\n\t"
		"add	%[p], #1"              "\n\t"
		"sub	%[num], #1"            "\n\t"
		"bne	L%=_begin"             "\n\t"
		"L%=_done:"                             "\n\t"
		: [p] "+r" (p),
		[pix] "=&r" (pix),
		[count] "=&r" (count),
		[dly] "=&r" (dly),
		[num] "+r" (num)
		: [bitmask] "r" (bitmask),
		[reg] "r" (reg)
		);
#else
#error "Sorry, only 48 MHz is supported, please set Tools > CPU Speed to 48 MHz"
#endif // F_CPU == 48000000
#elif defined(__SAMD21G18A__) // Arduino Zero
	// Tried this with a timer/counter, couldn't quite get adequate
	// resolution.  So yay, you get a load of goofball NOPs...
 
	uint8_t *ptr, *end, p, bitMask, portNum;
	uint32_t pinMask;
 
	portNum = g_APinDescription[pin].ulPort;
	pinMask = 1ul << g_APinDescription[pin].ulPin;
	ptr = pixels;
	end = ptr + numBytes;
	p = *ptr++;
	bitMask = 0x80;
 
	volatile uint32_t *set = &(PORT->Group[portNum].OUTSET.reg),
	    *clr = &(PORT->Group[portNum].OUTCLR.reg);
 
#ifdef NEO_KHZ400 // 800 KHz check needed only if 400 KHz support enabled
	if (is800KHz) {
#endif
	for ( ; ;)
	{
		*set = pinMask;
		asm ("nop; nop; nop; nop; nop; nop; nop; nop;");
		if (p & bitMask) {
			asm ("nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop;");
			*clr = pinMask;
		} else {
			*clr = pinMask;
			asm ("nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop;");
		}
		if (bitMask >>= 1) {
			asm ("nop; nop; nop; nop; nop; nop; nop; nop; nop;");
		} else {
			if (ptr >= end) {
				break;
			}
			p = *ptr++;
			bitMask = 0x80;
		}
	}
#ifdef NEO_KHZ400
} else {   // 400 KHz bitstream
	for ( ; ;)
	{
		*set = pinMask;
		asm ("nop; nop; nop; nop; nop; nop; nop; nop; nop; nop; nop;");
		if (p & bitMask) {
			asm ("nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop;");
			*clr = pinMask;
		} else {
			*clr = pinMask;
			asm ("nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop;");
		}
		asm ("nop; nop; nop; nop; nop; nop; nop; nop;"
		"nop; nop; nop; nop; nop; nop; nop; nop;"
		"nop; nop; nop; nop; nop; nop; nop; nop;"
		"nop; nop; nop; nop; nop; nop; nop; nop;");
		if (bitMask >>= 1) {
			asm ("nop; nop; nop; nop; nop; nop; nop;");
		} else {
			if (ptr >= end) {
				break;
			}
			p = *ptr++;
			bitMask = 0x80;
		}
	}
}
#endif
#elif defined (ARDUINO_STM32_FEATHER) // FEATHER WICED (120MHz)
	// Tried this with a timer/counter, couldn't quite get adequate
	// resolution.  So yay, you get a load of goofball NOPs...
 
	uint8_t *ptr, *end, p, bitMask;
	uint32_t pinMask;
 
	pinMask = BIT(PIN_MAP[pin].gpio_bit);
	ptr = pixels;
	end = ptr + numBytes;
	p = *ptr++;
	bitMask = 0x80;
 
	volatile uint16_t *set = &(PIN_MAP[pin].gpio_device->regs->BSRRL);
	volatile uint16_t *clr = &(PIN_MAP[pin].gpio_device->regs->BSRRH);
 
#ifdef NEO_KHZ400 // 800 KHz check needed only if 400 KHz support enabled
	if (is800KHz) {
#endif
	for ( ; ;)
	{
		if (p & bitMask) { // ONE
			// High 800ns
			*set = pinMask;
			asm ("nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop;");
			// Low 450ns
			*clr = pinMask;
			asm ("nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop;");
		} else { // ZERO
			// High 400ns
			*set = pinMask;
			asm ("nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop;");
			// Low 850ns
			*clr = pinMask;
			asm ("nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop; nop; nop; nop; nop;"
			"nop; nop; nop; nop;");
		}
		if (bitMask >>= 1) {
			// Move on to the next pixel
			asm ("nop;");
		} else {
			if (ptr >= end) {
				break;
			}
			p = *ptr++;
			bitMask = 0x80;
		}
	}
#ifdef NEO_KHZ400
} else {   // 400 KHz bitstream
	   // ToDo!
}
#endif
#else // Other ARM architecture -- Presumed Arduino Due
#define SCALE		VARIANT_MCK / 2UL / 1000000UL
#define INST		(2UL * F_CPU / VARIANT_MCK)
#define TIME_800_0	((int)(0.40 * SCALE + 0.5) - (5 * INST))
#define TIME_800_1	((int)(0.80 * SCALE + 0.5) - (5 * INST))
#define PERIOD_800	((int)(1.25 * SCALE + 0.5) - (5 * INST))
#define TIME_400_0	((int)(0.50 * SCALE + 0.5) - (5 * INST))
#define TIME_400_1	((int)(1.20 * SCALE + 0.5) - (5 * INST))
#define PERIOD_400	((int)(2.50 * SCALE + 0.5) - (5 * INST))
 
	int pinMask, time0, time1, period, t;
	Pio *port;
	volatile WoReg *portSet, *portClear, *timeValue, *timeReset;
	uint8_t *p, *end, pix, mask;
 
	pmc_set_writeprotect(false);
	pmc_enable_periph_clk((uint32_t)TC3_IRQn);
	TC_Configure(TC1, 0,
	    TC_CMR_WAVE | TC_CMR_WAVSEL_UP | TC_CMR_TCCLKS_TIMER_CLOCK1);
	TC_Start(TC1, 0);
 
	pinMask = g_APinDescription[pin].ulPin;         // Don't 'optimize' these into
	port = g_APinDescription[pin].pPort;            // declarations above.  Want to
	portSet = &(port->PIO_SODR);                    // burn a few cycles after
	portClear = &(port->PIO_CODR);                  // starting timer to minimize
	timeValue = &(TC1->TC_CHANNEL[0].TC_CV);        // the initial 'while'.
	timeReset = &(TC1->TC_CHANNEL[0].TC_CCR);
	p = pixels;
	end = p + numBytes;
	pix = *p++;
	mask = 0x80;
 
#ifdef NEO_KHZ400 // 800 KHz check needed only if 400 KHz support enabled
	if (is800KHz) {
#endif
	time0 = TIME_800_0;
	time1 = TIME_800_1;
	period = PERIOD_800;
#ifdef NEO_KHZ400
} else {   // 400 KHz bitstream
	time0 = TIME_400_0;
	time1 = TIME_400_1;
	period = PERIOD_400;
}
#endif
 
	for (t = time0; ; t = time0)
	{
		if (pix & mask) {
			t = time1;
		}
		while (*timeValue < period)
		{
		}
		*portSet = pinMask;
		*timeReset = TC_CCR_CLKEN | TC_CCR_SWTRG;
		while (*timeValue < t)
		{
		}
		*portClear = pinMask;
		if (!(mask >>= 1)) {    // This 'inside-out' loop logic utilizes
			if (p >= end) {
				break;  // idle time to minimize inter-byte delays.
			}
			pix = *p++;
			mask = 0x80;
		}
	}
	while (*timeValue < period) // Wait for last bit
	{
	}
	TC_Stop(TC1, 0);
#endif // end Due
 
// END ARM ----------------------------------------------------------------
#elif defined(ESP8266)
// ESP8266 ----------------------------------------------------------------
 
	// ESP8266 show() is external to enforce ICACHE_RAM_ATTR execution
	espShow(pin, pixels, numBytes, is800KHz);
#elif defined(__ARDUINO_ARC__)
// Arduino 101  -----------------------------------------------------------
 
#define NOPx7					    \
	{ __builtin_arc_nop();			    \
	  __builtin_arc_nop(); __builtin_arc_nop(); \
	  __builtin_arc_nop(); __builtin_arc_nop(); \
	  __builtin_arc_nop(); __builtin_arc_nop(); }
 
	PinDescription *pindesc = &g_APinDescription[pin];
	register uint32_t loop = 8 * numBytes; // one loop to handle all bytes and all bits
	register uint8_t *p = pixels;
	register uint32_t currByte = (uint32_t)(*p);
	register uint32_t currBit = 0x80 & currByte;
	register uint32_t bitCounter = 0;
	register uint32_t first = 1;
 
	// The loop is unusual. Very first iteration puts all the way LOW to the wire -
	// constant LOW does not affect NEOPIXEL, so there is no visible effect displayed.
	// During that very first iteration CPU caches instructions in the loop.
	// Because of the caching process, "CPU slows down". NEOPIXEL pulse is very time sensitive
	// that's why we let the CPU cache first and we start regular pulse from 2nd iteration
	if (pindesc->ulGPIOType == SS_GPIO) {
		register uint32_t reg = pindesc->ulGPIOBase + SS_GPIO_SWPORTA_DR;
		uint32_t reg_val = __builtin_arc_lr((volatile uint32_t)reg);
		register uint32_t reg_bit_high = reg_val | (1 << pindesc->ulGPIOId);
		register uint32_t reg_bit_low = reg_val & ~(1 << pindesc->ulGPIOId);
 
		loop += 1; // include first, special iteration
		while (loop--)
		{
			if (!first) {
				currByte <<= 1;
				bitCounter++;
			}
 
			// 1 is >550ns high and >450ns low; 0 is 200..500ns high and >450ns low
			__builtin_arc_sr(first ? reg_bit_low : reg_bit_high, (volatile uint32_t)reg);
			if (currBit) { // ~400ns HIGH (740ns overall)
				NOPx7
				    NOPx7
			}
			// ~340ns HIGH
			NOPx7
				    __builtin_arc_nop();
 
			// 820ns LOW; per spec, max allowed low here is 5000ns */
			__builtin_arc_sr(reg_bit_low, (volatile uint32_t)reg);
			NOPx7
			    NOPx7
 
			if (bitCounter >= 8) {
				bitCounter = 0;
				currByte = (uint32_t)(*++p);
			}
 
			currBit = 0x80 & currByte;
			first = 0;
		}
	} else if (pindesc->ulGPIOType == SOC_GPIO) {
		register uint32_t reg = pindesc->ulGPIOBase + SOC_GPIO_SWPORTA_DR;
		uint32_t reg_val = MMIO_REG_VAL(reg);
		register uint32_t reg_bit_high = reg_val | (1 << pindesc->ulGPIOId);
		register uint32_t reg_bit_low = reg_val & ~(1 << pindesc->ulGPIOId);
 
		loop += 1; // include first, special iteration
		while (loop--)
		{
			if (!first) {
				currByte <<= 1;
				bitCounter++;
			}
			MMIO_REG_VAL(reg) = first ? reg_bit_low : reg_bit_high;
			if (currBit) { // ~430ns HIGH (740ns overall)
				NOPx7
				NOPx7
				    __builtin_arc_nop();
			}
			// ~310ns HIGH
			NOPx7
 
			// 850ns LOW; per spec, max allowed low here is 5000ns */
			    MMIO_REG_VAL(reg) = reg_bit_low;
			NOPx7
			    NOPx7
 
			if (bitCounter >= 8) {
				bitCounter = 0;
				currByte = (uint32_t)(*++p);
			}
 
			currBit = 0x80 & currByte;
			first = 0;
		}
	}
#endif
 
 
// END ARCHITECTURE SELECT ------------------------------------------------
 
 
	interrupts();
	endTime = micros(); // Save EOD time for latch on next call
}
 
// Set the output pin number
void Adafruit_NeoPixel::setPin(uint8_t p)
{
	if (begun && (pin >= 0)) {
		pinMode(pin, INPUT);
	}
	pin = p;
	if (begun) {
		pinMode(p, OUTPUT);
		digitalWrite(p, LOW);
	}
#ifdef __AVR__
	port = portOutputRegister(digitalPinToPort(p));
	pinMask = digitalPinToBitMask(p);
#endif
}
 
 
// Set pixel color from separate R,G,B components:
void Adafruit_NeoPixel::setPixelColor(
	uint16_t n, uint8_t r, uint8_t g, uint8_t b)
{
	if (n < numLEDs) {
		if (brightness) { // See notes in setBrightness()
			r = (r * brightness) >> 8;
			g = (g * brightness) >> 8;
			b = (b * brightness) >> 8;
		}
		uint8_t *p;
		if (wOffset == rOffset) {       // Is an RGB-type strip
			p = &pixels[n * 3];     // 3 bytes per pixel
		} else {                        // Is a WRGB-type strip
			p = &pixels[n * 4];     // 4 bytes per pixel
			p[wOffset] = 0;         // But only R,G,B passed -- set W to 0
		}
		p[rOffset] = r;                 // R,G,B always stored
		p[gOffset] = g;
		p[bOffset] = b;
	}
}
 
 
void Adafruit_NeoPixel::setPixelColor(
	uint16_t n, uint8_t r, uint8_t g, uint8_t b, uint8_t w)
{
	if (n < numLEDs) {
		if (brightness) { // See notes in setBrightness()
			r = (r * brightness) >> 8;
			g = (g * brightness) >> 8;
			b = (b * brightness) >> 8;
			w = (w * brightness) >> 8;
		}
		uint8_t *p;
		if (wOffset == rOffset) {       // Is an RGB-type strip
			p = &pixels[n * 3];     // 3 bytes per pixel (ignore W)
		} else {                        // Is a WRGB-type strip
			p = &pixels[n * 4];     // 4 bytes per pixel
			p[wOffset] = w;         // Store W
		}
		p[rOffset] = r;                 // Store R,G,B
		p[gOffset] = g;
		p[bOffset] = b;
	}
}
 
 
// Set pixel color from 'packed' 32-bit RGB color:
void Adafruit_NeoPixel::setPixelColor(uint16_t n, uint32_t c)
{
	if (n < numLEDs) {
		uint8_t *p,
		    r = (uint8_t)(c >> 16),
		    g = (uint8_t)(c >>  8),
		    b = (uint8_t)c;
		if (brightness) { // See notes in setBrightness()
			r = (r * brightness) >> 8;
			g = (g * brightness) >> 8;
			b = (b * brightness) >> 8;
		}
		if (wOffset == rOffset) {
			p = &pixels[n * 3];
		} else {
			p = &pixels[n * 4];
			uint8_t w = (uint8_t)(c >> 24);
			p[wOffset] = brightness ? ((w * brightness) >> 8) : w;
		}
		p[rOffset] = r;
		p[gOffset] = g;
		p[bOffset] = b;
	}
}
 
 
// Convert separate R,G,B into packed 32-bit RGB color.
// Packed format is always RGB, regardless of LED strand color order.
uint32_t Adafruit_NeoPixel::Color(uint8_t r, uint8_t g, uint8_t b)
{
	return (((uint32_t)r << 16) | ((uint32_t)g <<  8) | b);
}
 
 
// Convert separate R,G,B,W into packed 32-bit WRGB color.
// Packed format is always WRGB, regardless of LED strand color order.
uint32_t Adafruit_NeoPixel::Color(uint8_t r, uint8_t g, uint8_t b, uint8_t w)
{
	return (((uint32_t)w << 24) | ((uint32_t)r << 16) | ((uint32_t)g <<  8) | b);
}
 
 
// Query color from previously-set pixel (returns packed 32-bit RGB value)
uint32_t Adafruit_NeoPixel::getPixelColor(uint16_t n) const
{
	if (n >= numLEDs) {
		return (0);  // Out of bounds, return no color.
	}
	uint8_t *p;
 
	if (wOffset == rOffset) { // Is RGB-type device
		p = &pixels[n * 3];
		if (brightness) {
			// Stored color was decimated by setBrightness().  Returned value
			// attempts to scale back to an approximation of the original 24-bit
			// value used when setting the pixel color, but there will always be
			// some error -- those bits are simply gone.  Issue is most
			// pronounced at low brightness levels.
			return ((((uint32_t)(p[rOffset] << 8) / brightness) << 16) |
			       (((uint32_t)(p[gOffset] << 8) / brightness) <<  8) |
			       ((uint32_t)(p[bOffset] << 8) / brightness));
		} else {
			// No brightness adjustment has been made -- return 'raw' color
			return (((uint32_t)p[rOffset] << 16) |
			       ((uint32_t)p[gOffset] <<  8) |
			       (uint32_t)p[bOffset]);
		}
	} else {                        // Is RGBW-type device
		p = &pixels[n * 4];
		if (brightness) {       // Return scaled color
			return ((((uint32_t)(p[wOffset] << 8) / brightness) << 24) |
			       (((uint32_t)(p[rOffset] << 8) / brightness) << 16) |
			       (((uint32_t)(p[gOffset] << 8) / brightness) <<  8) |
			       ((uint32_t)(p[bOffset] << 8) / brightness));
		} else { // Return raw color
			return (((uint32_t)p[wOffset] << 24) |
			       ((uint32_t)p[rOffset] << 16) |
			       ((uint32_t)p[gOffset] <<  8) |
			       (uint32_t)p[bOffset]);
		}
	}
}
 
 
// Returns pointer to pixels[] array.  Pixel data is stored in device-
// native format and is not translated here.  Application will need to be
// aware of specific pixel data format and handle colors appropriately.
uint8_t *Adafruit_NeoPixel::getPixels(void) const
{
	return (pixels);
}
 
 
uint16_t Adafruit_NeoPixel::numPixels(void) const
{
	return (numLEDs);
}
 
 
// Adjust output brightness; 0=darkest (off), 255=brightest.  This does
// NOT immediately affect what's currently displayed on the LEDs.  The
// next call to show() will refresh the LEDs at this level.  However,
// this process is potentially "lossy," especially when increasing
// brightness.  The tight timing in the WS2811/WS2812 code means there
// aren't enough free cycles to perform this scaling on the fly as data
// is issued.  So we make a pass through the existing color data in RAM
// and scale it (subsequent graphics commands also work at this
// brightness level).  If there's a significant step up in brightness,
// the limited number of steps (quantization) in the old data will be
// quite visible in the re-scaled version.  For a non-destructive
// change, you'll need to re-render the full strip data.  C'est la vie.
void Adafruit_NeoPixel::setBrightness(uint8_t b)
{
	// Stored brightness value is different than what's passed.
	// This simplifies the actual scaling math later, allowing a fast
	// 8x8-bit multiply and taking the MSB.  'brightness' is a uint8_t,
	// adding 1 here may (intentionally) roll over...so 0 = max brightness
	// (color values are interpreted literally; no scaling), 1 = min
	// brightness (off), 255 = just below max brightness.
	uint8_t newBrightness = b + 1;
 
	if (newBrightness != brightness) { // Compare against prior value
		// Brightness has changed -- re-scale existing data in RAM
		uint8_t c,
		    *ptr = pixels,
		    oldBrightness = brightness - 1; // De-wrap old brightness value
		uint16_t scale;
		if (oldBrightness == 0) {
			scale = 0;    // Avoid /0
		}else if (b == 255) {
			scale = 65535 / oldBrightness;
		} else {
			scale = (((uint16_t)newBrightness << 8) - 1) / oldBrightness;
		}
		for (uint16_t i = 0; i < numBytes; i++)
		{
			c = *ptr;
			*ptr++ = (c * scale) >> 8;
		}
		brightness = newBrightness;
	}
}
 
 
//Return the brightness value
uint8_t Adafruit_NeoPixel::getBrightness(void) const
{
	return (brightness - 1);
}
 
 
void Adafruit_NeoPixel::clear()
{
	memset(pixels, 0, numBytes);
}

Adafruit_NeoPixel.h :

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

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
/*--------------------------------------------------------------------
 *  This file is part of the Adafruit NeoPixel library.
 *
 *  NeoPixel is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU Lesser General Public License as
 *  published by the Free Software Foundation, either version 3 of
 *  the License, or (at your option) any later version.
 *
 *  NeoPixel is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with NeoPixel.  If not, see
 *  <http://www.gnu.org/licenses/>.
 *  --------------------------------------------------------------------*/
 
#ifndef ADAFRUIT_NEOPIXEL_H
#define ADAFRUIT_NEOPIXEL_H
 
#if (ARDUINO >= 100)
#include <Arduino.h>
#else
#include <WProgram.h>
#include <pins_arduino.h>
#endif
 
// The order of primary colors in the NeoPixel data stream can vary
// among device types, manufacturers and even different revisions of
// the same item.  The third parameter to the Adafruit_NeoPixel
// constructor encodes the per-pixel byte offsets of the red, green
// and blue primaries (plus white, if present) in the data stream --
// the following #defines provide an easier-to-use named version for
// each permutation.  e.g. NEO_GRB indicates a NeoPixel-compatible
// device expecting three bytes per pixel, with the first byte
// containing the green value, second containing red and third
// containing blue.  The in-memory representation of a chain of
// NeoPixels is the same as the data-stream order; no re-ordering of
// bytes is required when issuing data to the chain.
 
// Bits 5,4 of this value are the offset (0-3) from the first byte of
// a pixel to the location of the red color byte.  Bits 3,2 are the
// green offset and 1,0 are the blue offset.  If it is an RGBW-type
// device (supporting a white primary in addition to R,G,B), bits 7,6
// are the offset to the white byte...otherwise, bits 7,6 are set to
// the same value as 5,4 (red) to indicate an RGB (not RGBW) device.
// i.e. binary representation:
// 0bWWRRGGBB for RGBW devices
// 0bRRRRGGBB for RGB
 
// RGB NeoPixel permutations; white and red offsets are always same
// Offset:         W          R          G          B
#define NEO_RGB		((0 << 6) | (0 << 4) | (1 << 2) | (2))
#define NEO_RBG		((0 << 6) | (0 << 4) | (2 << 2) | (1))
#define NEO_GRB		((1 << 6) | (1 << 4) | (0 << 2) | (2))
#define NEO_GBR		((2 << 6) | (2 << 4) | (0 << 2) | (1))
#define NEO_BRG		((1 << 6) | (1 << 4) | (2 << 2) | (0))
#define NEO_BGR		((2 << 6) | (2 << 4) | (1 << 2) | (0))
 
// RGBW NeoPixel permutations; all 4 offsets are distinct
// Offset:         W          R          G          B
#define NEO_WRGB	((0 << 6) | (1 << 4) | (2 << 2) | (3))
#define NEO_WRBG	((0 << 6) | (1 << 4) | (3 << 2) | (2))
#define NEO_WGRB	((0 << 6) | (2 << 4) | (1 << 2) | (3))
#define NEO_WGBR	((0 << 6) | (3 << 4) | (1 << 2) | (2))
#define NEO_WBRG	((0 << 6) | (2 << 4) | (3 << 2) | (1))
#define NEO_WBGR	((0 << 6) | (3 << 4) | (2 << 2) | (1))
 
#define NEO_RWGB	((1 << 6) | (0 << 4) | (2 << 2) | (3))
#define NEO_RWBG	((1 << 6) | (0 << 4) | (3 << 2) | (2))
#define NEO_RGWB	((2 << 6) | (0 << 4) | (1 << 2) | (3))
#define NEO_RGBW	((3 << 6) | (0 << 4) | (1 << 2) | (2))
#define NEO_RBWG	((2 << 6) | (0 << 4) | (3 << 2) | (1))
#define NEO_RBGW	((3 << 6) | (0 << 4) | (2 << 2) | (1))
 
#define NEO_GWRB	((1 << 6) | (2 << 4) | (0 << 2) | (3))
#define NEO_GWBR	((1 << 6) | (3 << 4) | (0 << 2) | (2))
#define NEO_GRWB	((2 << 6) | (1 << 4) | (0 << 2) | (3))
#define NEO_GRBW	((3 << 6) | (1 << 4) | (0 << 2) | (2))
#define NEO_GBWR	((2 << 6) | (3 << 4) | (0 << 2) | (1))
#define NEO_GBRW	((3 << 6) | (2 << 4) | (0 << 2) | (1))
 
#define NEO_BWRG	((1 << 6) | (2 << 4) | (3 << 2) | (0))
#define NEO_BWGR	((1 << 6) | (3 << 4) | (2 << 2) | (0))
#define NEO_BRWG	((2 << 6) | (1 << 4) | (3 << 2) | (0))
#define NEO_BRGW	((3 << 6) | (1 << 4) | (2 << 2) | (0))
#define NEO_BGWR	((2 << 6) | (3 << 4) | (1 << 2) | (0))
#define NEO_BGRW	((3 << 6) | (2 << 4) | (1 << 2) | (0))
 
// Add NEO_KHZ400 to the color order value to indicate a 400 KHz
// device.  All but the earliest v1 NeoPixels expect an 800 KHz data
// stream, this is the default if unspecified.  Because flash space
// is very limited on ATtiny devices (e.g. Trinket, Gemma), v1
// NeoPixels aren't handled by default on those chips, though it can
// be enabled by removing the ifndef/endif below -- but code will be
// bigger.  Conversely, can disable the NEO_KHZ400 line on other MCUs
// to remove v1 support and save a little space.
 
#define NEO_KHZ800	0x0000  // 800 KHz datastream
#ifndef __AVR_ATtiny85__
#define NEO_KHZ400	0x0100  // 400 KHz datastream
#endif
 
// If 400 KHz support is enabled, the third parameter to the constructor
// requires a 16-bit value (in order to select 400 vs 800 KHz speed).
// If only 800 KHz is enabled (as is default on ATtiny), an 8-bit value
// is sufficient to encode pixel color order, saving some space.
 
#ifdef NEO_KHZ400
typedef uint16_t	neoPixelType;
#else
typedef uint8_t		neoPixelType;
#endif
 
class Adafruit_NeoPixel {
public:
 
	// Constructor: number of LEDs, pin number, LED type
	Adafruit_NeoPixel(uint16_t n, uint8_t p = 6, neoPixelType t = NEO_GRB + NEO_KHZ800);
	Adafruit_NeoPixel(void);
	~Adafruit_NeoPixel();
 
	void
	begin(void),
	show(void),
	setPin(uint8_t p),
	setPixelColor(uint16_t n, uint8_t r, uint8_t g, uint8_t b),
	setPixelColor(uint16_t n, uint8_t r, uint8_t g, uint8_t b, uint8_t w),
	setPixelColor(uint16_t n, uint32_t c),
	setBrightness(uint8_t),
	clear(),
	updateLength(uint16_t n),
	updateType(neoPixelType t);
	uint8_t
	*getPixels(void) const,
	getBrightness(void) const;
	uint16_t
	numPixels(void) const;
	static uint32_t
	Color(uint8_t r, uint8_t g, uint8_t b),
	Color(uint8_t r, uint8_t g, uint8_t b, uint8_t w);
	uint32_t
	getPixelColor(uint16_t n) const;
 
	inline bool
	canShow(void)
	{
		return ((micros() - endTime) >= 50L);
	}
 
 
private:
 
	boolean
#ifdef NEO_KHZ400       // If 400 KHz NeoPixel support enabled...
	is800KHz,       // ...true if 800 KHz pixels
#endif
	begun;          // true if begin() previously called
	uint16_t
	    numLEDs,    // Number of RGB LEDs in strip
	    numBytes;   // Size of 'pixels' buffer below (3 or 4 bytes/pixel)
	int8_t
	    pin;        // Output pin number (-1 if not yet set)
	uint8_t
	    brightness,
	    *pixels,    // Holds LED color values (3 or 4 bytes each)
	    rOffset,    // Index of red byte within each 3- or 4-byte pixel
	    gOffset,    // Index of green byte
	    bOffset,    // Index of blue byte
	    wOffset;    // Index of white byte (same as rOffset if no white)
	uint32_t
	    endTime;    // Latch timing reference
#ifdef __AVR__
	volatile uint8_t
	*port;          // Output PORT register
	uint8_t
	    pinMask;    // Output PORT bitmask
#endif
};
 
#endif // ADAFRUIT_NEOPIXEL_H

BalanceCar.h :

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

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
/*
 * @Description: In User Settings Edit
 * @Author: your name
 * @Date: 2019-10-08 09:35:07
 * @LastEditTime: 2019-10-11 16:25:04
 * @LastEditors: Please set LastEditors
 */
#include "MsTimer2.h"
#include "KalmanFilter.h"
#include "I2Cdev.h"
//#include "MPU6050_6Axis_MotionApps20.h"
 
#include "MPU6050.h"
#include "Wire.h"
MPU6050 mpu;
KalmanFilter kalmanfilter;
 
//Setting PID parameters
 
double kp_balance = 55, kd_balance = 0.75;
double kp_speed = 10, ki_speed = 0.26;
double kp_turn = 2.5, kd_turn = 0.5;
 
//Setting MPU6050 calibration parameters
double angle_zero = 0;            //x axle angle calibration
double angular_velocity_zero = 0; //x axle angular velocity calibration
 
volatile unsigned long encoder_count_right_a = 0;
volatile unsigned long encoder_count_left_a = 0;
int16_t ax, ay, az, gx, gy, gz;
float dt = 0.005, Q_angle = 0.001, Q_gyro = 0.005, R_angle = 0.5, C_0 = 1, K1 = 0.05;
 
int encoder_left_pulse_num_speed = 0;
int encoder_right_pulse_num_speed = 0;
double speed_control_output = 0;
double rotation_control_output = 0;
double speed_filter = 0;
int speed_control_period_count = 0;
double car_speed_integeral = 0;
double speed_filter_old = 0;
int setting_car_speed = 0;
int setting_turn_speed = 0;
double pwm_left = 0;
double pwm_right = 0;
float kalmanfilter_angle;
// char balance_angle_min = -27;
// char balance_angle_max = 27;
char balance_angle_min = -22;
char balance_angle_max = 22;
 
void carStop()
{
  digitalWrite(AIN1, HIGH);
  digitalWrite(BIN1, LOW);
  digitalWrite(STBY_PIN, HIGH);
  analogWrite(PWMA_LEFT, 0);
  analogWrite(PWMB_RIGHT, 0);
}
 
void carForward(unsigned char speed)
{
  digitalWrite(AIN1, 0);
  digitalWrite(BIN1, 0);
  analogWrite(PWMA_LEFT, speed);
  analogWrite(PWMB_RIGHT, speed);
}
 
void carBack(unsigned char speed)
{
  digitalWrite(AIN1, 1);
  digitalWrite(BIN1, 1);
  analogWrite(PWMA_LEFT, speed);
  analogWrite(PWMB_RIGHT, speed);
}
 
void balanceCar()
{
  sei();
  encoder_left_pulse_num_speed += pwm_left < 0 ? -encoder_count_left_a : encoder_count_left_a;
  encoder_right_pulse_num_speed += pwm_right < 0 ? -encoder_count_right_a : encoder_count_right_a;
  encoder_count_left_a = 0;
  encoder_count_right_a = 0;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  kalmanfilter.Angle(ax, ay, az, gx, gy, gz, dt, Q_angle, Q_gyro, R_angle, C_0, K1);
  kalmanfilter_angle = kalmanfilter.angle;
  double balance_control_output = kp_balance * (kalmanfilter_angle - angle_zero) + kd_balance * (kalmanfilter.Gyro_x - angular_velocity_zero);
 
  speed_control_period_count++;
  if (speed_control_period_count >= 8)
  {
    speed_control_period_count = 0;
    double car_speed = (encoder_left_pulse_num_speed + encoder_right_pulse_num_speed) * 0.5;
    encoder_left_pulse_num_speed = 0;
    encoder_right_pulse_num_speed = 0;
    speed_filter = speed_filter_old * 0.7 + car_speed * 0.3;
    speed_filter_old = speed_filter;
    car_speed_integeral += speed_filter;
    car_speed_integeral += -setting_car_speed;
    car_speed_integeral = constrain(car_speed_integeral, -3000, 3000);
    speed_control_output = -kp_speed * speed_filter - ki_speed * car_speed_integeral;
    rotation_control_output = setting_turn_speed + kd_turn * kalmanfilter.Gyro_z;
  }
 
  pwm_left = balance_control_output - speed_control_output - rotation_control_output;
  pwm_right = balance_control_output - speed_control_output + rotation_control_output;
 
  pwm_left = constrain(pwm_left, -255, 255);
  pwm_right = constrain(pwm_right, -255, 255);
  if (motion_mode != START && motion_mode != STOP && (kalmanfilter_angle < balance_angle_min || balance_angle_max < kalmanfilter_angle))
  {
    motion_mode = STOP;
    carStop();
  }
 
  if (motion_mode == STOP && key_flag != '4')
  {
    car_speed_integeral = 0;
    setting_car_speed = 0;
    pwm_left = 0;
    pwm_right = 0;
    carStop();
  }
  else if (motion_mode == STOP)
  {
    car_speed_integeral = 0;
    setting_car_speed = 0;
    pwm_left = 0;
    pwm_right = 0;
  }
  else
  {
    if (pwm_left < 0)
    {
      digitalWrite(AIN1, 1);
      analogWrite(PWMA_LEFT, -pwm_left);
    }
    else
    {
      digitalWrite(AIN1, 0);
      analogWrite(PWMA_LEFT, pwm_left);
    }
    if (pwm_right < 0)
    {
      digitalWrite(BIN1, 1);
      analogWrite(PWMB_RIGHT, -pwm_right);
    }
    else
    {
      digitalWrite(BIN1, 0);
      analogWrite(PWMB_RIGHT, pwm_right);
    }
  }
}
 
void encoderCountRightA()
{
  encoder_count_right_a++;
}
 
void encoderCountLeftA()
{
  encoder_count_left_a++;
}
 
void carInitialize()
{
  pinMode(AIN1, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(PWMA_LEFT, OUTPUT);
  pinMode(PWMB_RIGHT, OUTPUT);
  pinMode(STBY_PIN, OUTPUT);
  carStop();
  Wire.begin();
  mpu.initialize();
  attachInterrupt(digitalPinToInterrupt(ENCODER_LEFT_A_PIN), encoderCountLeftA, CHANGE);
  attachPinChangeInterrupt(ENCODER_RIGHT_A_PIN, encoderCountRightA, CHANGE);
  MsTimer2::set(5, balanceCar);
  MsTimer2::start();
}

Command.h :

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

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
char key_value = '\0';
char key_flag = '\0';
char key_mode = 0;
char prev_key_mode = 0;
unsigned long key_mode_time = 0;
 
void keyValue()
{
  if (millis() - key_mode_time > 500)
  {
    key_mode_time = millis();
 
    key_mode++;
 
    if (key_mode >= 5)
    {
      key_mode = 0;
    }
  }
}
 
bool getKeyValue()
{
  if (prev_key_mode != key_mode)
  {
    prev_key_mode = key_mode;
    switch (key_mode)
    {
    case 0:
      key_value = 's';
      function_mode = IDLE;
      break;
    case 1:
      key_value = '1';
      break;
    case 2:
      key_value = '2';
      break;
    case 3:
      key_value = '0';
      break;
    case 4:
      key_value = '3';
      break;
    default:
      break;
    }
    return true;
  }
  return false;
}
 
void keyInit()
{
  pinMode(KEY_MODE, INPUT_PULLUP);
  attachPinChangeInterrupt(KEY_MODE, keyValue, FALLING);
}
 
bool getBluetoothData()
{
  if (Serial.available())
  {
    char c = Serial.read();
    if (c != '\0' && c != '\n')
    {
      key_value = c;
      if (c == 'f' || c == 'b' || c == 'l' || c == 'i')
      {
        function_mode = BLUETOOTH;
      }
      if (c == 's')
      {
        function_mode = IDLE;
      }
      if (key_value == 'f' || key_value == 'b' || key_value == 'l' || key_value == 'i' || key_value == 's' ||
          key_value == '0' || key_value == '1' || key_value == '2' || key_value == '3' || key_value == '4' ||
          key_value == '5' || key_value == '6' || key_value == '7' || key_value == '8' || key_value == '9' ||
          key_value == '*' || key_value == '#')
      {
        return true;
      }
    }
  }
  return false;
}

Invité · 02/03/2023, 20h01

I2Cdev.cpp :

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

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
1001
1002
1003
1004
1005
1006
1007
1008
1009
1010
1011
1012
1013
1014
1015
1016
1017
1018
1019
1020
1021
1022
1023
1024
1025
1026
1027
1028
1029
1030
1031
1032
1033
1034
1035
1036
1037
1038
1039
1040
1041
1042
1043
1044
1045
1046
1047
1048
1049
1050
1051
1052
1053
1054
1055
1056
1057
1058
1059
1060
1061
1062
1063
1064
1065
1066
1067
1068
1069
1070
1071
1072
1073
1074
1075
1076
1077
1078
1079
1080
1081
1082
1083
1084
1085
1086
1087
1088
1089
1090
1091
1092
1093
1094
1095
1096
1097
1098
1099
1100
1101
1102
1103
1104
1105
1106
1107
1108
1109
1110
1111
1112
1113
1114
1115
1116
1117
1118
1119
1120
1121
1122
1123
1124
1125
1126
1127
1128
1129
1130
1131
1132
1133
1134
1135
1136
1137
1138
1139
1140
1141
1142
1143
1144
1145
1146
1147
1148
1149
1150
1151
1152
1153
1154
1155
1156
1157
1158
1159
1160
1161
1162
1163
1164
1165
1166
1167
1168
1169
1170
1171
1172
1173
1174
1175
1176
1177
1178
1179
1180
1181
1182
1183
1184
1185
1186
1187
1188
1189
1190
1191
1192
1193
1194
1195
1196
1197
1198
1199
1200
1201
1202
1203
1204
1205
1206
1207
1208
1209
1210
1211
1212
1213
1214
1215
1216
1217
1218
1219
1220
1221
1222
1223
1224
1225
1226
1227
1228
1229
1230
1231
1232
1233
1234
1235
1236
1237
1238
1239
1240
1241
1242
1243
1244
1245
1246
1247
1248
1249
1250
1251
1252
1253
1254
1255
1256
1257
1258
1259
1260
1261
1262
1263
1264
1265
1266
1267
1268
1269
1270
1271
1272
1273
1274
1275
1276
1277
1278
1279
1280
1281
1282
1283
1284
1285
1286
1287
1288
1289
1290
1291
1292
1293
1294
1295
1296
1297
1298
1299
1300
1301
1302
1303
1304
1305
1306
1307
1308
1309
1310
1311
1312
1313
1314
1315
1316
1317
1318
1319
1320
1321
1322
1323
1324
1325
1326
1327
1328
1329
1330
1331
1332
1333
1334
1335
1336
1337
1338
1339
1340
1341
1342
1343
1344
1345
1346
1347
1348
1349
1350
1351
1352
1353
1354
1355
1356
1357
1358
1359
1360
1361
1362
1363
1364
1365
1366
1367
1368
1369
1370
1371
1372
1373
1374
1375
1376
1377
1378
1379
1380
1381
1382
1383
1384
1385
1386
1387
1388
1389
1390
1391
1392
1393
1394
1395
1396
1397
1398
1399
1400
1401
1402
1403
1404
1405
1406
1407
1408
1409
1410
1411
1412
1413
1414
1415
1416
1417
1418
1419
1420
1421
1422
1423
1424
1425
1426
1427
1428
1429
1430
1431
1432
1433
1434
1435
1436
1437
1438
1439
1440
1441
1442
1443
1444
1445
1446
1447
1448
1449
1450
1451
1452
1453
1454
1455
1456
1457
// I2Cdev library collection - Main I2C device class
// Abstracts bit and byte I2C R/W functions into a convenient class
// 6/9/2012 by Jeff Rowberg <jeff@rowberg.net>
//
// Changelog:
//      2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications
//      2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan)
//      2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire
//                 - add compiler warnings when using outdated or IDE or limited I2Cdev implementation
//      2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums)
//      2011-10-03 - added automatic Arduino version detection for ease of use
//      2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications
//      2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x)
//      2011-08-03 - added optional timeout parameter to read* methods to easily change from default
//      2011-08-02 - added support for 16-bit registers
//                 - fixed incorrect Doxygen comments on some methods
//                 - added timeout value for read operations (thanks mem @ Arduino forums)
//      2011-07-30 - changed read/write function structures to return success or byte counts
//                 - made all methods static for multi-device memory savings
//      2011-07-28 - initial release
 
/* ============================================
I2Cdev device library code is placed under the MIT license
Copyright (c) 2013 Jeff Rowberg
 
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
 
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
 
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
===============================================
*/
 
#include "./I2Cdev.h"
 
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
 
    #ifdef I2CDEV_IMPLEMENTATION_WARNINGS
        #if ARDUINO < 100
            #warning Using outdated Arduino IDE with Wire library is functionally limiting.
            #warning Arduino IDE v1.0.1+ with I2Cdev Fastwire implementation is recommended.
            #warning This I2Cdev implementation does not support:
            #warning - Repeated starts conditions
            #warning - Timeout detection (some Wire requests block forever)
        #elif ARDUINO == 100
            #warning Using outdated Arduino IDE with Wire library is functionally limiting.
            #warning Arduino IDE v1.0.1+ with I2Cdev Fastwire implementation is recommended.
            #warning This I2Cdev implementation does not support:
            #warning - Repeated starts conditions
            #warning - Timeout detection (some Wire requests block forever)
        #elif ARDUINO > 100
            #warning Using current Arduino IDE with Wire library is functionally limiting.
            #warning Arduino IDE v1.0.1+ with I2CDEV_BUILTIN_FASTWIRE implementation is recommended.
            #warning This I2Cdev implementation does not support:
            #warning - Timeout detection (some Wire requests block forever)
        #endif
    #endif
 
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
 
    //#error The I2CDEV_BUILTIN_FASTWIRE implementation is known to be broken right now. Patience, Iago!
 
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE
 
    #ifdef I2CDEV_IMPLEMENTATION_WARNINGS
        #warning Using I2CDEV_BUILTIN_NBWIRE implementation may adversely affect interrupt detection.
        #warning This I2Cdev implementation does not support:
        #warning - Repeated starts conditions
    #endif
 
    // NBWire implementation based heavily on code by Gene Knight <Gene@Telobot.com>
    // Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html
    // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html
    TwoWire Wire;
 
#endif
 
/** Default constructor.
 */
I2Cdev::I2Cdev() {
}
 
/** Read a single bit from an 8-bit device register.
 * @param devAddr I2C slave device address
 * @param regAddr Register regAddr to read from
 * @param bitNum Bit position to read (0-7)
 * @param data Container for single bit value
 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
 * @return Status of read operation (true = success)
 */
int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout) {
    uint8_t b;
    uint8_t count = readByte(devAddr, regAddr, &b, timeout);
    *data = b & (1 << bitNum);
    return count;
}
 
/** Read a single bit from a 16-bit device register.
 * @param devAddr I2C slave device address
 * @param regAddr Register regAddr to read from
 * @param bitNum Bit position to read (0-15)
 * @param data Container for single bit value
 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
 * @return Status of read operation (true = success)
 */
int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout) {
    uint16_t b;
    uint8_t count = readWord(devAddr, regAddr, &b, timeout);
    *data = b & (1 << bitNum);
    return count;
}
 
/** Read multiple bits from an 8-bit device register.
 * @param devAddr I2C slave device address
 * @param regAddr Register regAddr to read from
 * @param bitStart First bit position to read (0-7)
 * @param length Number of bits to read (not more than 8)
 * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
 * @return Status of read operation (true = success)
 */
int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout) {
    // 01101001 read byte
    // 76543210 bit numbers
    //    xxx   args: bitStart=4, length=3
    //    010   masked
    //   -> 010 shifted
    uint8_t count, b;
    if ((count = readByte(devAddr, regAddr, &b, timeout)) != 0) {
        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
        b &= mask;
        b >>= (bitStart - length + 1);
        *data = b;
    }
    return count;
}
 
/** Read multiple bits from a 16-bit device register.
 * @param devAddr I2C slave device address
 * @param regAddr Register regAddr to read from
 * @param bitStart First bit position to read (0-15)
 * @param length Number of bits to read (not more than 16)
 * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
 * @return Status of read operation (1 = success, 0 = failure, -1 = timeout)
 */
int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout) {
    // 1101011001101001 read byte
    // fedcba9876543210 bit numbers
    //    xxx           args: bitStart=12, length=3
    //    010           masked
    //           -> 010 shifted
    uint8_t count;
    uint16_t w;
    if ((count = readWord(devAddr, regAddr, &w, timeout)) != 0) {
        uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1);
        w &= mask;
        w >>= (bitStart - length + 1);
        *data = w;
    }
    return count;
}
 
/** Read single byte from an 8-bit device register.
 * @param devAddr I2C slave device address
 * @param regAddr Register regAddr to read from
 * @param data Container for byte value read from device
 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
 * @return Status of read operation (true = success)
 */
int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout) {
    return readBytes(devAddr, regAddr, 1, data, timeout);
}
 
/** Read single word from a 16-bit device register.
 * @param devAddr I2C slave device address
 * @param regAddr Register regAddr to read from
 * @param data Container for word value read from device
 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
 * @return Status of read operation (true = success)
 */
int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout) {
    return readWords(devAddr, regAddr, 1, data, timeout);
}
 
/** Read multiple bytes from an 8-bit device register.
 * @param devAddr I2C slave device address
 * @param regAddr First register regAddr to read from
 * @param length Number of bytes to read
 * @param data Buffer to store read data in
 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
 * @return Number of bytes read (-1 indicates failure)
 */
int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout) {
    #ifdef I2CDEV_SERIAL_DEBUG
        Serial.print("I2C (0x");
        Serial.print(devAddr, HEX);
        Serial.print(") reading ");
        Serial.print(length, DEC);
        Serial.print(" bytes from 0x");
        Serial.print(regAddr, HEX);
        Serial.print("...");
    #endif
 
    int8_t count = 0;
    uint32_t t1 = millis();
 
    #if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE)
 
        #if (ARDUINO < 100)
            // Arduino v00xx (before v1.0), Wire library
 
            // I2C/TWI subsystem uses internal buffer that breaks with large data requests
            // so if user requests more than BUFFER_LENGTH bytes, we have to do it in
            // smaller chunks instead of all at once
            for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) {
                Wire.beginTransmission(devAddr);
                Wire.send(regAddr);
                Wire.endTransmission();
                Wire.beginTransmission(devAddr);
                Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH));
 
                for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) {
                    data[count] = Wire.receive();
                    #ifdef I2CDEV_SERIAL_DEBUG
                        Serial.print(data[count], HEX);
                        if (count + 1 < length) Serial.print(" ");
                    #endif
                }
 
                Wire.endTransmission();
            }
        #elif (ARDUINO == 100)
            // Arduino v1.0.0, Wire library
            // Adds standardized write() and read() stream methods instead of send() and receive()
 
            // I2C/TWI subsystem uses internal buffer that breaks with large data requests
            // so if user requests more than BUFFER_LENGTH bytes, we have to do it in
            // smaller chunks instead of all at once
            for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) {
                Wire.beginTransmission(devAddr);
                Wire.write(regAddr);
                Wire.endTransmission();
                Wire.beginTransmission(devAddr);
                Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH));
 
                for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) {
                    data[count] = Wire.read();
                    #ifdef I2CDEV_SERIAL_DEBUG
                        Serial.print(data[count], HEX);
                        if (count + 1 < length) Serial.print(" ");
                    #endif
                }
 
                Wire.endTransmission();
            }
        #elif (ARDUINO > 100)
            // Arduino v1.0.1+, Wire library
            // Adds official support for repeated start condition, yay!
 
            // I2C/TWI subsystem uses internal buffer that breaks with large data requests
            // so if user requests more than BUFFER_LENGTH bytes, we have to do it in
            // smaller chunks instead of all at once
            for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) {
                Wire.beginTransmission(devAddr);
                Wire.write(regAddr);
                Wire.endTransmission();
                Wire.beginTransmission(devAddr);
                Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH));
 
                for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) {
                    data[count] = Wire.read();
                    #ifdef I2CDEV_SERIAL_DEBUG
                        Serial.print(data[count], HEX);
                        if (count + 1 < length) Serial.print(" ");
                    #endif
                }
            }
        #endif
 
    #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
 
        // Fastwire library
        // no loop required for fastwire
        uint8_t status = Fastwire::readBuf(devAddr << 1, regAddr, data, length);
        if (status == 0) {
            count = length; // success
        } else {
            count = -1; // error
        }
 
    #endif
 
    // check for timeout
    if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout
 
    #ifdef I2CDEV_SERIAL_DEBUG
        Serial.print(". Done (");
        Serial.print(count, DEC);
        Serial.println(" read).");
    #endif
 
    return count;
}
 
/** Read multiple words from a 16-bit device register.
 * @param devAddr I2C slave device address
 * @param regAddr First register regAddr to read from
 * @param length Number of words to read
 * @param data Buffer to store read data in
 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
 * @return Number of words read (-1 indicates failure)
 */
int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout) {
    #ifdef I2CDEV_SERIAL_DEBUG
        Serial.print("I2C (0x");
        Serial.print(devAddr, HEX);
        Serial.print(") reading ");
        Serial.print(length, DEC);
        Serial.print(" words from 0x");
        Serial.print(regAddr, HEX);
        Serial.print("...");
    #endif
 
    int8_t count = 0;
    uint32_t t1 = millis();
 
    #if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE)
 
        #if (ARDUINO < 100)
            // Arduino v00xx (before v1.0), Wire library
 
            // I2C/TWI subsystem uses internal buffer that breaks with large data requests
            // so if user requests more than BUFFER_LENGTH bytes, we have to do it in
            // smaller chunks instead of all at once
            for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) {
                Wire.beginTransmission(devAddr);
                Wire.send(regAddr);
                Wire.endTransmission();
                Wire.beginTransmission(devAddr);
                Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes
 
                bool msb = true; // starts with MSB, then LSB
                for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) {
                    if (msb) {
                        // first byte is bits 15-8 (MSb=15)
                        data[count] = Wire.receive() << 8;
                    } else {
                        // second byte is bits 7-0 (LSb=0)
                        data[count] |= Wire.receive();
                        #ifdef I2CDEV_SERIAL_DEBUG
                            Serial.print(data[count], HEX);
                            if (count + 1 < length) Serial.print(" ");
                        #endif
                        count++;
                    }
                    msb = !msb;
                }
 
                Wire.endTransmission();
            }
        #elif (ARDUINO == 100)
            // Arduino v1.0.0, Wire library
            // Adds standardized write() and read() stream methods instead of send() and receive()
 
            // I2C/TWI subsystem uses internal buffer that breaks with large data requests
            // so if user requests more than BUFFER_LENGTH bytes, we have to do it in
            // smaller chunks instead of all at once
            for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) {
                Wire.beginTransmission(devAddr);
                Wire.write(regAddr);
                Wire.endTransmission();
                Wire.beginTransmission(devAddr);
                Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes
 
                bool msb = true; // starts with MSB, then LSB
                for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) {
                    if (msb) {
                        // first byte is bits 15-8 (MSb=15)
                        data[count] = Wire.read() << 8;
                    } else {
                        // second byte is bits 7-0 (LSb=0)
                        data[count] |= Wire.read();
                        #ifdef I2CDEV_SERIAL_DEBUG
                            Serial.print(data[count], HEX);
                            if (count + 1 < length) Serial.print(" ");
                        #endif
                        count++;
                    }
                    msb = !msb;
                }
 
                Wire.endTransmission();
            }
        #elif (ARDUINO > 100)
            // Arduino v1.0.1+, Wire library
            // Adds official support for repeated start condition, yay!
 
            // I2C/TWI subsystem uses internal buffer that breaks with large data requests
            // so if user requests more than BUFFER_LENGTH bytes, we have to do it in
            // smaller chunks instead of all at once
            for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) {
                Wire.beginTransmission(devAddr);
                Wire.write(regAddr);
                Wire.endTransmission();
                Wire.beginTransmission(devAddr);
                Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes
 
                bool msb = true; // starts with MSB, then LSB
                for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) {
                    if (msb) {
                        // first byte is bits 15-8 (MSb=15)
                        data[count] = Wire.read() << 8;
                    } else {
                        // second byte is bits 7-0 (LSb=0)
                        data[count] |= Wire.read();
                        #ifdef I2CDEV_SERIAL_DEBUG
                            Serial.print(data[count], HEX);
                            if (count + 1 < length) Serial.print(" ");
                        #endif
                        count++;
                    }
                    msb = !msb;
                }
 
                Wire.endTransmission();
            }
        #endif
 
    #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
 
        // Fastwire library
        // no loop required for fastwire
        uint16_t intermediate[(uint8_t)length];
        uint8_t status = Fastwire::readBuf(devAddr << 1, regAddr, (uint8_t *)intermediate, (uint8_t)(length * 2));
        if (status == 0) {
            count = length; // success
            for (uint8_t i = 0; i < length; i++) {
                data[i] = (intermediate[2*i] << 8) | intermediate[2*i + 1];
            }
        } else {
            count = -1; // error
        }
 
    #endif
 
    if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout
 
    #ifdef I2CDEV_SERIAL_DEBUG
        Serial.print(". Done (");
        Serial.print(count, DEC);
        Serial.println(" read).");
    #endif
 
    return count;
}
 
/** write a single bit in an 8-bit device register.
 * @param devAddr I2C slave device address
 * @param regAddr Register regAddr to write to
 * @param bitNum Bit position to write (0-7)
 * @param value New bit value to write
 * @return Status of operation (true = success)
 */
bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) {
    uint8_t b;
    readByte(devAddr, regAddr, &b);
    b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
    return writeByte(devAddr, regAddr, b);
}
 
/** write a single bit in a 16-bit device register.
 * @param devAddr I2C slave device address
 * @param regAddr Register regAddr to write to
 * @param bitNum Bit position to write (0-15)
 * @param value New bit value to write
 * @return Status of operation (true = success)
 */
bool I2Cdev::writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data) {
    uint16_t w;
    readWord(devAddr, regAddr, &w);
    w = (data != 0) ? (w | (1 << bitNum)) : (w & ~(1 << bitNum));
    return writeWord(devAddr, regAddr, w);
}
 
/** Write multiple bits in an 8-bit device register.
 * @param devAddr I2C slave device address
 * @param regAddr Register regAddr to write to
 * @param bitStart First bit position to write (0-7)
 * @param length Number of bits to write (not more than 8)
 * @param data Right-aligned value to write
 * @return Status of operation (true = success)
 */
bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) {
    //      010 value to write
    // 76543210 bit numbers
    //    xxx   args: bitStart=4, length=3
    // 00011100 mask byte
    // 10101111 original value (sample)
    // 10100011 original & ~mask
    // 10101011 masked | value
    uint8_t b;
    if (readByte(devAddr, regAddr, &b) != 0) {
        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
        data <<= (bitStart - length + 1); // shift data into correct position
        data &= mask; // zero all non-important bits in data
        b &= ~(mask); // zero all important bits in existing byte
        b |= data; // combine data with existing byte
        return writeByte(devAddr, regAddr, b);
    } else {
        return false;
    }
}
 
/** Write multiple bits in a 16-bit device register.
 * @param devAddr I2C slave device address
 * @param regAddr Register regAddr to write to
 * @param bitStart First bit position to write (0-15)
 * @param length Number of bits to write (not more than 16)
 * @param data Right-aligned value to write
 * @return Status of operation (true = success)
 */
bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data) {
    //              010 value to write
    // fedcba9876543210 bit numbers
    //    xxx           args: bitStart=12, length=3
    // 0001110000000000 mask word
    // 1010111110010110 original value (sample)
    // 1010001110010110 original & ~mask
    // 1010101110010110 masked | value
    uint16_t w;
    if (readWord(devAddr, regAddr, &w) != 0) {
        uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1);
        data <<= (bitStart - length + 1); // shift data into correct position
        data &= mask; // zero all non-important bits in data
        w &= ~(mask); // zero all important bits in existing word
        w |= data; // combine data with existing word
        return writeWord(devAddr, regAddr, w);
    } else {
        return false;
    }
}
 
/** Write single byte to an 8-bit device register.
 * @param devAddr I2C slave device address
 * @param regAddr Register address to write to
 * @param data New byte value to write
 * @return Status of operation (true = success)
 */
bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) {
    return writeBytes(devAddr, regAddr, 1, &data);
}
 
/** Write single word to a 16-bit device register.
 * @param devAddr I2C slave device address
 * @param regAddr Register address to write to
 * @param data New word value to write
 * @return Status of operation (true = success)
 */
bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data) {
    return writeWords(devAddr, regAddr, 1, &data);
}
 
/** Write multiple bytes to an 8-bit device register.
 * @param devAddr I2C slave device address
 * @param regAddr First register address to write to
 * @param length Number of bytes to write
 * @param data Buffer to copy new data from
 * @return Status of operation (true = success)
 */
bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data) {
    #ifdef I2CDEV_SERIAL_DEBUG
        Serial.print("I2C (0x");
        Serial.print(devAddr, HEX);
        Serial.print(") writing ");
        Serial.print(length, DEC);
        Serial.print(" bytes to 0x");
        Serial.print(regAddr, HEX);
        Serial.print("...");
    #endif
    uint8_t status = 0;
    #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
        Wire.beginTransmission(devAddr);
        Wire.send((uint8_t) regAddr); // send address
    #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
        Wire.beginTransmission(devAddr);
        Wire.write((uint8_t) regAddr); // send address
    #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
        Fastwire::beginTransmission(devAddr);
        Fastwire::write(regAddr);
    #endif
    for (uint8_t i = 0; i < length; i++) {
        #ifdef I2CDEV_SERIAL_DEBUG
            Serial.print(data[i], HEX);
            if (i + 1 < length) Serial.print(" ");
        #endif
        #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
            Wire.send((uint8_t) data[i]);
        #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
            Wire.write((uint8_t) data[i]);
        #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
            Fastwire::write((uint8_t) data[i]);
        #endif
    }
    #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
        Wire.endTransmission();
    #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
        status = Wire.endTransmission();
    #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
        Fastwire::stop();
        //status = Fastwire::endTransmission();
    #endif
    #ifdef I2CDEV_SERIAL_DEBUG
        Serial.println(". Done.");
    #endif
    return status == 0;
}
 
/** Write multiple words to a 16-bit device register.
 * @param devAddr I2C slave device address
 * @param regAddr First register address to write to
 * @param length Number of words to write
 * @param data Buffer to copy new data from
 * @return Status of operation (true = success)
 */
bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t* data) {
    #ifdef I2CDEV_SERIAL_DEBUG
        Serial.print("I2C (0x");
        Serial.print(devAddr, HEX);
        Serial.print(") writing ");
        Serial.print(length, DEC);
        Serial.print(" words to 0x");
        Serial.print(regAddr, HEX);
        Serial.print("...");
    #endif
    uint8_t status = 0;
    #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
        Wire.beginTransmission(devAddr);
        Wire.send(regAddr); // send address
    #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
        Wire.beginTransmission(devAddr);
        Wire.write(regAddr); // send address
    #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
        Fastwire::beginTransmission(devAddr);
        Fastwire::write(regAddr);
    #endif
    for (uint8_t i = 0; i < length * 2; i++) {
        #ifdef I2CDEV_SERIAL_DEBUG
            Serial.print(data[i], HEX);
            if (i + 1 < length) Serial.print(" ");
        #endif
        #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
            Wire.send((uint8_t)(data[i] >> 8));     // send MSB
            Wire.send((uint8_t)data[i++]);          // send LSB
        #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
            Wire.write((uint8_t)(data[i] >> 8));    // send MSB
            Wire.write((uint8_t)data[i++]);         // send LSB
        #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
            Fastwire::write((uint8_t)(data[i] >> 8));       // send MSB
            status = Fastwire::write((uint8_t)data[i++]);   // send LSB
            if (status != 0) break;
        #endif
    }
    #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
        Wire.endTransmission();
    #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
        status = Wire.endTransmission();
    #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
        Fastwire::stop();
        //status = Fastwire::endTransmission();
    #endif
    #ifdef I2CDEV_SERIAL_DEBUG
        Serial.println(". Done.");
    #endif
    return status == 0;
}
 
/** Default timeout value for read operations.
 * Set this to 0 to disable timeout detection.
 */
uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;
 
#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
    // I2C library
    //////////////////////
    // Copyright(C) 2012
    // Francesco Ferrara
    // ferrara[at]libero[point]it
    //////////////////////
 
    /*
    FastWire
    - 0.24 added stop
    - 0.23 added reset
 
     This is a library to help faster programs to read I2C devices.
     Copyright(C) 2012 Francesco Ferrara
     occhiobello at gmail dot com
     [used by Jeff Rowberg for I2Cdevlib with permission]
     */
 
    boolean Fastwire::waitInt() {
        int l = 250;
        while (!(TWCR & (1 << TWINT)) && l-- > 0);
        return l > 0;
    }
 
    void Fastwire::setup(int khz, boolean pullup) {
        TWCR = 0;
        #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__)
            // activate internal pull-ups for twi (PORTC bits 4 & 5)
            // as per note from atmega8 manual pg167
            if (pullup) PORTC |= ((1 << 4) | (1 << 5));
            else        PORTC &= ~((1 << 4) | (1 << 5));
        #elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__)
            // activate internal pull-ups for twi (PORTC bits 0 & 1)
            if (pullup) PORTC |= ((1 << 0) | (1 << 1));
            else        PORTC &= ~((1 << 0) | (1 << 1));
        #else
            // activate internal pull-ups for twi (PORTD bits 0 & 1)
            // as per note from atmega128 manual pg204
            if (pullup) PORTD |= ((1 << 0) | (1 << 1));
            else        PORTD &= ~((1 << 0) | (1 << 1));
        #endif
 
        TWSR = 0; // no prescaler => prescaler = 1
        TWBR = ((16000L / khz) - 16) / 2; // change the I2C clock rate
        TWCR = 1 << TWEN; // enable twi module, no interrupt
    }
 
    // added by Jeff Rowberg 2013-05-07:
    // Arduino Wire-style "beginTransmission" function
    // (takes 7-bit device address like the Wire method, NOT 8-bit: 0x68, not 0xD0/0xD1)
    byte Fastwire::beginTransmission(byte device) {
        byte twst, retry;
        retry = 2;
        do {
            TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA);
            if (!waitInt()) return 1;
            twst = TWSR & 0xF8;
            if (twst != TW_START && twst != TW_REP_START) return 2;
 
            //Serial.print(device, HEX);
            //Serial.print(" ");
            TWDR = device << 1; // send device address without read bit (1)
            TWCR = (1 << TWINT) | (1 << TWEN);
            if (!waitInt()) return 3;
            twst = TWSR & 0xF8;
        } while (twst == TW_MT_SLA_NACK && retry-- > 0);
        if (twst != TW_MT_SLA_ACK) return 4;
        return 0;
    }
 
    byte Fastwire::writeBuf(byte device, byte address, byte *data, byte num) {
        byte twst, retry;
 
        retry = 2;
        do {
            TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA);
            if (!waitInt()) return 1;
            twst = TWSR & 0xF8;
            if (twst != TW_START && twst != TW_REP_START) return 2;
 
            //Serial.print(device, HEX);
            //Serial.print(" ");
            TWDR = device & 0xFE; // send device address without read bit (1)
            TWCR = (1 << TWINT) | (1 << TWEN);
            if (!waitInt()) return 3;
            twst = TWSR & 0xF8;
        } while (twst == TW_MT_SLA_NACK && retry-- > 0);
        if (twst != TW_MT_SLA_ACK) return 4;
 
        //Serial.print(address, HEX);
        //Serial.print(" ");
        TWDR = address; // send data to the previously addressed device
        TWCR = (1 << TWINT) | (1 << TWEN);
        if (!waitInt()) return 5;
        twst = TWSR & 0xF8;
        if (twst != TW_MT_DATA_ACK) return 6;
 
        for (byte i = 0; i < num; i++) {
            //Serial.print(data[i], HEX);
            //Serial.print(" ");
            TWDR = data[i]; // send data to the previously addressed device
            TWCR = (1 << TWINT) | (1 << TWEN);
            if (!waitInt()) return 7;
            twst = TWSR & 0xF8;
            if (twst != TW_MT_DATA_ACK) return 8;
        }
        //Serial.print("\n");
 
        return 0;
    }
 
    byte Fastwire::write(byte value) {
        byte twst;
        //Serial.println(value, HEX);
        TWDR = value; // send data
        TWCR = (1 << TWINT) | (1 << TWEN);
        if (!waitInt()) return 1;
        twst = TWSR & 0xF8;
        if (twst != TW_MT_DATA_ACK) return 2;
        return 0;
    }
 
    byte Fastwire::readBuf(byte device, byte address, byte *data, byte num) {
        byte twst, retry;
 
        retry = 2;
        do {
            TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA);
            if (!waitInt()) return 16;
            twst = TWSR & 0xF8;
            if (twst != TW_START && twst != TW_REP_START) return 17;
 
            //Serial.print(device, HEX);
            //Serial.print(" ");
            TWDR = device & 0xfe; // send device address to write
            TWCR = (1 << TWINT) | (1 << TWEN);
            if (!waitInt()) return 18;
            twst = TWSR & 0xF8;
        } while (twst == TW_MT_SLA_NACK && retry-- > 0);
        if (twst != TW_MT_SLA_ACK) return 19;
 
        //Serial.print(address, HEX);
        //Serial.print(" ");
        TWDR = address; // send data to the previously addressed device
        TWCR = (1 << TWINT) | (1 << TWEN);
        if (!waitInt()) return 20;
        twst = TWSR & 0xF8;
        if (twst != TW_MT_DATA_ACK) return 21;
 
        /***/
 
        retry = 2;
        do {
            TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA);
            if (!waitInt()) return 22;
            twst = TWSR & 0xF8;
            if (twst != TW_START && twst != TW_REP_START) return 23;
 
            //Serial.print(device, HEX);
            //Serial.print(" ");
            TWDR = device | 0x01; // send device address with the read bit (1)
            TWCR = (1 << TWINT) | (1 << TWEN);
            if (!waitInt()) return 24;
            twst = TWSR & 0xF8;
        } while (twst == TW_MR_SLA_NACK && retry-- > 0);
        if (twst != TW_MR_SLA_ACK) return 25;
 
        for (uint8_t i = 0; i < num; i++) {
            if (i == num - 1)
                TWCR = (1 << TWINT) | (1 << TWEN);
            else
                TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWEA);
            if (!waitInt()) return 26;
            twst = TWSR & 0xF8;
            if (twst != TW_MR_DATA_ACK && twst != TW_MR_DATA_NACK) return twst;
            data[i] = TWDR;
            //Serial.print(data[i], HEX);
            //Serial.print(" ");
        }
        //Serial.print("\n");
        stop();
 
        return 0;
    }
 
    void Fastwire::reset() {
        TWCR = 0;
    }
 
    byte Fastwire::stop() {
        TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);
        if (!waitInt()) return 1;
        return 0;
    }
#endif
 
#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE
    // NBWire implementation based heavily on code by Gene Knight <Gene@Telobot.com>
    // Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html
    // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html
 
    /*
    call this version 1.0
    
    Offhand, the only funky part that I can think of is in nbrequestFrom, where the buffer
    length and index are set *before* the data is actually read. The problem is that these
    are variables local to the TwoWire object, and by the time we actually have read the
    data, and know what the length actually is, we have no simple access to the object's 
    variables. The actual bytes read *is* given to the callback function, though.
    
    The ISR code for a slave receiver is commented out. I don't have that setup, and can't
    verify it at this time. Save it for 2.0!
    
    The handling of the read and write processes here is much like in the demo sketch code: 
    the process is broken down into sequential functions, where each registers the next as a
    callback, essentially.
    
    For example, for the Read process, twi_read00 just returns if TWI is not yet in a 
    ready state. When there's another interrupt, and the interface *is* ready, then it
    sets up the read, starts it, and registers twi_read01 as the function to call after
    the *next* interrupt. twi_read01, then, just returns if the interface is still in a
    "reading" state. When the reading is done, it copies the information to the buffer,
    cleans up, and calls the user-requested callback function with the actual number of 
    bytes read.
    
    The writing is similar.
    
    Questions, comments and problems can go to Gene@Telobot.com.
    
    Thumbs Up!
    Gene Knight
    
    */
 
    uint8_t TwoWire::rxBuffer[NBWIRE_BUFFER_LENGTH];
    uint8_t TwoWire::rxBufferIndex = 0;
    uint8_t TwoWire::rxBufferLength = 0;
 
    uint8_t TwoWire::txAddress = 0;
    uint8_t TwoWire::txBuffer[NBWIRE_BUFFER_LENGTH];
    uint8_t TwoWire::txBufferIndex = 0;
    uint8_t TwoWire::txBufferLength = 0;
 
    //uint8_t TwoWire::transmitting = 0;
    void (*TwoWire::user_onRequest)(void);
    void (*TwoWire::user_onReceive)(int);
 
    static volatile uint8_t twi_transmitting;
    static volatile uint8_t twi_state;
    static uint8_t twi_slarw;
    static volatile uint8_t twi_error;
    static uint8_t twi_masterBuffer[TWI_BUFFER_LENGTH];
    static volatile uint8_t twi_masterBufferIndex;
    static uint8_t twi_masterBufferLength;
    static uint8_t twi_rxBuffer[TWI_BUFFER_LENGTH];
    static volatile uint8_t twi_rxBufferIndex;
    //static volatile uint8_t twi_Interrupt_Continue_Command;
    static volatile uint8_t twi_Return_Value;
    static volatile uint8_t twi_Done;
    void (*twi_cbendTransmissionDone)(int);
    void (*twi_cbreadFromDone)(int);
 
    void twi_init() {
        // initialize state
        twi_state = TWI_READY;
 
        // activate internal pull-ups for twi
        // as per note from atmega8 manual pg167
        sbi(PORTC, 4);
        sbi(PORTC, 5);
 
        // initialize twi prescaler and bit rate
        cbi(TWSR, TWPS0); // TWI Status Register - Prescaler bits
        cbi(TWSR, TWPS1);
 
        /* twi bit rate formula from atmega128 manual pg 204
        SCL Frequency = CPU Clock Frequency / (16 + (2 * TWBR))
        note: TWBR should be 10 or higher for master mode
        It is 72 for a 16mhz Wiring board with 100kHz TWI */
 
        TWBR = ((CPU_FREQ / TWI_FREQ) - 16) / 2; // bitrate register
        // enable twi module, acks, and twi interrupt
 
        TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA);
 
        /* TWEN - TWI Enable Bit
        TWIE - TWI Interrupt Enable
        TWEA - TWI Enable Acknowledge Bit
        TWINT - TWI Interrupt Flag
        TWSTA - TWI Start Condition
        */
    }
 
    typedef struct {
        uint8_t address;
        uint8_t* data;
        uint8_t length;
        uint8_t wait;
        uint8_t i;
    } twi_Write_Vars;
 
    twi_Write_Vars *ptwv = 0;
    static void (*fNextInterruptFunction)(void) = 0;
 
    void twi_Finish(byte bRetVal) {
        if (ptwv) {
            free(ptwv);
            ptwv = 0;
        }
        twi_Done = 0xFF;
        twi_Return_Value = bRetVal;
        fNextInterruptFunction = 0;
    }
 
    uint8_t twii_WaitForDone(uint16_t timeout) {
        uint32_t endMillis = millis() + timeout;
        while (!twi_Done && (timeout == 0 || millis() < endMillis)) continue;
        return twi_Return_Value;
    }
 
    void twii_SetState(uint8_t ucState) {
        twi_state = ucState;
    }
 
    void twii_SetError(uint8_t ucError) {
        twi_error = ucError ;
    }
 
    void twii_InitBuffer(uint8_t ucPos, uint8_t ucLength) {
        twi_masterBufferIndex = 0;
        twi_masterBufferLength = ucLength;
    }
 
    void twii_CopyToBuf(uint8_t* pData, uint8_t ucLength) {
        uint8_t i;
        for (i = 0; i < ucLength; ++i) {
            twi_masterBuffer[i] = pData[i];
        }
    }
 
    void twii_CopyFromBuf(uint8_t *pData, uint8_t ucLength) {
        uint8_t i;
        for (i = 0; i < ucLength; ++i) {
            pData[i] = twi_masterBuffer[i];
        }
    }
 
    void twii_SetSlaRW(uint8_t ucSlaRW) {
        twi_slarw = ucSlaRW;
    }
 
    void twii_SetStart() {
        TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA);
    }
 
    void twi_write01() {
        if (TWI_MTX == twi_state) return; // blocking test
        twi_transmitting = 0 ;
        if (twi_error == 0xFF)
            twi_Finish (0);    // success
        else if (twi_error == TW_MT_SLA_NACK)
            twi_Finish (2);    // error: address send, nack received
        else if (twi_error == TW_MT_DATA_NACK)
            twi_Finish (3);    // error: data send, nack received
        else
            twi_Finish (4);    // other twi error
        if (twi_cbendTransmissionDone) return twi_cbendTransmissionDone(twi_Return_Value);
        return;
    }
 
 
    void twi_write00() {
        if (TWI_READY != twi_state) return; // blocking test
        if (TWI_BUFFER_LENGTH < ptwv -> length) {
            twi_Finish(1); // end write with error 1
            return;
        }
        twi_Done = 0x00; // show as working
        twii_SetState(TWI_MTX); // to transmitting
        twii_SetError(0xFF); // to No Error
        twii_InitBuffer(0, ptwv -> length); // pointer and length
        twii_CopyToBuf(ptwv -> data, ptwv -> length); // get the data
        twii_SetSlaRW((ptwv -> address << 1) | TW_WRITE); // write command
        twii_SetStart(); // start the cycle
        fNextInterruptFunction = twi_write01; // next routine
        return twi_write01();
    }
 
    void twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait) {
        uint8_t i;
        ptwv = (twi_Write_Vars *)malloc(sizeof(twi_Write_Vars));
        ptwv -> address = address;
        ptwv -> data = data;
        ptwv -> length = length;
        ptwv -> wait = wait;
        fNextInterruptFunction = twi_write00;
        return twi_write00();
    }
 
    void twi_read01() {
        if (TWI_MRX == twi_state) return; // blocking test
        if (twi_masterBufferIndex < ptwv -> length) ptwv -> length = twi_masterBufferIndex;
        twii_CopyFromBuf(ptwv -> data, ptwv -> length);
        twi_Finish(ptwv -> length);
        if (twi_cbreadFromDone) return twi_cbreadFromDone(twi_Return_Value);
        return;
    }
 
    void twi_read00() {
        if (TWI_READY != twi_state) return; // blocking test
        if (TWI_BUFFER_LENGTH < ptwv -> length) twi_Finish(0); // error return
        twi_Done = 0x00; // show as working
        twii_SetState(TWI_MRX); // reading
        twii_SetError(0xFF); // reset error
        twii_InitBuffer(0, ptwv -> length - 1); // init to one less than length
        twii_SetSlaRW((ptwv -> address << 1) | TW_READ); // read command
        twii_SetStart(); // start cycle
        fNextInterruptFunction = twi_read01;
        return twi_read01();
    }
 
    void twi_readFrom(uint8_t address, uint8_t* data, uint8_t length) {
        uint8_t i;
 
        ptwv = (twi_Write_Vars *)malloc(sizeof(twi_Write_Vars));
        ptwv -> address = address;
        ptwv -> data = data;
        ptwv -> length = length;
        fNextInterruptFunction = twi_read00;
        return twi_read00();
    }
 
    void twi_reply(uint8_t ack) {
        // transmit master read ready signal, with or without ack
        if (ack){
            TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT) | _BV(TWEA);
        } else {
            TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT);
        }
    }
 
    void twi_stop(void) {
        // send stop condition
        TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTO);
 
        // wait for stop condition to be exectued on bus
        // TWINT is not set after a stop condition!
        while (TWCR & _BV(TWSTO)) {
            continue;
        }
 
        // update twi state
        twi_state = TWI_READY;
    }
 
    void twi_releaseBus(void) {
        // release bus
        TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT);
 
        // update twi state
        twi_state = TWI_READY;
    }
 
    SIGNAL(TWI_vect) {
        switch (TW_STATUS) {
            // All Master
            case TW_START:     // sent start condition
            case TW_REP_START: // sent repeated start condition
                // copy device address and r/w bit to output register and ack
                TWDR = twi_slarw;
                twi_reply(1);
                break;
 
            // Master Transmitter
            case TW_MT_SLA_ACK:  // slave receiver acked address
            case TW_MT_DATA_ACK: // slave receiver acked data
                // if there is data to send, send it, otherwise stop
                if (twi_masterBufferIndex < twi_masterBufferLength) {
                    // copy data to output register and ack
                    TWDR = twi_masterBuffer[twi_masterBufferIndex++];
                    twi_reply(1);
                } else {
                    twi_stop();
                }
                break;
 
            case TW_MT_SLA_NACK:  // address sent, nack received
                twi_error = TW_MT_SLA_NACK;
                twi_stop();
                break;
 
            case TW_MT_DATA_NACK: // data sent, nack received
                twi_error = TW_MT_DATA_NACK;
                twi_stop();
                break;
 
            case TW_MT_ARB_LOST: // lost bus arbitration
                twi_error = TW_MT_ARB_LOST;
                twi_releaseBus();
                break;
 
            // Master Receiver
            case TW_MR_DATA_ACK: // data received, ack sent
                // put byte into buffer
                twi_masterBuffer[twi_masterBufferIndex++] = TWDR;
 
            case TW_MR_SLA_ACK:  // address sent, ack received
                // ack if more bytes are expected, otherwise nack
                if (twi_masterBufferIndex < twi_masterBufferLength) {
                    twi_reply(1);
                } else {
                    twi_reply(0);
                }
                break;
 
            case TW_MR_DATA_NACK: // data received, nack sent
                // put final byte into buffer
                twi_masterBuffer[twi_masterBufferIndex++] = TWDR;
 
            case TW_MR_SLA_NACK: // address sent, nack received
                twi_stop();
                break;
 
        // TW_MR_ARB_LOST handled by TW_MT_ARB_LOST case
 
        // Slave Receiver (NOT IMPLEMENTED YET)
        /*
            case TW_SR_SLA_ACK:   // addressed, returned ack
            case TW_SR_GCALL_ACK: // addressed generally, returned ack
            case TW_SR_ARB_LOST_SLA_ACK:   // lost arbitration, returned ack
            case TW_SR_ARB_LOST_GCALL_ACK: // lost arbitration, returned ack
                // enter slave receiver mode
                twi_state = TWI_SRX;
 
                // indicate that rx buffer can be overwritten and ack
                twi_rxBufferIndex = 0;
                twi_reply(1);
                break;
 
            case TW_SR_DATA_ACK:       // data received, returned ack
            case TW_SR_GCALL_DATA_ACK: // data received generally, returned ack
                // if there is still room in the rx buffer
                if (twi_rxBufferIndex < TWI_BUFFER_LENGTH) {
                    // put byte in buffer and ack
                    twi_rxBuffer[twi_rxBufferIndex++] = TWDR;
                    twi_reply(1);
                } else {
                    // otherwise nack
                    twi_reply(0);
                }
                break;
 
            case TW_SR_STOP: // stop or repeated start condition received
                // put a null char after data if there's room
                if (twi_rxBufferIndex < TWI_BUFFER_LENGTH) {
                    twi_rxBuffer[twi_rxBufferIndex] = 0;
                }
 
                // sends ack and stops interface for clock stretching
                twi_stop();
 
                // callback to user defined callback
                twi_onSlaveReceive(twi_rxBuffer, twi_rxBufferIndex);
 
                // since we submit rx buffer to "wire" library, we can reset it
                twi_rxBufferIndex = 0;
 
                // ack future responses and leave slave receiver state
                twi_releaseBus();
                break;
 
            case TW_SR_DATA_NACK:       // data received, returned nack
            case TW_SR_GCALL_DATA_NACK: // data received generally, returned nack
                // nack back at master
                twi_reply(0);
                break;
 
            // Slave Transmitter
            case TW_ST_SLA_ACK:          // addressed, returned ack
            case TW_ST_ARB_LOST_SLA_ACK: // arbitration lost, returned ack
                // enter slave transmitter mode
                twi_state = TWI_STX;
 
                // ready the tx buffer index for iteration
                twi_txBufferIndex = 0;
 
                // set tx buffer length to be zero, to verify if user changes it
                twi_txBufferLength = 0;
 
                // request for txBuffer to be filled and length to be set
                // note: user must call twi_transmit(bytes, length) to do this
                twi_onSlaveTransmit();
 
                // if they didn't change buffer & length, initialize it
                if (0 == twi_txBufferLength) {
                    twi_txBufferLength = 1;
                    twi_txBuffer[0] = 0x00;
                }
                
                // transmit first byte from buffer, fall through
 
            case TW_ST_DATA_ACK: // byte sent, ack returned
                // copy data to output register
                TWDR = twi_txBuffer[twi_txBufferIndex++];
 
                // if there is more to send, ack, otherwise nack
                if (twi_txBufferIndex < twi_txBufferLength) {
                    twi_reply(1);
                } else {
                    twi_reply(0);
                }
                break;
 
            case TW_ST_DATA_NACK: // received nack, we are done
            case TW_ST_LAST_DATA: // received ack, but we are done already!
                // ack future responses
                twi_reply(1);
                // leave slave receiver state
                twi_state = TWI_READY;
                break;
            */
 
            // all
            case TW_NO_INFO:   // no state information
                break;
 
            case TW_BUS_ERROR: // bus error, illegal stop/start
                twi_error = TW_BUS_ERROR;
                twi_stop();
                break;
        }
 
        if (fNextInterruptFunction) return fNextInterruptFunction();
    }
 
    TwoWire::TwoWire() { }
 
    void TwoWire::begin(void) {
        rxBufferIndex = 0;
        rxBufferLength = 0;
 
        txBufferIndex = 0;
        txBufferLength = 0;
 
        twi_init();
    }
 
    void TwoWire::beginTransmission(uint8_t address) {
        //beginTransmission((uint8_t)address);
 
        // indicate that we are transmitting
        twi_transmitting = 1;
 
        // set address of targeted slave
        txAddress = address;
 
        // reset tx buffer iterator vars
        txBufferIndex = 0;
        txBufferLength = 0;
    }
 
    uint8_t TwoWire::endTransmission(uint16_t timeout) {
        // transmit buffer (blocking)
        //int8_t ret =
        twi_cbendTransmissionDone = NULL;
        twi_writeTo(txAddress, txBuffer, txBufferLength, 1);
        int8_t ret = twii_WaitForDone(timeout);
 
        // reset tx buffer iterator vars
        txBufferIndex = 0;
        txBufferLength = 0;
 
        // indicate that we are done transmitting
        // twi_transmitting = 0;
        return ret;
    }
 
    void TwoWire::nbendTransmission(void (*function)(int)) {
        twi_cbendTransmissionDone = function;
        twi_writeTo(txAddress, txBuffer, txBufferLength, 1);
        return;
    }
 
    void TwoWire::send(uint8_t data) {
        if (twi_transmitting) {
            // in master transmitter mode
            // don't bother if buffer is full
            if (txBufferLength >= NBWIRE_BUFFER_LENGTH) {
                return;
            }
 
            // put byte in tx buffer
            txBuffer[txBufferIndex] = data;
            ++txBufferIndex;
 
            // update amount in buffer
            txBufferLength = txBufferIndex;
        } else {
            // in slave send mode
            // reply to master
            //twi_transmit(&data, 1);
        }
    }
 
    uint8_t TwoWire::receive(void) {
        // default to returning null char
        // for people using with char strings
        uint8_t value = 0;
 
        // get each successive byte on each call
        if (rxBufferIndex < rxBufferLength) {
            value = rxBuffer[rxBufferIndex];
            ++rxBufferIndex;
        }
 
        return value;
    }
 
    uint8_t TwoWire::requestFrom(uint8_t address, int quantity, uint16_t timeout) {
        // clamp to buffer length
        if (quantity > NBWIRE_BUFFER_LENGTH) {
            quantity = NBWIRE_BUFFER_LENGTH;
        }
 
        // perform blocking read into buffer
        twi_cbreadFromDone = NULL;
        twi_readFrom(address, rxBuffer, quantity);
        uint8_t read = twii_WaitForDone(timeout);
 
        // set rx buffer iterator vars
        rxBufferIndex = 0;
        rxBufferLength = read;
 
        return read;
    }
 
    void TwoWire::nbrequestFrom(uint8_t address, int quantity, void (*function)(int)) {
        // clamp to buffer length
        if (quantity > NBWIRE_BUFFER_LENGTH) {
            quantity = NBWIRE_BUFFER_LENGTH;
        }
 
        // perform blocking read into buffer
        twi_cbreadFromDone = function;
        twi_readFrom(address, rxBuffer, quantity);
        //uint8_t read = twii_WaitForDone();
 
        // set rx buffer iterator vars
        //rxBufferIndex = 0;
        //rxBufferLength = read;
 
        rxBufferIndex = 0;
        rxBufferLength = quantity; // this is a hack
 
        return; //read;
    }
 
    uint8_t TwoWire::available(void) {
        return rxBufferLength - rxBufferIndex;
    }
 
#endif

I2Cdev.h :

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

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
#ifndef _I2CDEV_H_
#define _I2CDEV_H_
#define I2CDEV_IMPLEMENTATION       I2CDEV_ARDUINO_WIRE
#define I2CDEV_IMPLEMENTATION_WARNINGS
#define I2CDEV_ARDUINO_WIRE         1
#define I2CDEV_BUILTIN_NBWIRE       2
#define I2CDEV_BUILTIN_FASTWIRE     3
#define I2CDEV_I2CMASTER_LIBRARY    4
#ifdef ARDUINO
    #if ARDUINO < 100
        #include "WProgram.h"
    #else
        #include "Arduino.h"
    #endif
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        #include <Wire.h>
    #endif
    #if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY
        #include <I2C.h>
    #endif
#endif
#define I2CDEV_DEFAULT_READ_TIMEOUT     1000
class I2Cdev {
    public:
        I2Cdev();
        static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
        static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
        static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
        static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
        static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
        static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
        static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
        static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
        static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data);
        static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data);
        static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data);
        static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data);
        static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
        static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data);
        static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
        static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
        static uint16_t readTimeout;
};
#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
    #define TW_START                0x08
    #define TW_REP_START            0x10
    #define TW_MT_SLA_ACK           0x18
    #define TW_MT_SLA_NACK          0x20
    #define TW_MT_DATA_ACK          0x28
    #define TW_MT_DATA_NACK         0x30
    #define TW_MT_ARB_LOST          0x38
    #define TW_MR_ARB_LOST          0x38
    #define TW_MR_SLA_ACK           0x40
    #define TW_MR_SLA_NACK          0x48
    #define TW_MR_DATA_ACK          0x50
    #define TW_MR_DATA_NACK         0x58
    #define TW_OK                   0
    #define TW_ERROR                1
    class Fastwire {
        private:
            static boolean waitInt();
        public:
            static void setup(int khz, boolean pullup);
            static byte beginTransmission(byte device);
            static byte write(byte value);
            static byte writeBuf(byte device, byte address, byte *data, byte num);
            static byte readBuf(byte device, byte address, byte *data, byte num);
            static void reset();
            static byte stop();
    };
#endif
#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE
    #define NBWIRE_BUFFER_LENGTH 32
    class TwoWire {
        private:
            static uint8_t rxBuffer[];
            static uint8_t rxBufferIndex;
            static uint8_t rxBufferLength;
            static uint8_t txAddress;
            static uint8_t txBuffer[];
            static uint8_t txBufferIndex;
            static uint8_t txBufferLength;
            static void (*user_onRequest)(void);
            static void (*user_onReceive)(int);
            static void onRequestService(void);
            static void onReceiveService(uint8_t*, int);
        public:
            TwoWire();
            void begin();
            void begin(uint8_t);
            void begin(int);
            void beginTransmission(uint8_t);
            uint8_t endTransmission(uint16_t timeout=0);
            void nbendTransmission(void (*function)(int)) ;
            uint8_t requestFrom(uint8_t, int, uint16_t timeout=0);
            void nbrequestFrom(uint8_t, int, void (*function)(int));
            void send(uint8_t);
            void send(uint8_t*, uint8_t);
            void send(char*);
            uint8_t available(void);
            uint8_t receive(void);
            void onReceive(void (*)(int));
            void onRequest(void (*)(void));
    };
    #define TWI_READY   0
    #define TWI_MRX     1
    #define TWI_MTX     2
    #define TWI_SRX     3
    #define TWI_STX     4
    #define TW_WRITE    0
    #define TW_READ     1
    #define TW_MT_SLA_NACK      0x20
    #define TW_MT_DATA_NACK     0x30
    #define CPU_FREQ            16000000L
    #define TWI_FREQ            100000L
    #define TWI_BUFFER_LENGTH   32
    #define TW_STATUS_MASK              (_BV(TWS7)|_BV(TWS6)|_BV(TWS5)|_BV(TWS4)|_BV(TWS3))
    #define TW_STATUS                   (TWSR & TW_STATUS_MASK)
    #define TW_START                    0x08
    #define TW_REP_START                0x10
    #define TW_MT_SLA_ACK               0x18
    #define TW_MT_SLA_NACK              0x20
    #define TW_MT_DATA_ACK              0x28
    #define TW_MT_DATA_NACK             0x30
    #define TW_MT_ARB_LOST              0x38
    #define TW_MR_ARB_LOST              0x38
    #define TW_MR_SLA_ACK               0x40
    #define TW_MR_SLA_NACK              0x48
    #define TW_MR_DATA_ACK              0x50
    #define TW_MR_DATA_NACK             0x58
    #define TW_ST_SLA_ACK               0xA8
    #define TW_ST_ARB_LOST_SLA_ACK      0xB0
    #define TW_ST_DATA_ACK              0xB8
    #define TW_ST_DATA_NACK             0xC0
    #define TW_ST_LAST_DATA             0xC8
    #define TW_SR_SLA_ACK               0x60
    #define TW_SR_ARB_LOST_SLA_ACK      0x68
    #define TW_SR_GCALL_ACK             0x70
    #define TW_SR_ARB_LOST_GCALL_ACK    0x78
    #define TW_SR_DATA_ACK              0x80
    #define TW_SR_DATA_NACK             0x88
    #define TW_SR_GCALL_DATA_ACK        0x90
    #define TW_SR_GCALL_DATA_NACK       0x98
    #define TW_SR_STOP                  0xA0
    #define TW_NO_INFO                  0xF8
    #define TW_BUS_ERROR                0x00
    #ifndef sbi
        #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
    #endif
    #ifndef cbi
        #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
    #endif
    extern TwoWire Wire;
#endif
#endif

Invité · 02/03/2023, 20h04

KalmanFilter.cpp :

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

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
#include "KalmanFilter.h"
 
void KalmanFilter::Yiorderfilter(float angle_m, float gyro_m,float dt,float K1) {
  angle6 = K1 * angle_m + (1 - K1) * (angle6 + gyro_m * dt);
}
 
void KalmanFilter::Kalman_Filter(double angle_m, double gyro_m,float dt,float Q_angle,float Q_gyro,float R_angle,float C_0) {
  angle += (gyro_m - q_bias) * dt;
  angle_err = angle_m - angle;
  Pdot[0] = Q_angle - P[0][1] - P[1][0];
  Pdot[1] = - P[1][1];
  Pdot[2] = - P[1][1];
  Pdot[3] = Q_gyro;
  P[0][0] += Pdot[0] * dt;
  P[0][1] += Pdot[1] * dt;
  P[1][0] += Pdot[2] * dt;
  P[1][1] += Pdot[3] * dt;
  PCt_0 = C_0 * P[0][0];
  PCt_1 = C_0 * P[1][0];
  E = R_angle + C_0 * PCt_0;
  K_0 = PCt_0 / E;
  K_1 = PCt_1 / E;
  t_0 = PCt_0;
  t_1 = C_0 * P[0][1];
  P[0][0] -= K_0 * t_0;
  P[0][1] -= K_0 * t_1;
  P[1][0] -= K_1 * t_0;
  P[1][1] -= K_1 * t_1;
  angle += K_0 * angle_err;
  q_bias += K_1 * angle_err;
  angle_dot = gyro_m - q_bias;
}
 
void KalmanFilter::Angle(int16_t ax, int16_t ay, int16_t az, int16_t gx, int16_t gy, int16_t gz, float dt, float Q_angle, float Q_gyro, float R_angle, float C_0, float K1) {
  float Angle = atan2(ay , az) * 57.3;
  Gyro_x = (gx - 128.1) / 131;
  Kalman_Filter(Angle, Gyro_x, dt, Q_angle, Q_gyro, R_angle, C_0);
  Gyro_z = -gz / 131;
}

KalmanFilter.h :

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

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
#ifndef KalmanFilter_h
#define KalmanFilter_h
 
#if defined(ARDUINO) && (ARDUINO >= 100)
#include <Arduino.h>
#else
#include <WProgram.h>
#endif
 
class KalmanFilter
{
public:
  void Yiorderfilter(float angle_m, float gyro_m,float dt,float K1);
  void Kalman_Filter(double angle_m, double gyro_m,float dt,float Q_angle,float Q_gyro,float R_angle,float C_0);
  void Angle(int16_t ax, int16_t ay, int16_t az, int16_t gx, int16_t gy, int16_t gz, float dt, float Q_angle, float Q_gyro, float R_angle, float C_0, float K1);
  float Gyro_x, Gyro_y, Gyro_z;
  float accelz = 0;
  float angle;
  float angle6;
private:
  float angle_err, q_bias;
  float Pdot[4] = { 0, 0, 0, 0};
  float P[2][2] = {{ 1, 0 }, { 0, 1 }};
  float PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
  float angle_dot;
};
 
#endif

MPU6050.cpp :

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

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
1001
1002
1003
1004
1005
1006
1007
1008
1009
1010
1011
1012
1013
1014
1015
1016
1017
1018
1019
1020
1021
1022
1023
1024
1025
1026
1027
1028
1029
1030
1031
1032
1033
1034
1035
1036
1037
1038
1039
1040
1041
1042
1043
1044
1045
1046
1047
1048
1049
1050
1051
1052
1053
1054
1055
1056
1057
1058
1059
1060
1061
1062
1063
1064
1065
1066
1067
1068
1069
1070
1071
1072
1073
1074
1075
1076
1077
1078
1079
1080
1081
1082
1083
1084
1085
1086
1087
1088
1089
1090
1091
1092
1093
1094
1095
1096
1097
1098
1099
1100
1101
1102
1103
1104
1105
1106
1107
1108
1109
1110
1111
1112
1113
1114
1115
1116
1117
1118
1119
1120
1121
1122
1123
1124
1125
1126
1127
1128
1129
1130
1131
1132
1133
1134
1135
1136
1137
1138
1139
1140
1141
1142
1143
1144
1145
1146
1147
1148
1149
1150
1151
1152
1153
1154
1155
1156
1157
1158
1159
1160
1161
1162
1163
1164
1165
1166
1167
1168
1169
1170
1171
1172
1173
1174
1175
1176
1177
1178
1179
1180
1181
1182
1183
1184
1185
1186
1187
1188
1189
1190
1191
1192
1193
1194
1195
1196
1197
1198
1199
1200
1201
1202
1203
1204
1205
1206
1207
1208
1209
1210
1211
1212
1213
1214
1215
1216
1217
1218
1219
1220
1221
1222
1223
1224
1225
1226
1227
1228
1229
1230
1231
1232
1233
1234
1235
1236
1237
1238
1239
1240
1241
1242
1243
1244
1245
1246
1247
1248
1249
1250
1251
1252
1253
1254
1255
1256
1257
1258
1259
1260
1261
1262
1263
1264
1265
1266
1267
1268
1269
1270
1271
1272
1273
1274
1275
1276
1277
1278
1279
1280
1281
1282
#include "MPU6050.h"
MPU6050::MPU6050() { devAddr = MPU6050_DEFAULT_ADDRESS; }
MPU6050::MPU6050(uint8_t address) { devAddr = address; }
void MPU6050::initialize() {
  setClockSource(MPU6050_CLOCK_PLL_XGYRO);
  setFullScaleGyroRange(MPU6050_GYRO_FS_250);
  setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
  setSleepEnabled(false);
}
bool MPU6050::testConnection() { return getDeviceID() == 0x34; }
uint8_t MPU6050::getAuxVDDIOLevel() {
  I2Cdev::readBit(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setAuxVDDIOLevel(uint8_t level) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT,
                   level);
}
uint8_t MPU6050::getRate() {
  I2Cdev::readByte(devAddr, MPU6050_RA_SMPLRT_DIV, buffer);
  return buffer[0];
}
void MPU6050::setRate(uint8_t rate) {
  I2Cdev::writeByte(devAddr, MPU6050_RA_SMPLRT_DIV, rate);
}
uint8_t MPU6050::getExternalFrameSync() {
  I2Cdev::readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT,
                   MPU6050_CFG_EXT_SYNC_SET_LENGTH, buffer);
  return buffer[0];
}
void MPU6050::setExternalFrameSync(uint8_t sync) {
  I2Cdev::writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT,
                    MPU6050_CFG_EXT_SYNC_SET_LENGTH, sync);
}
uint8_t MPU6050::getDLPFMode() {
  I2Cdev::readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT,
                   MPU6050_CFG_DLPF_CFG_LENGTH, buffer);
  return buffer[0];
}
void MPU6050::setDLPFMode(uint8_t mode) {
  I2Cdev::writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT,
                    MPU6050_CFG_DLPF_CFG_LENGTH, mode);
}
uint8_t MPU6050::getFullScaleGyroRange() {
  I2Cdev::readBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT,
                   MPU6050_GCONFIG_FS_SEL_LENGTH, buffer);
  return buffer[0];
}
void MPU6050::setFullScaleGyroRange(uint8_t range) {
  I2Cdev::writeBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT,
                    MPU6050_GCONFIG_FS_SEL_LENGTH, range);
}
bool MPU6050::getAccelXSelfTest() {
  I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setAccelXSelfTest(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT,
                   enabled);
}
bool MPU6050::getAccelYSelfTest() {
  I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setAccelYSelfTest(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT,
                   enabled);
}
bool MPU6050::getAccelZSelfTest() {
  I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setAccelZSelfTest(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT,
                   enabled);
}
uint8_t MPU6050::getFullScaleAccelRange() {
  I2Cdev::readBits(devAddr, MPU6050_RA_ACCEL_CONFIG,
                   MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH,
                   buffer);
  return buffer[0];
}
void MPU6050::setFullScaleAccelRange(uint8_t range) {
  I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG,
                    MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH,
                    range);
}
uint8_t MPU6050::getDHPFMode() {
  I2Cdev::readBits(devAddr, MPU6050_RA_ACCEL_CONFIG,
                   MPU6050_ACONFIG_ACCEL_HPF_BIT,
                   MPU6050_ACONFIG_ACCEL_HPF_LENGTH, buffer);
  return buffer[0];
}
void MPU6050::setDHPFMode(uint8_t bandwidth) {
  I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG,
                    MPU6050_ACONFIG_ACCEL_HPF_BIT,
                    MPU6050_ACONFIG_ACCEL_HPF_LENGTH, bandwidth);
}
uint8_t MPU6050::getFreefallDetectionThreshold() {
  I2Cdev::readByte(devAddr, MPU6050_RA_FF_THR, buffer);
  return buffer[0];
}
void MPU6050::setFreefallDetectionThreshold(uint8_t threshold) {
  I2Cdev::writeByte(devAddr, MPU6050_RA_FF_THR, threshold);
}
uint8_t MPU6050::getFreefallDetectionDuration() {
  I2Cdev::readByte(devAddr, MPU6050_RA_FF_DUR, buffer);
  return buffer[0];
}
void MPU6050::setFreefallDetectionDuration(uint8_t duration) {
  I2Cdev::writeByte(devAddr, MPU6050_RA_FF_DUR, duration);
}
uint8_t MPU6050::getMotionDetectionThreshold() {
  I2Cdev::readByte(devAddr, MPU6050_RA_MOT_THR, buffer);
  return buffer[0];
}
void MPU6050::setMotionDetectionThreshold(uint8_t threshold) {
  I2Cdev::writeByte(devAddr, MPU6050_RA_MOT_THR, threshold);
}
uint8_t MPU6050::getMotionDetectionDuration() {
  I2Cdev::readByte(devAddr, MPU6050_RA_MOT_DUR, buffer);
  return buffer[0];
}
void MPU6050::setMotionDetectionDuration(uint8_t duration) {
  I2Cdev::writeByte(devAddr, MPU6050_RA_MOT_DUR, duration);
}
uint8_t MPU6050::getZeroMotionDetectionThreshold() {
  I2Cdev::readByte(devAddr, MPU6050_RA_ZRMOT_THR, buffer);
  return buffer[0];
}
void MPU6050::setZeroMotionDetectionThreshold(uint8_t threshold) {
  I2Cdev::writeByte(devAddr, MPU6050_RA_ZRMOT_THR, threshold);
}
uint8_t MPU6050::getZeroMotionDetectionDuration() {
  I2Cdev::readByte(devAddr, MPU6050_RA_ZRMOT_DUR, buffer);
  return buffer[0];
}
void MPU6050::setZeroMotionDetectionDuration(uint8_t duration) {
  I2Cdev::writeByte(devAddr, MPU6050_RA_ZRMOT_DUR, duration);
}
bool MPU6050::getTempFIFOEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setTempFIFOEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT,
                   enabled);
}
bool MPU6050::getXGyroFIFOEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, buffer);
  return buffer[0];
}
void MPU6050::setXGyroFIFOEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT,
                   enabled);
}
bool MPU6050::getYGyroFIFOEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, buffer);
  return buffer[0];
}
void MPU6050::setYGyroFIFOEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT,
                   enabled);
}
bool MPU6050::getZGyroFIFOEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, buffer);
  return buffer[0];
}
void MPU6050::setZGyroFIFOEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT,
                   enabled);
}
bool MPU6050::getAccelFIFOEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setAccelFIFOEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT,
                   enabled);
}
bool MPU6050::getSlave2FIFOEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setSlave2FIFOEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT,
                   enabled);
}
bool MPU6050::getSlave1FIFOEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setSlave1FIFOEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT,
                   enabled);
}
bool MPU6050::getSlave0FIFOEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setSlave0FIFOEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT,
                   enabled);
}
bool MPU6050::getMultiMasterEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setMultiMasterEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT,
                   enabled);
}
bool MPU6050::getWaitForExternalSensorEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setWaitForExternalSensorEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT,
                   enabled);
}
bool MPU6050::getSlave3FIFOEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setSlave3FIFOEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT,
                   enabled);
}
bool MPU6050::getSlaveReadWriteTransitionEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setSlaveReadWriteTransitionEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT,
                   enabled);
}
uint8_t MPU6050::getMasterClockSpeed() {
  I2Cdev::readBits(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT,
                   MPU6050_I2C_MST_CLK_LENGTH, buffer);
  return buffer[0];
}
void MPU6050::setMasterClockSpeed(uint8_t speed) {
  I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT,
                    MPU6050_I2C_MST_CLK_LENGTH, speed);
}
uint8_t MPU6050::getSlaveAddress(uint8_t num) {
  if (num > 3)
    return 0;
  I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + num * 3, buffer);
  return buffer[0];
}
void MPU6050::setSlaveAddress(uint8_t num, uint8_t address) {
  if (num > 3)
    return;
  I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + num * 3, address);
}
uint8_t MPU6050::getSlaveRegister(uint8_t num) {
  if (num > 3)
    return 0;
  I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV0_REG + num * 3, buffer);
  return buffer[0];
}
void MPU6050::setSlaveRegister(uint8_t num, uint8_t reg) {
  if (num > 3)
    return;
  I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_REG + num * 3, reg);
}
bool MPU6050::getSlaveEnabled(uint8_t num) {
  if (num > 3)
    return 0;
  I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3,
                  MPU6050_I2C_SLV_EN_BIT, buffer);
  return buffer[0];
}
void MPU6050::setSlaveEnabled(uint8_t num, bool enabled) {
  if (num > 3)
    return;
  I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3,
                   MPU6050_I2C_SLV_EN_BIT, enabled);
}
bool MPU6050::getSlaveWordByteSwap(uint8_t num) {
  if (num > 3)
    return 0;
  I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3,
                  MPU6050_I2C_SLV_BYTE_SW_BIT, buffer);
  return buffer[0];
}
void MPU6050::setSlaveWordByteSwap(uint8_t num, bool enabled) {
  if (num > 3)
    return;
  I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3,
                   MPU6050_I2C_SLV_BYTE_SW_BIT, enabled);
}
bool MPU6050::getSlaveWriteMode(uint8_t num) {
  if (num > 3)
    return 0;
  I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3,
                  MPU6050_I2C_SLV_REG_DIS_BIT, buffer);
  return buffer[0];
}
void MPU6050::setSlaveWriteMode(uint8_t num, bool mode) {
  if (num > 3)
    return;
  I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3,
                   MPU6050_I2C_SLV_REG_DIS_BIT, mode);
}
bool MPU6050::getSlaveWordGroupOffset(uint8_t num) {
  if (num > 3)
    return 0;
  I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3,
                  MPU6050_I2C_SLV_GRP_BIT, buffer);
  return buffer[0];
}
void MPU6050::setSlaveWordGroupOffset(uint8_t num, bool enabled) {
  if (num > 3)
    return;
  I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3,
                   MPU6050_I2C_SLV_GRP_BIT, enabled);
}
uint8_t MPU6050::getSlaveDataLength(uint8_t num) {
  if (num > 3)
    return 0;
  I2Cdev::readBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3,
                   MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, buffer);
  return buffer[0];
}
void MPU6050::setSlaveDataLength(uint8_t num, uint8_t length) {
  if (num > 3)
    return;
  I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3,
                    MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH,
                    length);
}
uint8_t MPU6050::getSlave4Address() {
  I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_ADDR, buffer);
  return buffer[0];
}
void MPU6050::setSlave4Address(uint8_t address) {
  I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV4_ADDR, address);
}
uint8_t MPU6050::getSlave4Register() {
  I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_REG, buffer);
  return buffer[0];
}
void MPU6050::setSlave4Register(uint8_t reg) {
  I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV4_REG, reg);
}
void MPU6050::setSlave4OutputByte(uint8_t data) {
  I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV4_DO, data);
}
bool MPU6050::getSlave4Enabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setSlave4Enabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT,
                   enabled);
}
bool MPU6050::getSlave4InterruptEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL,
                  MPU6050_I2C_SLV4_INT_EN_BIT, buffer);
  return buffer[0];
}
void MPU6050::setSlave4InterruptEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL,
                   MPU6050_I2C_SLV4_INT_EN_BIT, enabled);
}
bool MPU6050::getSlave4WriteMode() {
  I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL,
                  MPU6050_I2C_SLV4_REG_DIS_BIT, buffer);
  return buffer[0];
}
void MPU6050::setSlave4WriteMode(bool mode) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL,
                   MPU6050_I2C_SLV4_REG_DIS_BIT, mode);
}
uint8_t MPU6050::getSlave4MasterDelay() {
  I2Cdev::readBits(devAddr, MPU6050_RA_I2C_SLV4_CTRL,
                   MPU6050_I2C_SLV4_MST_DLY_BIT,
                   MPU6050_I2C_SLV4_MST_DLY_LENGTH, buffer);
  return buffer[0];
}
void MPU6050::setSlave4MasterDelay(uint8_t delay) {
  I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV4_CTRL,
                    MPU6050_I2C_SLV4_MST_DLY_BIT,
                    MPU6050_I2C_SLV4_MST_DLY_LENGTH, delay);
}
uint8_t MPU6050::getSlate4InputByte() {
  I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_DI, buffer);
  return buffer[0];
}
bool MPU6050::getPassthroughStatus() {
  I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS,
                  MPU6050_MST_PASS_THROUGH_BIT, buffer);
  return buffer[0];
}
bool MPU6050::getSlave4IsDone() {
  I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS,
                  MPU6050_MST_I2C_SLV4_DONE_BIT, buffer);
  return buffer[0];
}
bool MPU6050::getLostArbitration() {
  I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS,
                  MPU6050_MST_I2C_LOST_ARB_BIT, buffer);
  return buffer[0];
}
bool MPU6050::getSlave4Nack() {
  I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS,
                  MPU6050_MST_I2C_SLV4_NACK_BIT, buffer);
  return buffer[0];
}
bool MPU6050::getSlave3Nack() {
  I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS,
                  MPU6050_MST_I2C_SLV3_NACK_BIT, buffer);
  return buffer[0];
}
bool MPU6050::getSlave2Nack() {
  I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS,
                  MPU6050_MST_I2C_SLV2_NACK_BIT, buffer);
  return buffer[0];
}
bool MPU6050::getSlave1Nack() {
  I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS,
                  MPU6050_MST_I2C_SLV1_NACK_BIT, buffer);
  return buffer[0];
}
bool MPU6050::getSlave0Nack() {
  I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS,
                  MPU6050_MST_I2C_SLV0_NACK_BIT, buffer);
  return buffer[0];
}
bool MPU6050::getInterruptMode() {
  I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setInterruptMode(bool mode) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG,
                   MPU6050_INTCFG_INT_LEVEL_BIT, mode);
}
bool MPU6050::getInterruptDrive() {
  I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setInterruptDrive(bool drive) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT,
                   drive);
}
bool MPU6050::getInterruptLatch() {
  I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG,
                  MPU6050_INTCFG_LATCH_INT_EN_BIT, buffer);
  return buffer[0];
}
void MPU6050::setInterruptLatch(bool latch) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG,
                   MPU6050_INTCFG_LATCH_INT_EN_BIT, latch);
}
bool MPU6050::getInterruptLatchClear() {
  I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG,
                  MPU6050_INTCFG_INT_RD_CLEAR_BIT, buffer);
  return buffer[0];
}
void MPU6050::setInterruptLatchClear(bool clear) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG,
                   MPU6050_INTCFG_INT_RD_CLEAR_BIT, clear);
}
bool MPU6050::getFSyncInterruptLevel() {
  I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG,
                  MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, buffer);
  return buffer[0];
}
void MPU6050::setFSyncInterruptLevel(bool level) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG,
                   MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, level);
}
bool MPU6050::getFSyncInterruptEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG,
                  MPU6050_INTCFG_FSYNC_INT_EN_BIT, buffer);
  return buffer[0];
}
void MPU6050::setFSyncInterruptEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG,
                   MPU6050_INTCFG_FSYNC_INT_EN_BIT, enabled);
}
bool MPU6050::getI2CBypassEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG,
                  MPU6050_INTCFG_I2C_BYPASS_EN_BIT, buffer);
  return buffer[0];
}
void MPU6050::setI2CBypassEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG,
                   MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled);
}
bool MPU6050::getClockOutputEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setClockOutputEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG,
                   MPU6050_INTCFG_CLKOUT_EN_BIT, enabled);
}
uint8_t MPU6050::getIntEnabled() {
  I2Cdev::readByte(devAddr, MPU6050_RA_INT_ENABLE, buffer);
  return buffer[0];
}
void MPU6050::setIntEnabled(uint8_t enabled) {
  I2Cdev::writeByte(devAddr, MPU6050_RA_INT_ENABLE, enabled);
}
bool MPU6050::getIntFreefallEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setIntFreefallEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT,
                   enabled);
}
bool MPU6050::getIntMotionEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setIntMotionEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT,
                   enabled);
}
bool MPU6050::getIntZeroMotionEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setIntZeroMotionEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT,
                   enabled);
}
bool MPU6050::getIntFIFOBufferOverflowEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE,
                  MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer);
  return buffer[0];
}
void MPU6050::setIntFIFOBufferOverflowEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE,
                   MPU6050_INTERRUPT_FIFO_OFLOW_BIT, enabled);
}
bool MPU6050::getIntI2CMasterEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE,
                  MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer);
  return buffer[0];
}
void MPU6050::setIntI2CMasterEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE,
                   MPU6050_INTERRUPT_I2C_MST_INT_BIT, enabled);
}
bool MPU6050::getIntDataReadyEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE,
                  MPU6050_INTERRUPT_DATA_RDY_BIT, buffer);
  return buffer[0];
}
void MPU6050::setIntDataReadyEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE,
                   MPU6050_INTERRUPT_DATA_RDY_BIT, enabled);
}
uint8_t MPU6050::getIntStatus() {
  I2Cdev::readByte(devAddr, MPU6050_RA_INT_STATUS, buffer);
  return buffer[0];
}
bool MPU6050::getIntFreefallStatus() {
  I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FF_BIT,
                  buffer);
  return buffer[0];
}
bool MPU6050::getIntMotionStatus() {
  I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_MOT_BIT,
                  buffer);
  return buffer[0];
}
bool MPU6050::getIntZeroMotionStatus() {
  I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_ZMOT_BIT,
                  buffer);
  return buffer[0];
}
bool MPU6050::getIntFIFOBufferOverflowStatus() {
  I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS,
                  MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer);
  return buffer[0];
}
bool MPU6050::getIntI2CMasterStatus() {
  I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS,
                  MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer);
  return buffer[0];
}
bool MPU6050::getIntDataReadyStatus() {
  I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS,
                  MPU6050_INTERRUPT_DATA_RDY_BIT, buffer);
  return buffer[0];
}
void MPU6050::getMotion9(int16_t *ax, int16_t *ay, int16_t *az, int16_t *gx,
                         int16_t *gy, int16_t *gz, int16_t *mx, int16_t *my,
                         int16_t *mz) {
  getMotion6(ax, ay, az, gx, gy, gz);
}
void MPU6050::getMotion6(int16_t *ax, int16_t *ay, int16_t *az, int16_t *gx,
                         int16_t *gy, int16_t *gz) {
  I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 14, buffer);
  *ax = (((int16_t)buffer[0]) << 8) | buffer[1];
  *ay = (((int16_t)buffer[2]) << 8) | buffer[3];
  *az = (((int16_t)buffer[4]) << 8) | buffer[5];
  *gx = (((int16_t)buffer[8]) << 8) | buffer[9];
  *gy = (((int16_t)buffer[10]) << 8) | buffer[11];
  *gz = (((int16_t)buffer[12]) << 8) | buffer[13];
}
void MPU6050::getAcceleration(int16_t *x, int16_t *y, int16_t *z) {
  I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 6, buffer);
  *x = (((int16_t)buffer[0]) << 8) | buffer[1];
  *y = (((int16_t)buffer[2]) << 8) | buffer[3];
  *z = (((int16_t)buffer[4]) << 8) | buffer[5];
}
int16_t MPU6050::getAccelerationX() {
  I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 2, buffer);
  return (((int16_t)buffer[0]) << 8) | buffer[1];
}
int16_t MPU6050::getAccelerationY() {
  I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_YOUT_H, 2, buffer);
  return (((int16_t)buffer[0]) << 8) | buffer[1];
}
int16_t MPU6050::getAccelerationZ() {
  I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_ZOUT_H, 2, buffer);
  return (((int16_t)buffer[0]) << 8) | buffer[1];
}
int16_t MPU6050::getTemperature() {
  I2Cdev::readBytes(devAddr, MPU6050_RA_TEMP_OUT_H, 2, buffer);
  return (((int16_t)buffer[0]) << 8) | buffer[1];
}
void MPU6050::getRotation(int16_t *x, int16_t *y, int16_t *z) {
  I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 6, buffer);
  *x = (((int16_t)buffer[0]) << 8) | buffer[1];
  *y = (((int16_t)buffer[2]) << 8) | buffer[3];
  *z = (((int16_t)buffer[4]) << 8) | buffer[5];
}
int16_t MPU6050::getRotationX() {
  I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 2, buffer);
  return (((int16_t)buffer[0]) << 8) | buffer[1];
}
int16_t MPU6050::getRotationY() {
  I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_YOUT_H, 2, buffer);
  return (((int16_t)buffer[0]) << 8) | buffer[1];
}
int16_t MPU6050::getRotationZ() {
  I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_ZOUT_H, 2, buffer);
  return (((int16_t)buffer[0]) << 8) | buffer[1];
}
uint8_t MPU6050::getExternalSensorByte(int position) {
  I2Cdev::readByte(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, buffer);
  return buffer[0];
}
uint16_t MPU6050::getExternalSensorWord(int position) {
  I2Cdev::readBytes(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 2, buffer);
  return (((uint16_t)buffer[0]) << 8) | buffer[1];
}
uint32_t MPU6050::getExternalSensorDWord(int position) {
  I2Cdev::readBytes(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 4, buffer);
  return (((uint32_t)buffer[0]) << 24) | (((uint32_t)buffer[1]) << 16) |
         (((uint16_t)buffer[2]) << 8) | buffer[3];
}
bool MPU6050::getXNegMotionDetected() {
  I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS,
                  MPU6050_MOTION_MOT_XNEG_BIT, buffer);
  return buffer[0];
}
bool MPU6050::getXPosMotionDetected() {
  I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS,
                  MPU6050_MOTION_MOT_XPOS_BIT, buffer);
  return buffer[0];
}
bool MPU6050::getYNegMotionDetected() {
  I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS,
                  MPU6050_MOTION_MOT_YNEG_BIT, buffer);
  return buffer[0];
}
bool MPU6050::getYPosMotionDetected() {
  I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS,
                  MPU6050_MOTION_MOT_YPOS_BIT, buffer);
  return buffer[0];
}
bool MPU6050::getZNegMotionDetected() {
  I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS,
                  MPU6050_MOTION_MOT_ZNEG_BIT, buffer);
  return buffer[0];
}
bool MPU6050::getZPosMotionDetected() {
  I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS,
                  MPU6050_MOTION_MOT_ZPOS_BIT, buffer);
  return buffer[0];
}
bool MPU6050::getZeroMotionDetected() {
  I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS,
                  MPU6050_MOTION_MOT_ZRMOT_BIT, buffer);
  return buffer[0];
}
void MPU6050::setSlaveOutputByte(uint8_t num, uint8_t data) {
  if (num > 3)
    return;
  I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_DO + num, data);
}
bool MPU6050::getExternalShadowDelayEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL,
                  MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, buffer);
  return buffer[0];
}
void MPU6050::setExternalShadowDelayEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL,
                   MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, enabled);
}
bool MPU6050::getSlaveDelayEnabled(uint8_t num) {
  if (num > 4)
    return 0;
  I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, buffer);
  return buffer[0];
}
void MPU6050::setSlaveDelayEnabled(uint8_t num, bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, enabled);
}
void MPU6050::resetGyroscopePath() {
  I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET,
                   MPU6050_PATHRESET_GYRO_RESET_BIT, true);
}
void MPU6050::resetAccelerometerPath() {
  I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET,
                   MPU6050_PATHRESET_ACCEL_RESET_BIT, true);
}
void MPU6050::resetTemperaturePath() {
  I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET,
                   MPU6050_PATHRESET_TEMP_RESET_BIT, true);
}
uint8_t MPU6050::getAccelerometerPowerOnDelay() {
  I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL,
                   MPU6050_DETECT_ACCEL_ON_DELAY_BIT,
                   MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, buffer);
  return buffer[0];
}
void MPU6050::setAccelerometerPowerOnDelay(uint8_t delay) {
  I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL,
                    MPU6050_DETECT_ACCEL_ON_DELAY_BIT,
                    MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, delay);
}
uint8_t MPU6050::getFreefallDetectionCounterDecrement() {
  I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL,
                   MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH,
                   buffer);
  return buffer[0];
}
void MPU6050::setFreefallDetectionCounterDecrement(uint8_t decrement) {
  I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL,
                    MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH,
                    decrement);
}
uint8_t MPU6050::getMotionDetectionCounterDecrement() {
  I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL,
                   MPU6050_DETECT_MOT_COUNT_BIT,
                   MPU6050_DETECT_MOT_COUNT_LENGTH, buffer);
  return buffer[0];
}
void MPU6050::setMotionDetectionCounterDecrement(uint8_t decrement) {
  I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL,
                    MPU6050_DETECT_MOT_COUNT_BIT,
                    MPU6050_DETECT_MOT_COUNT_LENGTH, decrement);
}
bool MPU6050::getFIFOEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setFIFOEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT,
                   enabled);
}
bool MPU6050::getI2CMasterModeEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL,
                  MPU6050_USERCTRL_I2C_MST_EN_BIT, buffer);
  return buffer[0];
}
void MPU6050::setI2CMasterModeEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL,
                   MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled);
}
void MPU6050::switchSPIEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL,
                   MPU6050_USERCTRL_I2C_IF_DIS_BIT, enabled);
}
void MPU6050::resetFIFO() {
  I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL,
                   MPU6050_USERCTRL_FIFO_RESET_BIT, true);
}
void MPU6050::resetI2CMaster() {
  I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL,
                   MPU6050_USERCTRL_I2C_MST_RESET_BIT, true);
}
void MPU6050::resetSensors() {
  I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL,
                   MPU6050_USERCTRL_SIG_COND_RESET_BIT, true);
}
void MPU6050::reset() {
  I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1,
                   MPU6050_PWR1_DEVICE_RESET_BIT, true);
}
bool MPU6050::getSleepEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setSleepEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT,
                   enabled);
}
bool MPU6050::getWakeCycleEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setWakeCycleEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT,
                   enabled);
}
bool MPU6050::getTempSensorEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT,
                  buffer);
  return buffer[0] == 0;
}
void MPU6050::setTempSensorEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT,
                   !enabled);
}
uint8_t MPU6050::getClockSource() {
  I2Cdev::readBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT,
                   MPU6050_PWR1_CLKSEL_LENGTH, buffer);
  return buffer[0];
}
void MPU6050::setClockSource(uint8_t source) {
  I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT,
                    MPU6050_PWR1_CLKSEL_LENGTH, source);
}
uint8_t MPU6050::getWakeFrequency() {
  I2Cdev::readBits(devAddr, MPU6050_RA_PWR_MGMT_2,
                   MPU6050_PWR2_LP_WAKE_CTRL_BIT,
                   MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, buffer);
  return buffer[0];
}
void MPU6050::setWakeFrequency(uint8_t frequency) {
  I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_2,
                    MPU6050_PWR2_LP_WAKE_CTRL_BIT,
                    MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, frequency);
}
bool MPU6050::getStandbyXAccelEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setStandbyXAccelEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT,
                   enabled);
}
bool MPU6050::getStandbyYAccelEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setStandbyYAccelEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT,
                   enabled);
}
bool MPU6050::getStandbyZAccelEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setStandbyZAccelEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT,
                   enabled);
}
bool MPU6050::getStandbyXGyroEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setStandbyXGyroEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT,
                   enabled);
}
bool MPU6050::getStandbyYGyroEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setStandbyYGyroEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT,
                   enabled);
}
bool MPU6050::getStandbyZGyroEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setStandbyZGyroEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT,
                   enabled);
}
uint16_t MPU6050::getFIFOCount() {
  I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_COUNTH, 2, buffer);
  return (((uint16_t)buffer[0]) << 8) | buffer[1];
}
uint8_t MPU6050::getFIFOByte() {
  I2Cdev::readByte(devAddr, MPU6050_RA_FIFO_R_W, buffer);
  return buffer[0];
}
void MPU6050::getFIFOBytes(uint8_t *data, uint8_t length) {
  I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_R_W, length, data);
}
void MPU6050::setFIFOByte(uint8_t data) {
  I2Cdev::writeByte(devAddr, MPU6050_RA_FIFO_R_W, data);
}
uint8_t MPU6050::getDeviceID() {
  I2Cdev::readBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT,
                   MPU6050_WHO_AM_I_LENGTH, buffer);
  return buffer[0];
}
void MPU6050::setDeviceID(uint8_t id) {
  I2Cdev::writeBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT,
                    MPU6050_WHO_AM_I_LENGTH, id);
}
uint8_t MPU6050::getOTPBankValid() {
  I2Cdev::readBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setOTPBankValid(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT,
                   enabled);
}
int8_t MPU6050::getXGyroOffsetTC() {
  I2Cdev::readBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT,
                   MPU6050_TC_OFFSET_LENGTH, buffer);
  return buffer[0];
}
void MPU6050::setXGyroOffsetTC(int8_t offset) {
  I2Cdev::writeBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT,
                    MPU6050_TC_OFFSET_LENGTH, offset);
}
int8_t MPU6050::getYGyroOffsetTC() {
  I2Cdev::readBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT,
                   MPU6050_TC_OFFSET_LENGTH, buffer);
  return buffer[0];
}
void MPU6050::setYGyroOffsetTC(int8_t offset) {
  I2Cdev::writeBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT,
                    MPU6050_TC_OFFSET_LENGTH, offset);
}
int8_t MPU6050::getZGyroOffsetTC() {
  I2Cdev::readBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT,
                   MPU6050_TC_OFFSET_LENGTH, buffer);
  return buffer[0];
}
void MPU6050::setZGyroOffsetTC(int8_t offset) {
  I2Cdev::writeBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT,
                    MPU6050_TC_OFFSET_LENGTH, offset);
}
int8_t MPU6050::getXFineGain() {
  I2Cdev::readByte(devAddr, MPU6050_RA_X_FINE_GAIN, buffer);
  return buffer[0];
}
void MPU6050::setXFineGain(int8_t gain) {
  I2Cdev::writeByte(devAddr, MPU6050_RA_X_FINE_GAIN, gain);
}
int8_t MPU6050::getYFineGain() {
  I2Cdev::readByte(devAddr, MPU6050_RA_Y_FINE_GAIN, buffer);
  return buffer[0];
}
void MPU6050::setYFineGain(int8_t gain) {
  I2Cdev::writeByte(devAddr, MPU6050_RA_Y_FINE_GAIN, gain);
}
int8_t MPU6050::getZFineGain() {
  I2Cdev::readByte(devAddr, MPU6050_RA_Z_FINE_GAIN, buffer);
  return buffer[0];
}
void MPU6050::setZFineGain(int8_t gain) {
  I2Cdev::writeByte(devAddr, MPU6050_RA_Z_FINE_GAIN, gain);
}
int16_t MPU6050::getXAccelOffset() {
  I2Cdev::readBytes(devAddr, MPU6050_RA_XA_OFFS_H, 2, buffer);
  return (((int16_t)buffer[0]) << 8) | buffer[1];
}
void MPU6050::setXAccelOffset(int16_t offset) {
  I2Cdev::writeWord(devAddr, MPU6050_RA_XA_OFFS_H, offset);
}
int16_t MPU6050::getYAccelOffset() {
  I2Cdev::readBytes(devAddr, MPU6050_RA_YA_OFFS_H, 2, buffer);
  return (((int16_t)buffer[0]) << 8) | buffer[1];
}
void MPU6050::setYAccelOffset(int16_t offset) {
  I2Cdev::writeWord(devAddr, MPU6050_RA_YA_OFFS_H, offset);
}
int16_t MPU6050::getZAccelOffset() {
  I2Cdev::readBytes(devAddr, MPU6050_RA_ZA_OFFS_H, 2, buffer);
  return (((int16_t)buffer[0]) << 8) | buffer[1];
}
void MPU6050::setZAccelOffset(int16_t offset) {
  I2Cdev::writeWord(devAddr, MPU6050_RA_ZA_OFFS_H, offset);
}
int16_t MPU6050::getXGyroOffset() {
  I2Cdev::readBytes(devAddr, MPU6050_RA_XG_OFFS_USRH, 2, buffer);
  return (((int16_t)buffer[0]) << 8) | buffer[1];
}
void MPU6050::setXGyroOffset(int16_t offset) {
  I2Cdev::writeWord(devAddr, MPU6050_RA_XG_OFFS_USRH, offset);
}
int16_t MPU6050::getYGyroOffset() {
  I2Cdev::readBytes(devAddr, MPU6050_RA_YG_OFFS_USRH, 2, buffer);
  return (((int16_t)buffer[0]) << 8) | buffer[1];
}
void MPU6050::setYGyroOffset(int16_t offset) {
  I2Cdev::writeWord(devAddr, MPU6050_RA_YG_OFFS_USRH, offset);
}
int16_t MPU6050::getZGyroOffset() {
  I2Cdev::readBytes(devAddr, MPU6050_RA_ZG_OFFS_USRH, 2, buffer);
  return (((int16_t)buffer[0]) << 8) | buffer[1];
}
void MPU6050::setZGyroOffset(int16_t offset) {
  I2Cdev::writeWord(devAddr, MPU6050_RA_ZG_OFFS_USRH, offset);
}
bool MPU6050::getIntPLLReadyEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE,
                  MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer);
  return buffer[0];
}
void MPU6050::setIntPLLReadyEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE,
                   MPU6050_INTERRUPT_PLL_RDY_INT_BIT, enabled);
}
bool MPU6050::getIntDMPEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setIntDMPEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE,
                   MPU6050_INTERRUPT_DMP_INT_BIT, enabled);
}
bool MPU6050::getDMPInt5Status() {
  I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_5_BIT,
                  buffer);
  return buffer[0];
}
bool MPU6050::getDMPInt4Status() {
  I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_4_BIT,
                  buffer);
  return buffer[0];
}
bool MPU6050::getDMPInt3Status() {
  I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_3_BIT,
                  buffer);
  return buffer[0];
}
bool MPU6050::getDMPInt2Status() {
  I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_2_BIT,
                  buffer);
  return buffer[0];
}
bool MPU6050::getDMPInt1Status() {
  I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_1_BIT,
                  buffer);
  return buffer[0];
}
bool MPU6050::getDMPInt0Status() {
  I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_0_BIT,
                  buffer);
  return buffer[0];
}
bool MPU6050::getIntPLLReadyStatus() {
  I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS,
                  MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer);
  return buffer[0];
}
bool MPU6050::getIntDMPStatus() {
  I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DMP_INT_BIT,
                  buffer);
  return buffer[0];
}
bool MPU6050::getDMPEnabled() {
  I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT,
                  buffer);
  return buffer[0];
}
void MPU6050::setDMPEnabled(bool enabled) {
  I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT,
                   enabled);
}
void MPU6050::resetDMP() {
  I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL,
                   MPU6050_USERCTRL_DMP_RESET_BIT, true);
}
void MPU6050::setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank) {
  bank &= 0x1F;
  if (userBank)
    bank |= 0x20;
  if (prefetchEnabled)
    bank |= 0x40;
  I2Cdev::writeByte(devAddr, MPU6050_RA_BANK_SEL, bank);
}
void MPU6050::setMemoryStartAddress(uint8_t address) {
  I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_START_ADDR, address);
}
uint8_t MPU6050::readMemoryByte() {
  I2Cdev::readByte(devAddr, MPU6050_RA_MEM_R_W, buffer);
  return buffer[0];
}
void MPU6050::writeMemoryByte(uint8_t data) {
  I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_R_W, data);
}
void MPU6050::readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank,
                              uint8_t address) {
  setMemoryBank(bank);
  setMemoryStartAddress(address);
  uint8_t chunkSize;
  for (uint16_t i = 0; i < dataSize;) {
    chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
    if (i + chunkSize > dataSize)
      chunkSize = dataSize - i;
    if (chunkSize > 256 - address)
      chunkSize = 256 - address;
    I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, data + i);
    i += chunkSize;
    address += chunkSize;
    if (i < dataSize) {
      if (address == 0)
        bank++;
      setMemoryBank(bank);
      setMemoryStartAddress(address);
    }
  }
}
bool MPU6050::writeMemoryBlock(const uint8_t *data, uint16_t dataSize,
                               uint8_t bank, uint8_t address, bool verify,
                               bool useProgMem) {
  setMemoryBank(bank);
  setMemoryStartAddress(address);
  uint8_t chunkSize;
  uint8_t *verifyBuffer;
  uint8_t *progBuffer;
  uint16_t i;
  uint8_t j;
  if (verify)
    verifyBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
  if (useProgMem)
    progBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
  for (i = 0; i < dataSize;) {
    chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
    if (i + chunkSize > dataSize)
      chunkSize = dataSize - i;
    if (chunkSize > 256 - address)
      chunkSize = 256 - address;
    if (useProgMem) {
      for (j = 0; j < chunkSize; j++)
        progBuffer[j] = pgm_read_byte(data + i + j);
    } else {
      progBuffer = (uint8_t *)data + i;
    }
    I2Cdev::writeBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer);
    if (verify && verifyBuffer) {
      setMemoryBank(bank);
      setMemoryStartAddress(address);
      I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer);
      if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
        free(verifyBuffer);
        if (useProgMem)
          free(progBuffer);
        return false;
      }
    }
    i += chunkSize;
    address += chunkSize;
    if (i < dataSize) {
      if (address == 0)
        bank++;
      setMemoryBank(bank);
      setMemoryStartAddress(address);
    }
  }
  if (verify)
    free(verifyBuffer);
  if (useProgMem)
    free(progBuffer);
  return true;
}
bool MPU6050::writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize,
                                   uint8_t bank, uint8_t address, bool verify) {
  return writeMemoryBlock(data, dataSize, bank, address, verify, true);
}
bool MPU6050::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize,
                                       bool useProgMem) {
  uint8_t *progBuffer, success, special;
  uint16_t i, j;
  if (useProgMem) {
    progBuffer = (uint8_t *)malloc(8);
  }
  uint8_t bank, offset, length;
  for (i = 0; i < dataSize;) {
    if (useProgMem) {
      bank = pgm_read_byte(data + i++);
      offset = pgm_read_byte(data + i++);
      length = pgm_read_byte(data + i++);
    } else {
      bank = data[i++];
      offset = data[i++];
      length = data[i++];
    }
    if (length > 0) {
      if (useProgMem) {
        if (sizeof(progBuffer) < length)
          progBuffer = (uint8_t *)realloc(progBuffer, length);
        for (j = 0; j < length; j++)
          progBuffer[j] = pgm_read_byte(data + i + j);
      } else {
        progBuffer = (uint8_t *)data + i;
      }
      success = writeMemoryBlock(progBuffer, length, bank, offset, true);
      i += length;
    } else {
      if (useProgMem) {
        special = pgm_read_byte(data + i++);
      } else {
        special = data[i++];
      }
      if (special == 0x01) {
        I2Cdev::writeByte(devAddr, MPU6050_RA_INT_ENABLE, 0x32);
        success = true;
      } else {
        success = false;
      }
    }
    if (!success) {
      if (useProgMem)
        free(progBuffer);
      return false;
    }
  }
  if (useProgMem)
    free(progBuffer);
  return true;
}
bool MPU6050::writeProgDMPConfigurationSet(const uint8_t *data,
                                           uint16_t dataSize) {
  return writeDMPConfigurationSet(data, dataSize, true);
}
uint8_t MPU6050::getDMPConfig1() {
  I2Cdev::readByte(devAddr, MPU6050_RA_DMP_CFG_1, buffer);
  return buffer[0];
}
void MPU6050::setDMPConfig1(uint8_t config) {
  I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_1, config);
}
uint8_t MPU6050::getDMPConfig2() {
  I2Cdev::readByte(devAddr, MPU6050_RA_DMP_CFG_2, buffer);
  return buffer[0];
}
void MPU6050::setDMPConfig2(uint8_t config) {
  I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_2, config);
}

MPU6050.h :

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

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
#ifndef _MPU6050_H_
#define _MPU6050_H_
#include "I2Cdev.h"
#define MPU6050_ADDRESS_AD0_LOW     0x68
#define MPU6050_ADDRESS_AD0_HIGH    0x69
#define MPU6050_DEFAULT_ADDRESS     MPU6050_ADDRESS_AD0_LOW
#define MPU6050_RA_XG_OFFS_TC       0x00
#define MPU6050_RA_YG_OFFS_TC       0x01
#define MPU6050_RA_ZG_OFFS_TC       0x02
#define MPU6050_RA_X_FINE_GAIN      0x03
#define MPU6050_RA_Y_FINE_GAIN      0x04
#define MPU6050_RA_Z_FINE_GAIN      0x05
#define MPU6050_RA_XA_OFFS_H        0x06
#define MPU6050_RA_XA_OFFS_L_TC     0x07
#define MPU6050_RA_YA_OFFS_H        0x08
#define MPU6050_RA_YA_OFFS_L_TC     0x09
#define MPU6050_RA_ZA_OFFS_H        0x0A
#define MPU6050_RA_ZA_OFFS_L_TC     0x0B
#define MPU6050_RA_XG_OFFS_USRH     0x13
#define MPU6050_RA_XG_OFFS_USRL     0x14
#define MPU6050_RA_YG_OFFS_USRH     0x15
#define MPU6050_RA_YG_OFFS_USRL     0x16
#define MPU6050_RA_ZG_OFFS_USRH     0x17
#define MPU6050_RA_ZG_OFFS_USRL     0x18
#define MPU6050_RA_SMPLRT_DIV       0x19
#define MPU6050_RA_CONFIG           0x1A
#define MPU6050_RA_GYRO_CONFIG      0x1B
#define MPU6050_RA_ACCEL_CONFIG     0x1C
#define MPU6050_RA_FF_THR           0x1D
#define MPU6050_RA_FF_DUR           0x1E
#define MPU6050_RA_MOT_THR          0x1F
#define MPU6050_RA_MOT_DUR          0x20
#define MPU6050_RA_ZRMOT_THR        0x21
#define MPU6050_RA_ZRMOT_DUR        0x22
#define MPU6050_RA_FIFO_EN          0x23
#define MPU6050_RA_I2C_MST_CTRL     0x24
#define MPU6050_RA_I2C_SLV0_ADDR    0x25
#define MPU6050_RA_I2C_SLV0_REG     0x26
#define MPU6050_RA_I2C_SLV0_CTRL    0x27
#define MPU6050_RA_I2C_SLV1_ADDR    0x28
#define MPU6050_RA_I2C_SLV1_REG     0x29
#define MPU6050_RA_I2C_SLV1_CTRL    0x2A
#define MPU6050_RA_I2C_SLV2_ADDR    0x2B
#define MPU6050_RA_I2C_SLV2_REG     0x2C
#define MPU6050_RA_I2C_SLV2_CTRL    0x2D
#define MPU6050_RA_I2C_SLV3_ADDR    0x2E
#define MPU6050_RA_I2C_SLV3_REG     0x2F
#define MPU6050_RA_I2C_SLV3_CTRL    0x30
#define MPU6050_RA_I2C_SLV4_ADDR    0x31
#define MPU6050_RA_I2C_SLV4_REG     0x32
#define MPU6050_RA_I2C_SLV4_DO      0x33
#define MPU6050_RA_I2C_SLV4_CTRL    0x34
#define MPU6050_RA_I2C_SLV4_DI      0x35
#define MPU6050_RA_I2C_MST_STATUS   0x36
#define MPU6050_RA_INT_PIN_CFG      0x37
#define MPU6050_RA_INT_ENABLE       0x38
#define MPU6050_RA_DMP_INT_STATUS   0x39
#define MPU6050_RA_INT_STATUS       0x3A
#define MPU6050_RA_ACCEL_XOUT_H     0x3B
#define MPU6050_RA_ACCEL_XOUT_L     0x3C
#define MPU6050_RA_ACCEL_YOUT_H     0x3D
#define MPU6050_RA_ACCEL_YOUT_L     0x3E
#define MPU6050_RA_ACCEL_ZOUT_H     0x3F
#define MPU6050_RA_ACCEL_ZOUT_L     0x40
#define MPU6050_RA_TEMP_OUT_H       0x41
#define MPU6050_RA_TEMP_OUT_L       0x42
#define MPU6050_RA_GYRO_XOUT_H      0x43
#define MPU6050_RA_GYRO_XOUT_L      0x44
#define MPU6050_RA_GYRO_YOUT_H      0x45
#define MPU6050_RA_GYRO_YOUT_L      0x46
#define MPU6050_RA_GYRO_ZOUT_H      0x47
#define MPU6050_RA_GYRO_ZOUT_L      0x48
#define MPU6050_RA_EXT_SENS_DATA_00 0x49
#define MPU6050_RA_EXT_SENS_DATA_01 0x4A
#define MPU6050_RA_EXT_SENS_DATA_02 0x4B
#define MPU6050_RA_EXT_SENS_DATA_03 0x4C
#define MPU6050_RA_EXT_SENS_DATA_04 0x4D
#define MPU6050_RA_EXT_SENS_DATA_05 0x4E
#define MPU6050_RA_EXT_SENS_DATA_06 0x4F
#define MPU6050_RA_EXT_SENS_DATA_07 0x50
#define MPU6050_RA_EXT_SENS_DATA_08 0x51
#define MPU6050_RA_EXT_SENS_DATA_09 0x52
#define MPU6050_RA_EXT_SENS_DATA_10 0x53
#define MPU6050_RA_EXT_SENS_DATA_11 0x54
#define MPU6050_RA_EXT_SENS_DATA_12 0x55
#define MPU6050_RA_EXT_SENS_DATA_13 0x56
#define MPU6050_RA_EXT_SENS_DATA_14 0x57
#define MPU6050_RA_EXT_SENS_DATA_15 0x58
#define MPU6050_RA_EXT_SENS_DATA_16 0x59
#define MPU6050_RA_EXT_SENS_DATA_17 0x5A
#define MPU6050_RA_EXT_SENS_DATA_18 0x5B
#define MPU6050_RA_EXT_SENS_DATA_19 0x5C
#define MPU6050_RA_EXT_SENS_DATA_20 0x5D
#define MPU6050_RA_EXT_SENS_DATA_21 0x5E
#define MPU6050_RA_EXT_SENS_DATA_22 0x5F
#define MPU6050_RA_EXT_SENS_DATA_23 0x60
#define MPU6050_RA_MOT_DETECT_STATUS    0x61
#define MPU6050_RA_I2C_SLV0_DO      0x63
#define MPU6050_RA_I2C_SLV1_DO      0x64
#define MPU6050_RA_I2C_SLV2_DO      0x65
#define MPU6050_RA_I2C_SLV3_DO      0x66
#define MPU6050_RA_I2C_MST_DELAY_CTRL   0x67
#define MPU6050_RA_SIGNAL_PATH_RESET    0x68
#define MPU6050_RA_MOT_DETECT_CTRL      0x69
#define MPU6050_RA_USER_CTRL        0x6A
#define MPU6050_RA_PWR_MGMT_1       0x6B
#define MPU6050_RA_PWR_MGMT_2       0x6C
#define MPU6050_RA_BANK_SEL         0x6D
#define MPU6050_RA_MEM_START_ADDR   0x6E
#define MPU6050_RA_MEM_R_W          0x6F
#define MPU6050_RA_DMP_CFG_1        0x70
#define MPU6050_RA_DMP_CFG_2        0x71
#define MPU6050_RA_FIFO_COUNTH      0x72
#define MPU6050_RA_FIFO_COUNTL      0x73
#define MPU6050_RA_FIFO_R_W         0x74
#define MPU6050_RA_WHO_AM_I         0x75
#define MPU6050_TC_PWR_MODE_BIT     7
#define MPU6050_TC_OFFSET_BIT       6
#define MPU6050_TC_OFFSET_LENGTH    6
#define MPU6050_TC_OTP_BNK_VLD_BIT  0
#define MPU6050_VDDIO_LEVEL_VLOGIC  0
#define MPU6050_VDDIO_LEVEL_VDD     1
#define MPU6050_CFG_EXT_SYNC_SET_BIT    5
#define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3
#define MPU6050_CFG_DLPF_CFG_BIT    2
#define MPU6050_CFG_DLPF_CFG_LENGTH 3
#define MPU6050_EXT_SYNC_DISABLED       0x0
#define MPU6050_EXT_SYNC_TEMP_OUT_L     0x1
#define MPU6050_EXT_SYNC_GYRO_XOUT_L    0x2
#define MPU6050_EXT_SYNC_GYRO_YOUT_L    0x3
#define MPU6050_EXT_SYNC_GYRO_ZOUT_L    0x4
#define MPU6050_EXT_SYNC_ACCEL_XOUT_L   0x5
#define MPU6050_EXT_SYNC_ACCEL_YOUT_L   0x6
#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L   0x7
#define MPU6050_DLPF_BW_256         0x00
#define MPU6050_DLPF_BW_188         0x01
#define MPU6050_DLPF_BW_98          0x02
#define MPU6050_DLPF_BW_42          0x03
#define MPU6050_DLPF_BW_20          0x04
#define MPU6050_DLPF_BW_10          0x05
#define MPU6050_DLPF_BW_5           0x06
#define MPU6050_GCONFIG_FS_SEL_BIT      4
#define MPU6050_GCONFIG_FS_SEL_LENGTH   2
#define MPU6050_GYRO_FS_250         0x00
#define MPU6050_GYRO_FS_500         0x01
#define MPU6050_GYRO_FS_1000        0x02
#define MPU6050_GYRO_FS_2000        0x03
#define MPU6050_ACONFIG_XA_ST_BIT           7
#define MPU6050_ACONFIG_YA_ST_BIT           6
#define MPU6050_ACONFIG_ZA_ST_BIT           5
#define MPU6050_ACONFIG_AFS_SEL_BIT         4
#define MPU6050_ACONFIG_AFS_SEL_LENGTH      2
#define MPU6050_ACONFIG_ACCEL_HPF_BIT       2
#define MPU6050_ACONFIG_ACCEL_HPF_LENGTH    3
#define MPU6050_ACCEL_FS_2          0x00
#define MPU6050_ACCEL_FS_4          0x01
#define MPU6050_ACCEL_FS_8          0x02
#define MPU6050_ACCEL_FS_16         0x03
#define MPU6050_DHPF_RESET          0x00
#define MPU6050_DHPF_5              0x01
#define MPU6050_DHPF_2P5            0x02
#define MPU6050_DHPF_1P25           0x03
#define MPU6050_DHPF_0P63           0x04
#define MPU6050_DHPF_HOLD           0x07
#define MPU6050_TEMP_FIFO_EN_BIT    7
#define MPU6050_XG_FIFO_EN_BIT      6
#define MPU6050_YG_FIFO_EN_BIT      5
#define MPU6050_ZG_FIFO_EN_BIT      4
#define MPU6050_ACCEL_FIFO_EN_BIT   3
#define MPU6050_SLV2_FIFO_EN_BIT    2
#define MPU6050_SLV1_FIFO_EN_BIT    1
#define MPU6050_SLV0_FIFO_EN_BIT    0
#define MPU6050_MULT_MST_EN_BIT     7
#define MPU6050_WAIT_FOR_ES_BIT     6
#define MPU6050_SLV_3_FIFO_EN_BIT   5
#define MPU6050_I2C_MST_P_NSR_BIT   4
#define MPU6050_I2C_MST_CLK_BIT     3
#define MPU6050_I2C_MST_CLK_LENGTH  4
#define MPU6050_CLOCK_DIV_348       0x0
#define MPU6050_CLOCK_DIV_333       0x1
#define MPU6050_CLOCK_DIV_320       0x2
#define MPU6050_CLOCK_DIV_308       0x3
#define MPU6050_CLOCK_DIV_296       0x4
#define MPU6050_CLOCK_DIV_286       0x5
#define MPU6050_CLOCK_DIV_276       0x6
#define MPU6050_CLOCK_DIV_267       0x7
#define MPU6050_CLOCK_DIV_258       0x8
#define MPU6050_CLOCK_DIV_500       0x9
#define MPU6050_CLOCK_DIV_471       0xA
#define MPU6050_CLOCK_DIV_444       0xB
#define MPU6050_CLOCK_DIV_421       0xC
#define MPU6050_CLOCK_DIV_400       0xD
#define MPU6050_CLOCK_DIV_381       0xE
#define MPU6050_CLOCK_DIV_364       0xF
#define MPU6050_I2C_SLV_RW_BIT      7
#define MPU6050_I2C_SLV_ADDR_BIT    6
#define MPU6050_I2C_SLV_ADDR_LENGTH 7
#define MPU6050_I2C_SLV_EN_BIT      7
#define MPU6050_I2C_SLV_BYTE_SW_BIT 6
#define MPU6050_I2C_SLV_REG_DIS_BIT 5
#define MPU6050_I2C_SLV_GRP_BIT     4
#define MPU6050_I2C_SLV_LEN_BIT     3
#define MPU6050_I2C_SLV_LEN_LENGTH  4
#define MPU6050_I2C_SLV4_RW_BIT         7
#define MPU6050_I2C_SLV4_ADDR_BIT       6
#define MPU6050_I2C_SLV4_ADDR_LENGTH    7
#define MPU6050_I2C_SLV4_EN_BIT         7
#define MPU6050_I2C_SLV4_INT_EN_BIT     6
#define MPU6050_I2C_SLV4_REG_DIS_BIT    5
#define MPU6050_I2C_SLV4_MST_DLY_BIT    4
#define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5
#define MPU6050_MST_PASS_THROUGH_BIT    7
#define MPU6050_MST_I2C_SLV4_DONE_BIT   6
#define MPU6050_MST_I2C_LOST_ARB_BIT    5
#define MPU6050_MST_I2C_SLV4_NACK_BIT   4
#define MPU6050_MST_I2C_SLV3_NACK_BIT   3
#define MPU6050_MST_I2C_SLV2_NACK_BIT   2
#define MPU6050_MST_I2C_SLV1_NACK_BIT   1
#define MPU6050_MST_I2C_SLV0_NACK_BIT   0
#define MPU6050_INTCFG_INT_LEVEL_BIT        7
#define MPU6050_INTCFG_INT_OPEN_BIT         6
#define MPU6050_INTCFG_LATCH_INT_EN_BIT     5
#define MPU6050_INTCFG_INT_RD_CLEAR_BIT     4
#define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT  3
#define MPU6050_INTCFG_FSYNC_INT_EN_BIT     2
#define MPU6050_INTCFG_I2C_BYPASS_EN_BIT    1
#define MPU6050_INTCFG_CLKOUT_EN_BIT        0
#define MPU6050_INTMODE_ACTIVEHIGH  0x00
#define MPU6050_INTMODE_ACTIVELOW   0x01
#define MPU6050_INTDRV_PUSHPULL     0x00
#define MPU6050_INTDRV_OPENDRAIN    0x01
#define MPU6050_INTLATCH_50USPULSE  0x00
#define MPU6050_INTLATCH_WAITCLEAR  0x01
#define MPU6050_INTCLEAR_STATUSREAD 0x00
#define MPU6050_INTCLEAR_ANYREAD    0x01
#define MPU6050_INTERRUPT_FF_BIT            7
#define MPU6050_INTERRUPT_MOT_BIT           6
#define MPU6050_INTERRUPT_ZMOT_BIT          5
#define MPU6050_INTERRUPT_FIFO_OFLOW_BIT    4
#define MPU6050_INTERRUPT_I2C_MST_INT_BIT   3
#define MPU6050_INTERRUPT_PLL_RDY_INT_BIT   2
#define MPU6050_INTERRUPT_DMP_INT_BIT       1
#define MPU6050_INTERRUPT_DATA_RDY_BIT      0
#define MPU6050_DMPINT_5_BIT            5
#define MPU6050_DMPINT_4_BIT            4
#define MPU6050_DMPINT_3_BIT            3
#define MPU6050_DMPINT_2_BIT            2
#define MPU6050_DMPINT_1_BIT            1
#define MPU6050_DMPINT_0_BIT            0
#define MPU6050_MOTION_MOT_XNEG_BIT     7
#define MPU6050_MOTION_MOT_XPOS_BIT     6
#define MPU6050_MOTION_MOT_YNEG_BIT     5
#define MPU6050_MOTION_MOT_YPOS_BIT     4
#define MPU6050_MOTION_MOT_ZNEG_BIT     3
#define MPU6050_MOTION_MOT_ZPOS_BIT     2
#define MPU6050_MOTION_MOT_ZRMOT_BIT    0
#define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT   7
#define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT   4
#define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT   3
#define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT   2
#define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT   1
#define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT   0
#define MPU6050_PATHRESET_GYRO_RESET_BIT    2
#define MPU6050_PATHRESET_ACCEL_RESET_BIT   1
#define MPU6050_PATHRESET_TEMP_RESET_BIT    0
#define MPU6050_DETECT_ACCEL_ON_DELAY_BIT       5
#define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH    2
#define MPU6050_DETECT_FF_COUNT_BIT             3
#define MPU6050_DETECT_FF_COUNT_LENGTH          2
#define MPU6050_DETECT_MOT_COUNT_BIT            1
#define MPU6050_DETECT_MOT_COUNT_LENGTH         2
#define MPU6050_DETECT_DECREMENT_RESET  0x0
#define MPU6050_DETECT_DECREMENT_1      0x1
#define MPU6050_DETECT_DECREMENT_2      0x2
#define MPU6050_DETECT_DECREMENT_4      0x3
#define MPU6050_USERCTRL_DMP_EN_BIT             7
#define MPU6050_USERCTRL_FIFO_EN_BIT            6
#define MPU6050_USERCTRL_I2C_MST_EN_BIT         5
#define MPU6050_USERCTRL_I2C_IF_DIS_BIT         4
#define MPU6050_USERCTRL_DMP_RESET_BIT          3
#define MPU6050_USERCTRL_FIFO_RESET_BIT         2
#define MPU6050_USERCTRL_I2C_MST_RESET_BIT      1
#define MPU6050_USERCTRL_SIG_COND_RESET_BIT     0
#define MPU6050_PWR1_DEVICE_RESET_BIT   7
#define MPU6050_PWR1_SLEEP_BIT          6
#define MPU6050_PWR1_CYCLE_BIT          5
#define MPU6050_PWR1_TEMP_DIS_BIT       3
#define MPU6050_PWR1_CLKSEL_BIT         2
#define MPU6050_PWR1_CLKSEL_LENGTH      3
#define MPU6050_CLOCK_INTERNAL          0x00
#define MPU6050_CLOCK_PLL_XGYRO         0x01
#define MPU6050_CLOCK_PLL_YGYRO         0x02
#define MPU6050_CLOCK_PLL_ZGYRO         0x03
#define MPU6050_CLOCK_PLL_EXT32K        0x04
#define MPU6050_CLOCK_PLL_EXT19M        0x05
#define MPU6050_CLOCK_KEEP_RESET        0x07
#define MPU6050_PWR2_LP_WAKE_CTRL_BIT       7
#define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH    2
#define MPU6050_PWR2_STBY_XA_BIT            5
#define MPU6050_PWR2_STBY_YA_BIT            4
#define MPU6050_PWR2_STBY_ZA_BIT            3
#define MPU6050_PWR2_STBY_XG_BIT            2
#define MPU6050_PWR2_STBY_YG_BIT            1
#define MPU6050_PWR2_STBY_ZG_BIT            0
#define MPU6050_WAKE_FREQ_1P25      0x0
#define MPU6050_WAKE_FREQ_2P5       0x1
#define MPU6050_WAKE_FREQ_5         0x2
#define MPU6050_WAKE_FREQ_10        0x3
#define MPU6050_BANKSEL_PRFTCH_EN_BIT       6
#define MPU6050_BANKSEL_CFG_USER_BANK_BIT   5
#define MPU6050_BANKSEL_MEM_SEL_BIT         4
#define MPU6050_BANKSEL_MEM_SEL_LENGTH      5
#define MPU6050_WHO_AM_I_BIT        6
#define MPU6050_WHO_AM_I_LENGTH     6
#define MPU6050_DMP_MEMORY_BANKS        8
#define MPU6050_DMP_MEMORY_BANK_SIZE    256
#define MPU6050_DMP_MEMORY_CHUNK_SIZE   16
class MPU6050 {
    public:
        MPU6050();
        MPU6050(uint8_t address);
        void initialize();
        bool testConnection();
        uint8_t getAuxVDDIOLevel();
        void setAuxVDDIOLevel(uint8_t level);
        uint8_t getRate();
        void setRate(uint8_t rate);
        uint8_t getExternalFrameSync();
        void setExternalFrameSync(uint8_t sync);
        uint8_t getDLPFMode();
        void setDLPFMode(uint8_t bandwidth);
        uint8_t getFullScaleGyroRange();
        void setFullScaleGyroRange(uint8_t range);
        bool getAccelXSelfTest();
        void setAccelXSelfTest(bool enabled);
        bool getAccelYSelfTest();
        void setAccelYSelfTest(bool enabled);
        bool getAccelZSelfTest();
        void setAccelZSelfTest(bool enabled);
        uint8_t getFullScaleAccelRange();
        void setFullScaleAccelRange(uint8_t range);
        uint8_t getDHPFMode();
        void setDHPFMode(uint8_t mode);
        uint8_t getFreefallDetectionThreshold();
        void setFreefallDetectionThreshold(uint8_t threshold);
        uint8_t getFreefallDetectionDuration();
        void setFreefallDetectionDuration(uint8_t duration);
        uint8_t getMotionDetectionThreshold();
        void setMotionDetectionThreshold(uint8_t threshold);
        uint8_t getMotionDetectionDuration();
        void setMotionDetectionDuration(uint8_t duration);
        uint8_t getZeroMotionDetectionThreshold();
        void setZeroMotionDetectionThreshold(uint8_t threshold);
        uint8_t getZeroMotionDetectionDuration();
        void setZeroMotionDetectionDuration(uint8_t duration);
        bool getTempFIFOEnabled();
        void setTempFIFOEnabled(bool enabled);
        bool getXGyroFIFOEnabled();
        void setXGyroFIFOEnabled(bool enabled);
        bool getYGyroFIFOEnabled();
        void setYGyroFIFOEnabled(bool enabled);
        bool getZGyroFIFOEnabled();
        void setZGyroFIFOEnabled(bool enabled);
        bool getAccelFIFOEnabled();
        void setAccelFIFOEnabled(bool enabled);
        bool getSlave2FIFOEnabled();
        void setSlave2FIFOEnabled(bool enabled);
        bool getSlave1FIFOEnabled();
        void setSlave1FIFOEnabled(bool enabled);
        bool getSlave0FIFOEnabled();
        void setSlave0FIFOEnabled(bool enabled);
        bool getMultiMasterEnabled();
        void setMultiMasterEnabled(bool enabled);
        bool getWaitForExternalSensorEnabled();
        void setWaitForExternalSensorEnabled(bool enabled);
        bool getSlave3FIFOEnabled();
        void setSlave3FIFOEnabled(bool enabled);
        bool getSlaveReadWriteTransitionEnabled();
        void setSlaveReadWriteTransitionEnabled(bool enabled);
        uint8_t getMasterClockSpeed();
        void setMasterClockSpeed(uint8_t speed);
        uint8_t getSlaveAddress(uint8_t num);
        void setSlaveAddress(uint8_t num, uint8_t address);
        uint8_t getSlaveRegister(uint8_t num);
        void setSlaveRegister(uint8_t num, uint8_t reg);
        bool getSlaveEnabled(uint8_t num);
        void setSlaveEnabled(uint8_t num, bool enabled);
        bool getSlaveWordByteSwap(uint8_t num);
        void setSlaveWordByteSwap(uint8_t num, bool enabled);
        bool getSlaveWriteMode(uint8_t num);
        void setSlaveWriteMode(uint8_t num, bool mode);
        bool getSlaveWordGroupOffset(uint8_t num);
        void setSlaveWordGroupOffset(uint8_t num, bool enabled);
        uint8_t getSlaveDataLength(uint8_t num);
        void setSlaveDataLength(uint8_t num, uint8_t length);
        uint8_t getSlave4Address();
        void setSlave4Address(uint8_t address);
        uint8_t getSlave4Register();
        void setSlave4Register(uint8_t reg);
        void setSlave4OutputByte(uint8_t data);
        bool getSlave4Enabled();
        void setSlave4Enabled(bool enabled);
        bool getSlave4InterruptEnabled();
        void setSlave4InterruptEnabled(bool enabled);
        bool getSlave4WriteMode();
        void setSlave4WriteMode(bool mode);
        uint8_t getSlave4MasterDelay();
        void setSlave4MasterDelay(uint8_t delay);
        uint8_t getSlate4InputByte();
        bool getPassthroughStatus();
        bool getSlave4IsDone();
        bool getLostArbitration();
        bool getSlave4Nack();
        bool getSlave3Nack();
        bool getSlave2Nack();
        bool getSlave1Nack();
        bool getSlave0Nack();
        bool getInterruptMode();
        void setInterruptMode(bool mode);
        bool getInterruptDrive();
        void setInterruptDrive(bool drive);
        bool getInterruptLatch();
        void setInterruptLatch(bool latch);
        bool getInterruptLatchClear();
        void setInterruptLatchClear(bool clear);
        bool getFSyncInterruptLevel();
        void setFSyncInterruptLevel(bool level);
        bool getFSyncInterruptEnabled();
        void setFSyncInterruptEnabled(bool enabled);
        bool getI2CBypassEnabled();
        void setI2CBypassEnabled(bool enabled);
        bool getClockOutputEnabled();
        void setClockOutputEnabled(bool enabled);
        uint8_t getIntEnabled();
        void setIntEnabled(uint8_t enabled);
        bool getIntFreefallEnabled();
        void setIntFreefallEnabled(bool enabled);
        bool getIntMotionEnabled();
        void setIntMotionEnabled(bool enabled);
        bool getIntZeroMotionEnabled();
        void setIntZeroMotionEnabled(bool enabled);
        bool getIntFIFOBufferOverflowEnabled();
        void setIntFIFOBufferOverflowEnabled(bool enabled);
        bool getIntI2CMasterEnabled();
        void setIntI2CMasterEnabled(bool enabled);
        bool getIntDataReadyEnabled();
        void setIntDataReadyEnabled(bool enabled);
        uint8_t getIntStatus();
        bool getIntFreefallStatus();
        bool getIntMotionStatus();
        bool getIntZeroMotionStatus();
        bool getIntFIFOBufferOverflowStatus();
        bool getIntI2CMasterStatus();
        bool getIntDataReadyStatus();
        void getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz);
        void getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz);
        void getAcceleration(int16_t* x, int16_t* y, int16_t* z);
        int16_t getAccelerationX();
        int16_t getAccelerationY();
        int16_t getAccelerationZ();
        int16_t getTemperature();
        void getRotation(int16_t* x, int16_t* y, int16_t* z);
        int16_t getRotationX();
        int16_t getRotationY();
        int16_t getRotationZ();
        uint8_t getExternalSensorByte(int position);
        uint16_t getExternalSensorWord(int position);
        uint32_t getExternalSensorDWord(int position);
        bool getXNegMotionDetected();
        bool getXPosMotionDetected();
        bool getYNegMotionDetected();
        bool getYPosMotionDetected();
        bool getZNegMotionDetected();
        bool getZPosMotionDetected();
        bool getZeroMotionDetected();
        void setSlaveOutputByte(uint8_t num, uint8_t data);
        bool getExternalShadowDelayEnabled();
        void setExternalShadowDelayEnabled(bool enabled);
        bool getSlaveDelayEnabled(uint8_t num);
        void setSlaveDelayEnabled(uint8_t num, bool enabled);
        void resetGyroscopePath();
        void resetAccelerometerPath();
        void resetTemperaturePath();
        uint8_t getAccelerometerPowerOnDelay();
        void setAccelerometerPowerOnDelay(uint8_t delay);
        uint8_t getFreefallDetectionCounterDecrement();
        void setFreefallDetectionCounterDecrement(uint8_t decrement);
        uint8_t getMotionDetectionCounterDecrement();
        void setMotionDetectionCounterDecrement(uint8_t decrement);
        bool getFIFOEnabled();
        void setFIFOEnabled(bool enabled);
        bool getI2CMasterModeEnabled();
        void setI2CMasterModeEnabled(bool enabled);
        void switchSPIEnabled(bool enabled);
        void resetFIFO();
        void resetI2CMaster();
        void resetSensors();
        void reset();
        bool getSleepEnabled();
        void setSleepEnabled(bool enabled);
        bool getWakeCycleEnabled();
        void setWakeCycleEnabled(bool enabled);
        bool getTempSensorEnabled();
        void setTempSensorEnabled(bool enabled);
        uint8_t getClockSource();
        void setClockSource(uint8_t source);
        uint8_t getWakeFrequency();
        void setWakeFrequency(uint8_t frequency);
        bool getStandbyXAccelEnabled();
        void setStandbyXAccelEnabled(bool enabled);
        bool getStandbyYAccelEnabled();
        void setStandbyYAccelEnabled(bool enabled);
        bool getStandbyZAccelEnabled();
        void setStandbyZAccelEnabled(bool enabled);
        bool getStandbyXGyroEnabled();
        void setStandbyXGyroEnabled(bool enabled);
        bool getStandbyYGyroEnabled();
        void setStandbyYGyroEnabled(bool enabled);
        bool getStandbyZGyroEnabled();
        void setStandbyZGyroEnabled(bool enabled);
        uint16_t getFIFOCount();
        uint8_t getFIFOByte();
        void setFIFOByte(uint8_t data);
        void getFIFOBytes(uint8_t *data, uint8_t length);
        uint8_t getDeviceID();
        void setDeviceID(uint8_t id);
        uint8_t getOTPBankValid();
        void setOTPBankValid(bool enabled);
        int8_t getXGyroOffsetTC();
        void setXGyroOffsetTC(int8_t offset);
        int8_t getYGyroOffsetTC();
        void setYGyroOffsetTC(int8_t offset);
        int8_t getZGyroOffsetTC();
        void setZGyroOffsetTC(int8_t offset);
        int8_t getXFineGain();
        void setXFineGain(int8_t gain);
        int8_t getYFineGain();
        void setYFineGain(int8_t gain);
        int8_t getZFineGain();
        void setZFineGain(int8_t gain);
        int16_t getXAccelOffset();
        void setXAccelOffset(int16_t offset);
        int16_t getYAccelOffset();
        void setYAccelOffset(int16_t offset);
        int16_t getZAccelOffset();
        void setZAccelOffset(int16_t offset);
        int16_t getXGyroOffset();
        void setXGyroOffset(int16_t offset);
        int16_t getYGyroOffset();
        void setYGyroOffset(int16_t offset);
        int16_t getZGyroOffset();
        void setZGyroOffset(int16_t offset);
        bool getIntPLLReadyEnabled();
        void setIntPLLReadyEnabled(bool enabled);
        bool getIntDMPEnabled();
        void setIntDMPEnabled(bool enabled);
        bool getDMPInt5Status();
        bool getDMPInt4Status();
        bool getDMPInt3Status();
        bool getDMPInt2Status();
        bool getDMPInt1Status();
        bool getDMPInt0Status();
        bool getIntPLLReadyStatus();
        bool getIntDMPStatus();
        bool getDMPEnabled();
        void setDMPEnabled(bool enabled);
        void resetDMP();
        void setMemoryBank(uint8_t bank, bool prefetchEnabled=false, bool userBank=false);
        void setMemoryStartAddress(uint8_t address);
        uint8_t readMemoryByte();
        void writeMemoryByte(uint8_t data);
        void readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0);
        bool writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool verify=true, bool useProgMem=false);
        bool writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool verify=true);
        bool writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem=false);
        bool writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize);
        uint8_t getDMPConfig1();
        void setDMPConfig1(uint8_t config);
        uint8_t getDMPConfig2();
        void setDMPConfig2(uint8_t config);
        #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS20
            uint8_t *dmpPacketBuffer;
            uint16_t dmpPacketSize;
            uint8_t dmpInitialize();
            bool dmpPacketAvailable();
            uint8_t dmpSetFIFORate(uint8_t fifoRate);
            uint8_t dmpGetFIFORate();
            uint8_t dmpGetSampleStepSizeMS();
            uint8_t dmpGetSampleFrequency();
            int32_t dmpDecodeTemperature(int8_t tempReg);
            uint8_t dmpRunFIFORateProcesses();
            uint8_t dmpSendQuaternion(uint_fast16_t accuracy);
            uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
            uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
            uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
            uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
            uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
            uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
            uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
            uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
            uint8_t dmpSendPacketNumber(uint_fast16_t accuracy);
            uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
            uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
            uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0);
            uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0);
            uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0);
            uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0);
            uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0);
            uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0);
            uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0);
            uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0);
            uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0);
            uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0);
            uint8_t dmpSetLinearAccelFilterCoefficient(float coef);
            uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0);
            uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0);
            uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity);
            uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0);
            uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0);
            uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q);
            uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0);
            uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0);
            uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0);
            uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0);
            uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0);
            uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0);
            uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q);
            uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0);
            uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
            uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0);
            uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
            uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0);
            uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetEuler(float *data, Quaternion *q);
            uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity);
            uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0);
            uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0);
            uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData);
            uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL);
            uint8_t dmpSetFIFOProcessedCallback(void (*func) (void));
            uint8_t dmpInitFIFOParam();
            uint8_t dmpCloseFIFO();
            uint8_t dmpSetGyroDataSource(uint8_t source);
            uint8_t dmpDecodeQuantizedAccel();
            uint32_t dmpGetGyroSumOfSquare();
            uint32_t dmpGetAccelSumOfSquare();
            void dmpOverrideQuaternion(long *q);
            uint16_t dmpGetFIFOPacketSize();
        #endif
        #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS41
            uint8_t *dmpPacketBuffer;
            uint16_t dmpPacketSize;
            uint8_t dmpInitialize();
            bool dmpPacketAvailable();
            uint8_t dmpSetFIFORate(uint8_t fifoRate);
            uint8_t dmpGetFIFORate();
            uint8_t dmpGetSampleStepSizeMS();
            uint8_t dmpGetSampleFrequency();
            int32_t dmpDecodeTemperature(int8_t tempReg);
            uint8_t dmpRunFIFORateProcesses();
            uint8_t dmpSendQuaternion(uint_fast16_t accuracy);
            uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
            uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
            uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
            uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
            uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
            uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
            uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
            uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
            uint8_t dmpSendPacketNumber(uint_fast16_t accuracy);
            uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
            uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
            uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0);
            uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0);
            uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0);
            uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0);
            uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0);
            uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0);
            uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0);
            uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0);
            uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0);
            uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0);
            uint8_t dmpGetMag(int16_t *data, const uint8_t* packet=0);
            uint8_t dmpSetLinearAccelFilterCoefficient(float coef);
            uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0);
            uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0);
            uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity);
            uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0);
            uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0);
            uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q);
            uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0);
            uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0);
            uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0);
            uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0);
            uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0);
            uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0);
            uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q);
            uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0);
            uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
            uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0);
            uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
            uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0);
            uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0);
            uint8_t dmpGetEuler(float *data, Quaternion *q);
            uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity);
            uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0);
            uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0);
            uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData);
            uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL);
            uint8_t dmpSetFIFOProcessedCallback(void (*func) (void));
            uint8_t dmpInitFIFOParam();
            uint8_t dmpCloseFIFO();
            uint8_t dmpSetGyroDataSource(uint8_t source);
            uint8_t dmpDecodeQuantizedAccel();
            uint32_t dmpGetGyroSumOfSquare();
            uint32_t dmpGetAccelSumOfSquare();
            void dmpOverrideQuaternion(long *q);
            uint16_t dmpGetFIFOPacketSize();
        #endif
    private:
        uint8_t devAddr;
        uint8_t buffer[14];
};
#endif /* _MPU6050_H_ */

MsTimer2.cpp :

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

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
#include "MsTimer2.h"
 
unsigned long MsTimer2::msecs;
void (*MsTimer2::func)();
volatile unsigned long MsTimer2::count;
volatile char MsTimer2::overflowing;
volatile unsigned int MsTimer2::tcnt2;
 
void MsTimer2::set(unsigned long ms, void (*f)()) {
	float prescaler = 0.0;
 
#if defined (__AVR_ATmega168__) || defined (__AVR_ATmega48__) || defined (__AVR_ATmega88__) || defined (__AVR_ATmega328P__) || (__AVR_ATmega1280__)
	TIMSK2 &= ~(1<<TOIE2);
	TCCR2A &= ~((1<<WGM21) | (1<<WGM20));
	TCCR2B &= ~(1<<WGM22);
	ASSR &= ~(1<<AS2);
	TIMSK2 &= ~(1<<OCIE2A);
 
	if ((F_CPU >= 1000000UL) && (F_CPU <= 16000000UL)) {
		TCCR2B |= (1<<CS22);
		TCCR2B &= ~((1<<CS21) | (1<<CS20));
		prescaler = 64.0;
	} else if (F_CPU < 1000000UL) {
		TCCR2B |= (1<<CS21);
		TCCR2B &= ~((1<<CS22) | (1<<CS20));
		prescaler = 8.0;
	} else { 		TCCR2B |= ((1<<CS22) | (1<<CS20));
		TCCR2B &= ~(1<<CS21);
		prescaler = 128.0;
	}
#elif defined (__AVR_ATmega8__)
	TIMSK &= ~(1<<TOIE2);
	TCCR2 &= ~((1<<WGM21) | (1<<WGM20));
	TIMSK &= ~(1<<OCIE2);
	ASSR &= ~(1<<AS2);
 
	if ((F_CPU >= 1000000UL) && (F_CPU <= 16000000UL)) {
		TCCR2 |= (1<<CS22);
		TCCR2 &= ~((1<<CS21) | (1<<CS20));
		prescaler = 64.0;
	} else if (F_CPU < 1000000UL) {
		TCCR2 |= (1<<CS21);
		TCCR2 &= ~((1<<CS22) | (1<<CS20));
		prescaler = 8.0;
	} else {
		TCCR2 |= ((1<<CS22) && (1<<CS20));
		TCCR2 &= ~(1<<CS21);
		prescaler = 128.0;
	}
#elif defined (__AVR_ATmega128__)
	TIMSK &= ~(1<<TOIE2);
	TCCR2 &= ~((1<<WGM21) | (1<<WGM20));
	TIMSK &= ~(1<<OCIE2);
 
	if ((F_CPU >= 1000000UL) && (F_CPU <= 16000000UL)) {
		TCCR2 |= ((1<<CS21) | (1<<CS20));
		TCCR2 &= ~(1<<CS22);
		prescaler = 64.0;
	} else if (F_CPU < 1000000UL) {
		TCCR2 |= (1<<CS21);
		TCCR2 &= ~((1<<CS22) | (1<<CS20));
		prescaler = 8.0;
	} else {
		TCCR2 |= (1<<CS22);
		TCCR2 &= ~((1<<CS21) | (1<<CS20));
		prescaler = 256.0;
	}
#endif
 
	tcnt2 = 256 - (int)((float)F_CPU * 0.001 / prescaler);
 
	if (ms == 0)
		msecs = 1;
	else
		msecs = ms;
 
	func = f;
}
 
void MsTimer2::start() {
	count = 0;
	overflowing = 0;
#if defined (__AVR_ATmega168__) || defined (__AVR_ATmega48__) || defined (__AVR_ATmega88__) || defined (__AVR_ATmega328P__) || (__AVR_ATmega1280__)
	TCNT2 = tcnt2;
	TIMSK2 |= (1<<TOIE2);
#elif defined (__AVR_ATmega128__)
	TCNT2 = tcnt2;
	TIMSK |= (1<<TOIE2);
#elif defined (__AVR_ATmega8__)
	TCNT2 = tcnt2;
	TIMSK |= (1<<TOIE2);
#endif
}
 
void MsTimer2::stop() {
#if defined (__AVR_ATmega168__) || defined (__AVR_ATmega48__) || defined (__AVR_ATmega88__) || defined (__AVR_ATmega328P__) || (__AVR_ATmega1280__)
	TIMSK2 &= ~(1<<TOIE2);
#elif defined (__AVR_ATmega128__)
	TIMSK &= ~(1<<TOIE2);
#elif defined (__AVR_ATmega8__)
	TIMSK &= ~(1<<TOIE2);
#endif
}
 
void MsTimer2::_overflow() {
	count += 1;
 
	if (count >= msecs && !overflowing) {
		overflowing = 1;
		count = 0;
		(*func)();
		overflowing = 0;
	}
}
 
ISR(TIMER2_OVF_vect) {
#if defined (__AVR_ATmega168__) || defined (__AVR_ATmega48__) || defined (__AVR_ATmega88__) || defined (__AVR_ATmega328P__) || (__AVR_ATmega1280__)
	TCNT2 = MsTimer2::tcnt2;
#elif defined (__AVR_ATmega128__)
	TCNT2 = MsTimer2::tcnt2;
#elif defined (__AVR_ATmega8__)
	TCNT2 = MsTimer2::tcnt2;
#endif
	MsTimer2::_overflow();
}

Invité · 02/03/2023, 20h06

MsTimer2.h :

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
#ifndef MsTimer2_h
#define MsTimer2_h
 
#include <avr/interrupt.h>
 
namespace MsTimer2 {
	extern unsigned long msecs;
	extern void (*func)();
	extern volatile unsigned long count;
	extern volatile char overflowing;
	extern volatile unsigned int tcnt2;
 
	void set(unsigned long ms, void (*f)());
	void start();
	void stop();
	void _overflow();
}
 
#endif

PinChangeInt.h :

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

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
#define PCINT_VERSION 2402
#define	detachPinChangeInterrupt(pin)				PCintPort::detachInterrupt(pin)
#define	attachPinChangeInterrupt(pin,userFunc,mode)	PCintPort::attachInterrupt(pin, &userFunc,mode)
#define getInterruptedPin()							PCintPort::getArduinoPin()
#ifndef PinChangeInt_h
#define	PinChangeInt_h
#include "stddef.h"
#include <Arduino.h>
#include <new.h>
#include <wiring_private.h>
#undef DEBUG
#undef	INLINE_PCINT
#define INLINE_PCINT
#if defined __AVR_ATmega2560__ || defined __AVR_ATmega1280__ || defined __AVR_ATmega1281__ || defined __AVR_ATmega2561__ || defined __AVR_ATmega640__
	#define __USE_PORT_JK
	#define NO_PORTA_PINCHANGES
	#define NO_PORTC_PINCHANGES
	#define NO_PORTD_PINCHANGES
	#if ((defined(NO_PORTB_PINCHANGES) && defined(NO_PORTJ_PINCHANGES)) || \
			(defined(NO_PORTJ_PINCHANGES) && defined(NO_PORTK_PINCHANGES)) || \
			(defined(NO_PORTK_PINCHANGES) && defined(NO_PORTB_PINCHANGES)))
		#define	INLINE_PCINT inline
	#endif
#else
	#define NO_PORTJ_PINCHANGES
	#define NO_PORTK_PINCHANGES
	#if defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__)
		#ifndef NO_PORTA_PINCHANGES
			#define __USE_PORT_A
		#endif
	#else
		#define NO_PORTA_PINCHANGES
	#endif
	#if (   (defined(NO_PORTA_PINCHANGES) && defined(NO_PORTB_PINCHANGES) && defined(NO_PORTC_PINCHANGES)) || \
			(defined(NO_PORTA_PINCHANGES) && defined(NO_PORTB_PINCHANGES) && defined(NO_PORTD_PINCHANGES)) || \
			(defined(NO_PORTA_PINCHANGES) && defined(NO_PORTC_PINCHANGES) && defined(NO_PORTD_PINCHANGES)) || \
			(defined(NO_PORTB_PINCHANGES) && defined(NO_PORTC_PINCHANGES) && defined(NO_PORTD_PINCHANGES)) )
		#define	INLINE_PCINT inline
	#endif
#endif
#define	PCdetachInterrupt(pin)	PCintPort::detachInterrupt(pin)
#define	PCattachInterrupt(pin,userFunc,mode) PCintPort::attachInterrupt(pin, userFunc,mode)
#define PCgetArduinoPin() PCintPort::getArduinoPin()
typedef void (*PCIntvoidFuncPtr)(void);
class PCintPort {
public:
	PCintPort(int index,int pcindex, volatile uint8_t& maskReg) :
	portInputReg(*portInputRegister(index)),
	portPCMask(maskReg),
	PCICRbit(1 << pcindex),
	portRisingPins(0),
	portFallingPins(0),
	firstPin(NULL)
#ifdef PINMODE
	,intrCount(0)
#endif
	{
		#ifdef FLASH
		ledsetup();
		#endif
	}
	volatile	uint8_t&		portInputReg;
	static		int8_t attachInterrupt(uint8_t pin, PCIntvoidFuncPtr userFunc, int mode);
	static		void detachInterrupt(uint8_t pin);
	INLINE_PCINT void PCint();
	static volatile uint8_t curr;
	#ifndef NO_PIN_NUMBER
	static	volatile uint8_t	arduinoPin;
	#endif
	#ifndef NO_PIN_STATE
	static volatile	uint8_t	pinState;
	#endif
	#ifdef PINMODE
	static volatile uint8_t pinmode;
	static volatile uint8_t s_portRisingPins;
	static volatile uint8_t s_portFallingPins;
	static volatile uint8_t s_lastPinView;
	static volatile uint8_t s_pmask;
	static volatile char s_PORT;
	static volatile uint8_t s_changedPins;
	static volatile uint8_t s_portRisingPins_nCurr;
	static volatile uint8_t s_portFallingPins_nNCurr;
	static volatile uint8_t s_currXORlastPinView;
	volatile uint8_t intrCount;
	static volatile uint8_t s_count;
	static volatile uint8_t pcint_multi;
	static volatile uint8_t PCIFRbug;
	#endif
	#ifdef FLASH
	static void ledsetup(void);
	#endif
protected:
	class PCintPin {
	public:
		PCintPin() :
		PCintFunc((PCIntvoidFuncPtr)NULL),
		mode(0) {}
		PCIntvoidFuncPtr PCintFunc;
		uint8_t 	mode;
		uint8_t		mask;
		uint8_t arduinoPin;
		PCintPin* next;
	};
	void 		enable(PCintPin* pin, PCIntvoidFuncPtr userFunc, uint8_t mode);
	int8_t		addPin(uint8_t arduinoPin,PCIntvoidFuncPtr userFunc, uint8_t mode);
	volatile	uint8_t&		portPCMask;
	const		uint8_t			PCICRbit;
	volatile	uint8_t			portRisingPins;
	volatile	uint8_t			portFallingPins;
	volatile uint8_t		lastPinView;
	PCintPin*	firstPin;
};
#ifndef LIBCALL_PINCHANGEINT
volatile uint8_t PCintPort::curr=0;
#ifndef NO_PIN_NUMBER
volatile uint8_t PCintPort::arduinoPin=0;
#endif
#ifndef NO_PIN_STATE
volatile uint8_t PCintPort::pinState=0;
#endif
#ifdef PINMODE
volatile uint8_t PCintPort::pinmode=0;
volatile uint8_t PCintPort::s_portRisingPins=0;
volatile uint8_t PCintPort::s_portFallingPins=0;
volatile uint8_t PCintPort::s_lastPinView=0;
volatile uint8_t PCintPort::s_pmask=0;
volatile char	 PCintPort::s_PORT='x';
volatile uint8_t PCintPort::s_changedPins=0;
volatile uint8_t PCintPort::s_portRisingPins_nCurr=0;
volatile uint8_t PCintPort::s_portFallingPins_nNCurr=0;
volatile uint8_t PCintPort::s_currXORlastPinView=0;
volatile uint8_t PCintPort::s_count=0;
volatile uint8_t PCintPort::pcint_multi=0;
volatile uint8_t PCintPort::PCIFRbug=0;
#endif
#ifdef FLASH
#define PINLED 13
volatile uint8_t *led_port;
uint8_t led_mask;
uint8_t not_led_mask;
boolean ledsetup_run=false;
void PCintPort::ledsetup(void) {
	if (! ledsetup_run) {
		led_port=portOutputRegister(digitalPinToPort(PINLED));
		led_mask=digitalPinToBitMask(PINLED);
		not_led_mask=led_mask^0xFF;
		pinMode(PINLED, OUTPUT); digitalWrite(PINLED, LOW);
		ledsetup_run=true;
	}
};
#endif
#if defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) 
#ifndef NO_PORTA_PINCHANGES
PCintPort portA=PCintPort(1, 0,PCMSK0);
#endif
#ifndef NO_PORTB_PINCHANGES
PCintPort portB=PCintPort(2, 1,PCMSK1);
#endif
#ifndef NO_PORTC_PINCHANGES
PCintPort portC=PCintPort(3, 2,PCMSK2);
#endif
#ifndef NO_PORTD_PINCHANGES
PCintPort portD=PCintPort(4, 3,PCMSK3);
#endif
#else
#ifndef NO_PORTB_PINCHANGES
PCintPort portB=PCintPort(2, 0,PCMSK0);
#endif
#ifndef NO_PORTC_PINCHANGES
PCintPort portC=PCintPort(3, 1,PCMSK1);
#endif
#ifndef NO_PORTD_PINCHANGES
PCintPort portD=PCintPort(4, 2,PCMSK2);
#endif
#endif
#ifdef __USE_PORT_JK
#ifndef NO_PORTJ_PINCHANGES
PCintPort portJ=PCintPort(10,1,PCMSK1);
#endif
#ifndef NO_PORTK_PINCHANGES
PCintPort portK=PCintPort(11,2,PCMSK2);
#endif
#endif
static PCintPort *lookupPortNumToPort( int portNum ) {
    PCintPort *port = NULL;
	switch (portNum) {
#ifndef NO_PORTA_PINCHANGES
	case 1:
		port=&portA;
		break;
#endif
#ifndef NO_PORTB_PINCHANGES
	case 2:
		port=&portB;
		break;
#endif
#ifndef NO_PORTC_PINCHANGES
	case 3:
		port=&portC;
		break;
#endif
#ifndef NO_PORTD_PINCHANGES
	case 4:
		port=&portD;
		break;
#endif
#ifdef __USE_PORT_JK
#ifndef NO_PORTJ_PINCHANGES
	case 10:
		port=&portJ;
		break;
#endif
#ifndef NO_PORTK_PINCHANGES
	case 11:
		port=&portK;
		break;
#endif
#endif
    }
    return port;
}
void PCintPort::enable(PCintPin* p, PCIntvoidFuncPtr userFunc, uint8_t mode) {
	p->mode=mode;
	p->PCintFunc=userFunc;
#ifndef NO_PORTJ_PINCHANGES
	if ((p->arduinoPin == 14) || (p->arduinoPin == 15)) {
		portPCMask |= (p->mask << 1);
	}
	else {
		portPCMask |= p->mask;
	}
#else
    portPCMask |= p->mask;
#endif
	if ((p->mode == RISING) || (p->mode == CHANGE)) portRisingPins |= p->mask;
	if ((p->mode == FALLING) || (p->mode == CHANGE)) portFallingPins |= p->mask;
	PCICR |= PCICRbit;
}
int8_t PCintPort::addPin(uint8_t arduinoPin, PCIntvoidFuncPtr userFunc, uint8_t mode)
{
	PCintPin* tmp;
	tmp=firstPin;
	if (firstPin != NULL) {
		do {
			if (tmp->arduinoPin == arduinoPin) { enable(tmp, userFunc, mode); return(0); }
			if (tmp->next == NULL) break;
			tmp=tmp->next;
		} while (true);
	}
	PCintPin* p=new PCintPin;
	if (p == NULL) return(-1);
	p->arduinoPin=arduinoPin;
	p->mode = mode;
	p->next=NULL;
	p->mask = digitalPinToBitMask(arduinoPin);
	if (firstPin == NULL) firstPin=p;
	else tmp->next=p;
#ifdef DEBUG
	Serial.print("addPin. pin given: "); Serial.print(arduinoPin, DEC);
	int addr = (int) p;
	Serial.print(" instance addr: "); Serial.println(addr, HEX);
	Serial.print("userFunc addr: "); Serial.println((int)p->PCintFunc, HEX);
#endif
	enable(p, userFunc, mode);
#ifdef DEBUG
	Serial.print("addPin. pin given: "); Serial.print(arduinoPin, DEC), Serial.print (" pin stored: ");
	int addr = (int) p;
	Serial.print(" instance addr: "); Serial.println(addr, HEX);
#endif
	return(1);
}
int8_t PCintPort::attachInterrupt(uint8_t arduinoPin, PCIntvoidFuncPtr userFunc, int mode)
{
	PCintPort *port;
	uint8_t portNum = digitalPinToPort(arduinoPin);
	if ((portNum == NOT_A_PORT) || (userFunc == NULL)) return(-1);
	port=lookupPortNumToPort(portNum);
	port->lastPinView=port->portInputReg;
#ifdef DEBUG
	Serial.print("attachInterrupt- pin: "); Serial.println(arduinoPin, DEC);
#endif
	return(port->addPin(arduinoPin,userFunc,mode));
}
void PCintPort::detachInterrupt(uint8_t arduinoPin)
{
	PCintPort *port;
	PCintPin* current;
	uint8_t mask;
	uint8_t portNum = digitalPinToPort(arduinoPin);
	if (portNum == NOT_A_PORT) return;
	port=lookupPortNumToPort(portNum);
	mask=digitalPinToBitMask(arduinoPin);
	current=port->firstPin;
	while (current) {
		if (current->mask == mask) {
			uint8_t oldSREG = SREG;
			cli();
#ifndef NO_PORTJ_PINCHANGES
			if ((arduinoPin == 14) || (arduinoPin == 15)) {
				port->portPCMask &= ~(mask << 1);
			}
			else {
				port->portPCMask &= ~mask;
			}
#else
			port->portPCMask &= ~mask;
#endif
			if (port->portPCMask == 0) PCICR &= ~(port->PCICRbit);
			port->portRisingPins &= ~current->mask; port->portFallingPins &= ~current->mask;
			SREG = oldSREG; 
			return;
		}
		current=current->next;
	}
}
void PCintPort::PCint() {
	#ifdef FLASH
	if (*led_port & led_mask) *led_port&=not_led_mask;
	else *led_port|=led_mask;
    #endif
	#ifndef DISABLE_PCINT_MULTI_SERVICE
	uint8_t pcifr;
	while (true) {
	#endif
		#ifdef PINMODE
		PCintPort::s_lastPinView=lastPinView;
		intrCount++;
		PCintPort::s_count=intrCount;
		#endif
		uint8_t changedPins = (PCintPort::curr ^ lastPinView) &
							  ((portRisingPins & PCintPort::curr ) | ( portFallingPins & ~PCintPort::curr ));
		#ifdef PINMODE
		PCintPort::s_currXORlastPinView=PCintPort::curr ^ lastPinView;
		PCintPort::s_portRisingPins_nCurr=portRisingPins & PCintPort::curr;
		PCintPort::s_portFallingPins_nNCurr=portFallingPins & ~PCintPort::curr;
		#endif
		lastPinView = PCintPort::curr;
		PCintPin* p = firstPin;
		while (p) {
			if (p->mask & changedPins) {
				#ifndef NO_PIN_STATE
				PCintPort::pinState=PCintPort::curr & p->mask ? HIGH : LOW;
				#endif
				#ifndef NO_PIN_NUMBER
				PCintPort::arduinoPin=p->arduinoPin;
				#endif
				#ifdef PINMODE
				PCintPort::pinmode=p->mode;
				PCintPort::s_portRisingPins=portRisingPins;
				PCintPort::s_portFallingPins=portFallingPins;
				PCintPort::s_pmask=p->mask;
				PCintPort::s_changedPins=changedPins;
				#endif
				p->PCintFunc();
			}
			p=p->next;
		}
	#ifndef DISABLE_PCINT_MULTI_SERVICE
		pcifr = PCIFR & PCICRbit;
		if (pcifr == 0) break;
		PCIFR |= PCICRbit;
		#ifdef PINMODE
		PCintPort::pcint_multi++;
		if (PCIFR & PCICRbit) PCintPort::PCIFRbug=1;
		#endif
		PCintPort::curr=portInputReg;
	}
	#endif
}
#ifndef NO_PORTA_PINCHANGES
ISR(PCINT0_vect) {
	#ifdef PINMODE
	PCintPort::s_PORT='A';
	#endif
	PCintPort::curr = portA.portInputReg;
	portA.PCint();
}
#define PORTBVECT PCINT1_vect
#define PORTCVECT PCINT2_vect
#define PORTDVECT PCINT3_vect
#else
#define PORTBVECT PCINT0_vect
#define PORTCVECT PCINT1_vect
#define PORTDVECT PCINT2_vect
#endif
#ifndef NO_PORTB_PINCHANGES
ISR(PORTBVECT) {
	#ifdef PINMODE
	PCintPort::s_PORT='B';
	#endif
	PCintPort::curr = portB.portInputReg;
	portB.PCint();
}
#endif
#ifndef NO_PORTC_PINCHANGES
ISR(PORTCVECT) {
	#ifdef PINMODE
	PCintPort::s_PORT='C';
	#endif
	PCintPort::curr = portC.portInputReg;
	portC.PCint();
}
#endif
#ifndef NO_PORTD_PINCHANGES
ISR(PORTDVECT){
	#ifdef PINMODE
	PCintPort::s_PORT='D';
	#endif
	PCintPort::curr = portD.portInputReg;
	portD.PCint();
}
#endif
#ifdef __USE_PORT_JK
#ifndef NO_PORTJ_PINCHANGES
ISR(PCINT1_vect) {
	#ifdef PINMODE
	PCintPort::s_PORT='J';
	#endif
	PCintPort::curr = portJ.portInputReg;
	portJ.PCint();
}
#endif
#ifndef NO_PORTK_PINCHANGES
ISR(PCINT2_vect){
	#ifdef PINMODE
	PCintPort::s_PORT='K';
	#endif
	PCintPort::curr = portK.portInputReg;
	portK.PCint();
}
#endif
#endif
#ifdef GET_PCINT_VERSION
uint16_t getPCIntVersion () {
	return ((uint16_t) PCINT_VERSION);
}
#endif
#endif
#endif

Pins.h :

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
#ifndef Pins_h
#define Pins_h
 
#define VOL_MEASURE_PIN A2
#define ECHO_PIN A3
#define TRIG_PIN 11
#define RECV_PIN 9
#define NUMPIXELS 4
#define RGB_PIN 3
#define AIN1 7
#define PWMA_LEFT 5
#define BIN1 12
#define PWMB_RIGHT 6
#define STBY_PIN 8
#define ENCODER_LEFT_A_PIN 2
#define ENCODER_RIGHT_A_PIN 4
#define IR_SEND_PIN 9
#define LEFT_RECEIVE_PIN A0
#define RIGHT_RECEIVE_PIN A1
#define KEY_MODE 10
#endif

Rgb.h :

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

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
/*
 * @Description: In User Settings Edit
 * @Author: your name
 * @Date: 2019-10-08 09:35:07
 * @LastEditTime: 2019-10-10 15:57:17
 * @LastEditors: Please set LastEditors
 */
#include "Adafruit_NeoPixel.h"
 
class RGB : public Adafruit_NeoPixel
{
public:
  RGB() : Adafruit_NeoPixel(NUMPIXELS, RGB_PIN, NEO_GRB + NEO_KHZ800) {}
  uint8_t gamma[256] = {
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1,
      1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 2,
      2, 3, 3, 3, 3, 3, 3, 3, 4, 4, 4, 4, 4, 5, 5, 5,
      5, 6, 6, 6, 6, 7, 7, 7, 7, 8, 8, 8, 9, 9, 9, 10,
      10, 10, 11, 11, 11, 12, 12, 13, 13, 13, 14, 14, 15, 15, 16, 16,
      17, 17, 18, 18, 19, 19, 20, 20, 21, 21, 22, 22, 23, 24, 24, 25,
      25, 26, 27, 27, 28, 29, 29, 30, 31, 32, 32, 33, 34, 35, 35, 36,
      37, 38, 39, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 50,
      51, 52, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 66, 67, 68,
      69, 70, 72, 73, 74, 75, 77, 78, 79, 81, 82, 83, 85, 86, 87, 89,
      90, 92, 93, 95, 96, 98, 99, 101, 102, 104, 105, 107, 109, 110, 112, 114,
      115, 117, 119, 120, 122, 124, 126, 127, 129, 131, 133, 135, 137, 138, 140, 142,
      144, 146, 148, 150, 152, 154, 156, 158, 160, 162, 164, 167, 169, 171, 173, 175,
      177, 180, 182, 184, 186, 189, 191, 193, 196, 198, 200, 203, 205, 208, 210, 213,
      215, 218, 220, 223, 225, 228, 231, 233, 236, 239, 241, 244, 247, 249, 252, 255};
  uint32_t led_rgb_new[NUMPIXELS];
  uint32_t led_rgb_old[NUMPIXELS];
  int brightness = 50;
  unsigned long rgb_delay_time = 0;
  unsigned long dispaly_timeout = 0;
  char flag = 0;
 
  bool rgbDelay(unsigned long wait)
  {
    rgb_delay_time = millis();
    while (millis() - rgb_delay_time < wait)
    {
      if (getBluetoothData() || getKeyValue())
      {
        return true;
      }
      if (motion_mode == STOP && balance_angle_min <= kalmanfilter_angle && kalmanfilter_angle <= balance_angle_max)
      {
        motion_mode = STANDBY;
        lightOff();
        function_mode = IDLE;
        return true;
      }
    }
    return false;
  }
 
  uint32_t Wheel(byte WheelPos)
  {
    WheelPos = 255 - WheelPos;
    if (WheelPos < 85)
    {
      return Color(255 - WheelPos * 3, 0, WheelPos * 3);
    }
    if (WheelPos < 170)
    {
      WheelPos -= 85;
      return Color(0, WheelPos * 3, 255 - WheelPos * 3);
    }
    WheelPos -= 170;
    return Color(WheelPos * 3, 255 - WheelPos * 3, 0);
  }
  uint8_t red(uint32_t c)
  {
    return (c >> 8);
  }
  uint8_t green(uint32_t c)
  {
    return (c >> 16);
  }
  uint8_t blue(uint32_t c)
  {
    return (c);
  }
  bool pulseWhite(uint8_t wait)
  {
    for (int j = 0; j < 256; j++)
    {
      for (uint16_t i = 0; i < numPixels(); i++)
      {
        setPixelColor(i, Color(0, 0, 0, gamma[j]));
      }
      if (rgbDelay(wait))
      {
        return true;
      }
      show();
    }
    for (int j = 255; j >= 0; j--)
    {
      for (uint16_t i = 0; i < numPixels(); i++)
      {
        setPixelColor(i, Color(0, 0, 0, gamma[j]));
      }
      if (rgbDelay(wait))
      {
        return true;
      }
      show();
    }
    return false;
  }
 
  bool theaterChase(uint8_t r, uint8_t g, uint8_t b, uint8_t wait)
  {
    for (int j = 0; j < 200; j++)
    {
      for (int q = 0; q < 3; q++)
      {
        for (uint16_t i = 0; i < numPixels(); i = i + 3)
        {
          setPixelColor(i + q, Color(r, g, b));
        }
        show();
        if (rgbDelay(wait))
        {
          return true;
        }
        for (uint16_t i = 0; i < numPixels(); i = i + 3)
        {
          setPixelColor(i + q, 0);
        }
      }
    }
    return false;
  }
 
  bool rainbow(uint8_t wait)
  {
    for (uint16_t k = 0; k <= 100; k++)
    {
      for (uint16_t j = 0; j < 256; j++)
      {
        for (uint16_t i = 0; i < numPixels(); i++)
        {
          setPixelColor(i, Wheel((i + j) & 255));
        }
        show();
        if (rgbDelay(wait))
        {
          return true;
        }
      }
    }
    return false;
  }
 
  bool rainbowCycle(uint8_t wait)
  {
    for (uint16_t j = 0; j < 256 * 100; j++)
    {
      for (uint16_t i = 0; i < numPixels(); i++)
      {
        setPixelColor(i, Wheel(((i * 256 / numPixels()) + j) & 255));
      }
      show();
      if (rgbDelay(wait))
      {
        return true;
      }
    }
    return false;
  }
 
  bool theaterChaseRainbow(uint8_t wait)
  {
    for (int k = 0; k < 30; k++)
    {
      for (int j = 0; j < 256; j++)
      {
        for (int q = 0; q < 3; q++)
        {
          for (uint16_t i = 0; i < numPixels(); i = i + 3)
          {
            setPixelColor(i + q, Wheel((i + j) % 255));
          }
          show();
          if (rgbDelay(wait))
          {
            return true;
          }
          for (uint16_t i = 0; i < numPixels(); i = i + 3)
          {
            setPixelColor(i + q, 0);
          }
        }
      }
    }
    return false;
  }
 
  bool rainbowFade2White(uint8_t wait, int rainbowLoops, int whiteLoops)
  {
    float fadeMax = 100.0;
    int fadeVal = 0;
    uint32_t wheelVal;
    int redVal, greenVal, blueVal;
    for (int k = 0; k < rainbowLoops; k++)
    {
      for (int j = 0; j < 256; j++)
      {
        for (uint16_t i = 0; i < numPixels(); i++)
        {
          wheelVal = Wheel(((i * 256 / numPixels()) + j) & 255);
          redVal = red(wheelVal) * float(fadeVal / fadeMax);
          greenVal = green(wheelVal) * float(fadeVal / fadeMax);
          blueVal = blue(wheelVal) * float(fadeVal / fadeMax);
          setPixelColor(i, Color(redVal, greenVal, blueVal));
        }
        if (k == 0 && fadeVal < fadeMax - 1)
        {
          fadeVal++;
        }
        else if (k == rainbowLoops - 1 && j > 255 - fadeMax)
        {
          fadeVal--;
        }
        show();
        if (rgbDelay(wait))
        {
          return true;
        }
      }
    }
 
    if (rgbDelay(500))
    {
      return true;
    }
 
    for (int k = 0; k < whiteLoops; k++)
    {
 
      for (int j = 0; j < 256; j++)
      {
 
        for (uint16_t i = 0; i < numPixels(); i++)
        {
          setPixelColor(i, Color(0, 0, 0, gamma[j]));
        }
        show();
      }
      if (rgbDelay(2000))
      {
        return true;
      }
      for (int j = 255; j >= 0; j--)
      {
 
        for (uint16_t i = 0; i < numPixels(); i++)
        {
          setPixelColor(i, Color(0, 0, 0, gamma[j]));
        }
        show();
      }
    }
 
    if (rgbDelay(500))
    {
      return true;
    }
 
    return false;
  }
 
  bool whiteOverRainbow(uint8_t wait, uint8_t whiteSpeed, uint8_t whiteLength)
  {
    if (whiteLength >= numPixels())
      whiteLength = numPixels() - 1;
 
    unsigned int head = whiteLength - 1;
    unsigned int tail = 0;
 
    int loops = 100;
    int loopNum = 0;
 
    static unsigned long lastTime = 0;
 
    while (true)
    {
      for (int j = 0; j < 256; j++)
      {
        for (uint16_t i = 0; i < numPixels(); i++)
        {
          if ((i >= tail && i <= head) || (tail > head && i >= tail) || (tail > head && i <= head))
          {
            setPixelColor(i, Color(0, 0, 0, 255));
          }
          else
          {
            setPixelColor(i, Wheel(((i * 256 / numPixels()) + j) & 255));
          }
        }
 
        if (millis() - lastTime > whiteSpeed)
        {
          head++;
          tail++;
          if (head == numPixels())
          {
            loopNum++;
          }
          lastTime = millis();
        }
 
        if (loopNum == loops)
          return false;
 
        head %= numPixels();
        tail %= numPixels();
        show();
        if (rgbDelay(wait))
        {
          return true;
        }
      }
    }
    return false;
  }
 
  void initialize()
  {
    begin();
    setBrightness(brightness);
    show();
  }
  //闪烁颜色设置
  void setColorNew(unsigned char r0, unsigned char g0, unsigned char b0,
                   unsigned char r1, unsigned char g1, unsigned char b1,
                   unsigned char r2, unsigned char g2, unsigned char b2,
                   unsigned char r3, unsigned char g3, unsigned char b3)
  {
    led_rgb_new[0] = Color(r0, g0, b0);
    led_rgb_new[1] = Color(r1, g1, b1);
    led_rgb_new[2] = Color(r2, g2, b2);
    led_rgb_new[3] = Color(r3, g3, b3);
  }
  //闪烁颜色设置
  void setColorOld(unsigned char r0, unsigned char g0, unsigned char b0,
                   unsigned char r1, unsigned char g1, unsigned char b1,
                   unsigned char r2, unsigned char g2, unsigned char b2,
                   unsigned char r3, unsigned char g3, unsigned char b3)
  {
    led_rgb_old[0] = Color(r0, g0, b0);
    led_rgb_old[1] = Color(r1, g1, b1);
    led_rgb_old[2] = Color(r2, g2, b2);
    led_rgb_old[3] = Color(r3, g3, b3);
  }
  void blink(unsigned long delay_time) //灯光闪烁
  {
    if ((millis() - previous_millis < delay_time) && (delay_flag == 0))
    {
      delay_flag = 1;
      for (size_t i = 0; i < numPixels(); i++)
      {
        setPixelColor(i, led_rgb_new[i]);
      }
      show();
    }
    else if ((millis() - previous_millis < delay_time * 2) && (millis() - previous_millis > delay_time) && (delay_flag == 1))
    {
      delay_flag = 2;
      for (size_t i = 0; i < numPixels(); i++)
      {
        setPixelColor(i, led_rgb_old[i]);
      }
      show();
    }
    else if (millis() - previous_millis >= delay_time * 2)
    {
      delay_flag = 0;
      previous_millis = millis();
    }
  }
  void lightOff() //关闭灯光
  {
    setColorNew(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
    setColorOld(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
  }
  void brightRedColor() //亮红灯
  {
    setColorNew(255, 0, 0, 255, 0, 0, 255, 0, 0, 255, 0, 0);
    setColorOld(255, 0, 0, 255, 0, 0, 255, 0, 0, 255, 0, 0);
  }
  void flashRedColor() //闪红灯
  {
    setColorNew(255, 0, 0, 255, 0, 0, 255, 0, 0, 255, 0, 0);
    setColorOld(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
  }
  void brightBlueColor() //亮蓝灯
  {
    setColorNew(0, 0, 255, 0, 0, 255, 0, 0, 255, 0, 0, 255);
    setColorOld(0, 0, 255, 0, 0, 255, 0, 0, 255, 0, 0, 255);
  }
  void flashBlueColorFront() //前面闪蓝灯
  {
    setColorNew(0, 0, 0, 0, 0, 0, 0, 0, 255, 0, 0, 255);
    setColorOld(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
  }
  void flashBlueColorback() //后面闪蓝灯
  {
    setColorNew(0, 0, 255, 0, 0, 255, 0, 0, 0, 0, 0, 0);
    setColorOld(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
  }
  void flashBlueColorLeft() //左侧闪蓝灯
  {
    setColorNew(0, 0, 0, 0, 0, 255, 0, 0, 255, 0, 0, 0);
    setColorOld(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
  }
  void flashBlueColorRight() //右侧闪蓝灯
  {
    setColorNew(0, 0, 255, 0, 0, 0, 0, 0, 0, 0, 0, 255);
    setColorOld(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
  }
  void brightYellowColor() //亮黄灯
  {
    setColorNew(255, 255, 0, 255, 255, 0, 255, 255, 0, 255, 255, 0);
    setColorOld(255, 255, 0, 255, 255, 0, 255, 255, 0, 255, 255, 0);
  }
  void flashYellowColorFront() //前面闪黄灯
  {
    setColorNew(0, 0, 0, 0, 0, 0, 255, 255, 0, 255, 255, 0);
    setColorOld(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
  }
  void flashYellowColorback() //后面闪黄灯
  {
    setColorNew(255, 255, 0, 255, 255, 0, 0, 0, 0, 0, 0, 0);
    setColorOld(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
  }
  void flashYellowColorLeft() //左侧闪黄灯
  {
    setColorNew(0, 0, 0, 255, 255, 0, 255, 255, 0, 0, 0, 0);
    setColorOld(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
  }
  void flashYellowColorRight() //右侧闪黄灯
  {
    setColorNew(255, 255, 0, 0, 0, 0, 0, 0, 0, 255, 255, 0);
    setColorOld(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
  }
  void brightGreenColor() //亮绿灯
  {
    setColorNew(0, 255, 0, 0, 255, 0, 0, 255, 0, 0, 255, 0);
    setColorOld(0, 255, 0, 0, 255, 0, 0, 255, 0, 0, 255, 0);
  }
  void flashGreenColorFront() //前面闪绿灯
  {
    setColorNew(0, 0, 0, 0, 0, 0, 0, 255, 0, 0, 255, 0);
    setColorOld(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
  }
  void flashGreenColorback() //后面闪绿灯
  {
    setColorNew(0, 255, 0, 0, 255, 0, 0, 0, 0, 0, 0, 0);
    setColorOld(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
  }
  void flashGreenColorLeft() //左侧闪绿灯
  {
    setColorNew(0, 0, 0, 0, 255, 0, 0, 255, 0, 0, 0, 0);
    setColorOld(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
  }
  void flashGreenColorRight() //右侧闪绿灯
  {
    setColorNew(0, 255, 0, 0, 0, 0, 0, 0, 0, 0, 255, 0);
    setColorOld(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
  }
 
private:
  unsigned char delay_flag = 0;
  unsigned long previous_millis = 0;
} rgb;

Ultrasonic.h :

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

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
/*
 * @Description: In User Settings Edit
 * @Author: your name
 * @Date: 2019-10-08 09:35:07
 * @LastEditTime: 2019-10-10 14:24:57
 * @LastEditors: Please set LastEditors
 */
bool left_flag[10] = {false};
unsigned char left_count_flag = 0;
unsigned char left_index = 0;
bool right_flag[10] = {false};
unsigned char right_count_flag = 0;
unsigned char right_index = 0;
 
char measure_flag = 0;
double distance_value = 0;
unsigned long measure_prev_time = 0;
unsigned long get_distance_prev_time = 0;
char obstacle_avoidance_flag = 0;
unsigned long obstacle_avoidance_prev_time = 0;
int follow_flag = 0;
unsigned long follow_prev_time = 0;
long int turn_count = 0;
 
unsigned long ir_send_time = 0;
unsigned long left_receive_time = 0;
unsigned char left_receive_flag = 0;
unsigned int left_count = 0;
unsigned long left_count_time = 0;
unsigned long left_test_time = 0;
bool left_test_flag = false;
bool left_is_obstacle = false;
 
unsigned long right_receive_time = 0;
unsigned char right_receive_flag = 0;
unsigned int right_count = 0;
unsigned long right_count_time = 0;
unsigned long right_test_time = 0;
bool right_test_flag = false;
bool right_is_obstacle = false;
 
void rightFilter(bool value)
{
  if (right_flag[right_index])
    right_count_flag--;
  if (value)
    right_count_flag++;
  right_flag[right_index] = value;
  right_index++;
  if (right_index >= 10)
    right_index = 0;
}
void leftFilter(bool value)
{
  if (left_flag[left_index])
    left_count_flag--;
  if (value)
    left_count_flag++;
  left_flag[left_index] = value;
  left_index++;
  if (left_index >= 10)
    left_index = 0;
}
/**
 * @brief 发送40个38KHz的脉冲
 * 
 * @param pin 产生脉冲的引脚
 */
void send38K(int pin)
{
  for (int i = 0; i < 39; i++)
  {
    digitalWrite(pin, LOW);
    delayMicroseconds(9);
    digitalWrite(pin, HIGH);
    delayMicroseconds(9);
  }
}
 
/**
 * @brief 接收引脚中断服务函数
 * 
 */
void leftReceive()
{
  if (left_receive_flag == 0)
  {
    left_receive_time = micros();
    left_receive_flag = 1;
    attachPinChangeInterrupt(LEFT_RECEIVE_PIN, leftReceive, RISING);
  }
  else if (left_receive_flag == 1)
  {
    left_test_time = micros() - left_receive_time;
    left_count++;
    left_receive_flag = 0;
    attachPinChangeInterrupt(LEFT_RECEIVE_PIN, leftReceive, FALLING);
  }
}
void rightReceive()
{
  if (right_receive_flag == 0)
  {
    right_receive_time = micros();
    right_receive_flag = 1;
    attachPinChangeInterrupt(RIGHT_RECEIVE_PIN, rightReceive, RISING);
  }
  else if (right_receive_flag == 1)
  {
    right_test_time = micros() - right_receive_time;
    right_count++;
    right_receive_flag = 0;
    attachPinChangeInterrupt(RIGHT_RECEIVE_PIN, rightReceive, FALLING);
  }
}
 
void ultrasonicInit()
{
  pinMode(ECHO_PIN, INPUT);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(IR_SEND_PIN, OUTPUT);
  pinMode(LEFT_RECEIVE_PIN, INPUT_PULLUP);
  pinMode(RIGHT_RECEIVE_PIN, INPUT_PULLUP);
  attachPinChangeInterrupt(LEFT_RECEIVE_PIN, leftReceive, FALLING);
  attachPinChangeInterrupt(RIGHT_RECEIVE_PIN, rightReceive, FALLING);
}
 
void checkObstacle()
{
  if (millis() - ir_send_time > 15)
  {
    send38K(IR_SEND_PIN);
    ir_send_time = millis();
  }
 
  if (millis() - left_count_time > 50)
  {
    // Serial.print(left_count);
    if (left_count >= 3)
    {
      // Serial.print("\tLeft");
      left_is_obstacle = true;
      leftFilter(true);
    }
    else
    {
      left_is_obstacle = false;
      leftFilter(false);
    }
 
    // Serial.print(left_count_flag);
    if (left_count_flag >= 5)
    {
      // Serial.print("\tLeft");
      left_is_obstacle = true;
    }
    else
    {
      left_is_obstacle = false;
    }
 
    // Serial.println();
    left_count = 0;
    left_count_time = millis();
  }
 
  if (millis() - right_count_time > 50)
  {
    // Serial.print("\t\t\t");
    // Serial.print(right_count);
    if (right_count >= 3)
    {
      // Serial.print("\tRight");
      rightFilter(true);
    }
    else
    {
      rightFilter(false);
    }
 
    // Serial.print("\t\t\t");
    // Serial.print(right_count_flag);
    if (right_count_flag >= 5)
    {
      // Serial.print("\tRight");
      right_is_obstacle = true;
    }
    else
    {
      right_is_obstacle = false;
    }
    // Serial.println();
 
    right_count = 0;
    right_count_time = millis();
  }
}
 
void obstacleAvoidanceMode()
{
  unsigned int super_min = 3;
  unsigned int distance_min = 10;
  unsigned int distance_max = 45;
  unsigned int super_max = 400;
 
  if (left_is_obstacle && right_is_obstacle)
  {
    if (distance_value >= distance_min && distance_max <= distance_max)
    {
      if (turn_count >= 0)
      {
        motion_mode = TURNRIGHT;
        rgb.flashYellowColorRight();
        turn_count++;
      }
      else
      {
        motion_mode = TURNLEFT;
        rgb.flashYellowColorLeft();
        turn_count--;
      }
    }
    else
    {
      motion_mode = BACKWARD;
      rgb.flashYellowColorback();
    }
  }
  else if (left_is_obstacle && !right_is_obstacle)
  {
    motion_mode = TURNRIGHT;
    rgb.flashYellowColorRight();
    turn_count++;
  }
  else if (!left_is_obstacle && right_is_obstacle)
  {
    motion_mode = TURNLEFT;
    rgb.flashYellowColorLeft();
    turn_count--;
  }
  else
  {
    switch (obstacle_avoidance_flag)
    {
    case 0:
      if (distance_value < distance_max || distance_value > super_max)
      {
        obstacle_avoidance_flag = 1;
        motion_mode = STANDBY;
        rgb.brightYellowColor();
      }
      else
      {
        motion_mode = FORWARD;
        rgb.flashYellowColorFront();
      }
      break;
    case 1:
      if (distance_value >= distance_max && distance_value <= super_max)
      {
        obstacle_avoidance_flag = 0;
        motion_mode = FORWARD;
        rgb.flashYellowColorFront();
      }
      else if (distance_value >= super_min && distance_value <= distance_min)
      {
        motion_mode = BACKWARD;
        rgb.flashYellowColorback();
      }
      else
      {
        if (turn_count >= 0)
        {
          motion_mode = TURNRIGHT;
          rgb.flashYellowColorRight();
          turn_count++;
        }
        else
        {
          motion_mode = TURNLEFT;
          rgb.flashYellowColorLeft();
          turn_count--;
        }
      }
      break;
    default:
      break;
    }
  }
}
 
void followMode()
{
  unsigned int distance_min = 3;
  unsigned int distance_max = 35;
 
  switch (follow_flag)
  {
  case 0:
    if (millis() - follow_prev_time < 500)
    {
      motion_mode = STANDBY;
      rgb.brightGreenColor();
    }
    else if (millis() - follow_prev_time >= 500)
    {
      if (left_is_obstacle || right_is_obstacle)
      {
        follow_flag = 1;
      }
      else
      {
        if (distance_value > distance_min && distance_value < distance_max)
        {
          follow_flag = 2;
        }
        else
        {
          follow_prev_time = millis();
        }
      }
    }
    break;
  case 1:
    if (left_is_obstacle && !right_is_obstacle)
    {
      rgb.flashGreenColorLeft();
      motion_mode = TURNLEFT;
    }
    else if (!left_is_obstacle && right_is_obstacle)
    {
      rgb.flashGreenColorRight();
      motion_mode = TURNRIGHT;
    }
    else if (left_is_obstacle && right_is_obstacle)
    {
      rgb.flashGreenColorFront();
      motion_mode = FORWARD;
    }
    else
    {
      follow_flag = 0;
      motion_mode = STANDBY;
      rgb.brightGreenColor();
    }
    break;
  case 2:
    if (distance_value > distance_min && distance_value < distance_max)
    {
      rgb.flashGreenColorFront();
      motion_mode = FORWARD;
    }
    else
    {
      follow_flag = 0;
      motion_mode = STANDBY;
      rgb.brightGreenColor();
    }
    break;
  default:
    break;
  }
}
 
void followMode2()
{
  unsigned int distance_min = 5;
  unsigned int distance_max = 35;
 
  switch (follow_flag)
  {
  case 0:
    if (millis() - follow_prev_time < 500)
    {
      motion_mode = STANDBY;
      rgb.brightGreenColor();
    }
    else if (millis() - follow_prev_time >= 500)
    {
      if (left_is_obstacle || right_is_obstacle)
      {
        follow_flag = 1;
      }
      else
      {
        if (distance_value > distance_min && distance_value < distance_max)
        {
          follow_flag = 2;
        }
        else
        {
          follow_prev_time = millis();
        }
      }
    }
    break;
  case 1:
    if (left_is_obstacle && !right_is_obstacle)
    {
      rgb.flashGreenColorLeft();
      motion_mode = TURNLEFT;
    }
    else if (!left_is_obstacle && right_is_obstacle)
    {
      rgb.flashGreenColorRight();
      motion_mode = TURNRIGHT;
    }
    else if (left_is_obstacle && right_is_obstacle)
    {
      rgb.flashGreenColorFront();
      motion_mode = BACKWARD;
    }
    else
    {
      follow_flag = 0;
      motion_mode = STANDBY;
      rgb.brightGreenColor();
    }
    break;
  case 2:
    if (distance_value > distance_min && distance_value < distance_max)
    {
      rgb.flashGreenColorFront();
      motion_mode = BACKWARD;
    }
    else
    {
      follow_flag = 0;
      motion_mode = STANDBY;
      rgb.brightGreenColor();
    }
    break;
  default:
    break;
  }
}
 
void measureDistance()
{
  if (measure_flag == 0)
  {
    measure_prev_time = micros();
    attachPinChangeInterrupt(ECHO_PIN, measureDistance, FALLING);
    measure_flag = 1;
  }
  else if (measure_flag == 1)
  {
    distance_value = (micros() - measure_prev_time) * 0.017; //340.29 m/s / 2 -> (340.29*100 cm) /(1000*1000 us) / 2 = 0.0170145
    measure_flag = 2;
  }
}
 
void getDistance()
{
  if (millis() - get_distance_prev_time > 50)
  {
    get_distance_prev_time = millis();
    measure_flag = 0;
    attachPinChangeInterrupt(ECHO_PIN, measureDistance, RISING);
    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);
  }
}

helper_3dmath.h :

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

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
// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D math helper
// 6/5/2012 by Jeff Rowberg <jeff@rowberg.net>
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
//     2012-06-05 - add 3D math helper file to DMP6 example sketch
 
/* ============================================
I2Cdev device library code is placed under the MIT license
Copyright (c) 2012 Jeff Rowberg
 
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
 
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
 
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
===============================================
*/
 
#ifndef _HELPER_3DMATH_H_
#define _HELPER_3DMATH_H_
 
class Quaternion {
    public:
        float w;
        float x;
        float y;
        float z;
 
        Quaternion() {
            w = 1.0f;
            x = 0.0f;
            y = 0.0f;
            z = 0.0f;
        }
 
        Quaternion(float nw, float nx, float ny, float nz) {
            w = nw;
            x = nx;
            y = ny;
            z = nz;
        }
 
        Quaternion getProduct(Quaternion q) {
            // Quaternion multiplication is defined by:
            //     (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2)
            //     (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2)
            //     (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2)
            //     (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2
            return Quaternion(
                w*q.w - x*q.x - y*q.y - z*q.z,  // new w
                w*q.x + x*q.w + y*q.z - z*q.y,  // new x
                w*q.y - x*q.z + y*q.w + z*q.x,  // new y
                w*q.z + x*q.y - y*q.x + z*q.w); // new z
        }
 
        Quaternion getConjugate() {
            return Quaternion(w, -x, -y, -z);
        }
 
        float getMagnitude() {
            return sqrt(w*w + x*x + y*y + z*z);
        }
 
        void normalize() {
            float m = getMagnitude();
            w /= m;
            x /= m;
            y /= m;
            z /= m;
        }
 
        Quaternion getNormalized() {
            Quaternion r(w, x, y, z);
            r.normalize();
            return r;
        }
};
 
class VectorInt16 {
    public:
        int16_t x;
        int16_t y;
        int16_t z;
 
        VectorInt16() {
            x = 0;
            y = 0;
            z = 0;
        }
 
        VectorInt16(int16_t nx, int16_t ny, int16_t nz) {
            x = nx;
            y = ny;
            z = nz;
        }
 
        float getMagnitude() {
            return sqrt(x*x + y*y + z*z);
        }
 
        void normalize() {
            float m = getMagnitude();
            x /= m;
            y /= m;
            z /= m;
        }
 
        VectorInt16 getNormalized() {
            VectorInt16 r(x, y, z);
            r.normalize();
            return r;
        }
 
        void rotate(Quaternion *q) {
            // http://www.cprogramming.com/tutorial/3d/quaternions.html
            // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
            // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
            // ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1
 
            // P_out = q * P_in * conj(q)
            // - P_out is the output vector
            // - q is the orientation quaternion
            // - P_in is the input vector (a*aReal)
            // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z])
            Quaternion p(0, x, y, z);
 
            // quaternion multiplication: q * p, stored back in p
            p = q -> getProduct(p);
 
            // quaternion multiplication: p * conj(q), stored back in p
            p = p.getProduct(q -> getConjugate());
 
            // p quaternion is now [0, x', y', z']
            x = p.x;
            y = p.y;
            z = p.z;
        }
 
        VectorInt16 getRotated(Quaternion *q) {
            VectorInt16 r(x, y, z);
            r.rotate(q);
            return r;
        }
};
 
class VectorFloat {
    public:
        float x;
        float y;
        float z;
 
        VectorFloat() {
            x = 0;
            y = 0;
            z = 0;
        }
 
        VectorFloat(float nx, float ny, float nz) {
            x = nx;
            y = ny;
            z = nz;
        }
 
        float getMagnitude() {
            return sqrt(x*x + y*y + z*z);
        }
 
        void normalize() {
            float m = getMagnitude();
            x /= m;
            y /= m;
            z /= m;
        }
 
        VectorFloat getNormalized() {
            VectorFloat r(x, y, z);
            r.normalize();
            return r;
        }
 
        void rotate(Quaternion *q) {
            Quaternion p(0, x, y, z);
 
            // quaternion multiplication: q * p, stored back in p
            p = q -> getProduct(p);
 
            // quaternion multiplication: p * conj(q), stored back in p
            p = p.getProduct(q -> getConjugate());
 
            // p quaternion is now [0, x', y', z']
            x = p.x;
            y = p.y;
            z = p.z;
        }
 
        VectorFloat getRotated(Quaternion *q) {
            VectorFloat r(x, y, z);
            r.rotate(q);
            return r;
        }
};
 
#endif /* _HELPER_3DMATH_H_ */

mode.h :

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
enum FUNCTION_MODE
{
  IDLE,
  IRREMOTE,
  OBSTACLE,
  FOLLOW,
  BLUETOOTH,
  FOLLOW2,
} function_mode = IDLE;
 
enum MOTION_MODE
{
  STANDBY,
  FORWARD,
  BACKWARD,
  TURNLEFT,
  TURNRIGHT,
  STOP,
  START,
} motion_mode = START;

voltage.h :

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

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
int low_voltage_flag = 1;
unsigned long vol_measure_time = 0;
void voltageInit()
{
  analogReference(INTERNAL);
}
void voltageMeasure() //测量电压
{
  if (millis() - vol_measure_time > 1000) //每1000毫秒测量一次
  {
    vol_measure_time = millis();
    double voltage = (analogRead(VOL_MEASURE_PIN) * 1.1 / 1024) * ((10 + 1.5) / 1.5); //读取电压值
    if (voltage > 7.8)                                                                //电池供电
    {
      if (low_voltage_flag == 1)
      {
        rgb.lightOff();
        digitalWrite(STBY_PIN,HIGH);
      }
      low_voltage_flag = 0; //满电压标志
    }
    else
    {
      if (voltage < 7.0) //The battery is low in power and needs to be charged.
      {
        motion_mode = STOP;
        digitalWrite(STBY_PIN,LOW);
      }
      //待机,停止,自动启动, 启动模式
      if (motion_mode == STANDBY || motion_mode == STOP || motion_mode == START)
      {
        rgb.flashRedColor();
      }
      low_voltage_flag = 1; //低电压标志
    }
  }
}

Invité · 02/03/2023, 20h09

Nom : Manuel utilisateur (FR) 20200825_page-0001.jpg
Affichages : 364
Taille : 265,3 Ko

Nom : Manuel utilisateur (FR) 20200825_page-0002.jpg
Affichages : 359
Taille : 180,5 Ko

Nom : Manuel utilisateur (FR) 20200825_page-0003.jpg
Affichages : 373
Taille : 277,0 Ko

Nom : Manuel utilisateur (FR) 20200825_page-0004.jpg
Affichages : 365
Taille : 269,2 Ko

Nom : Manuel utilisateur (FR) 20200825_page-0005.jpg
Affichages : 367
Taille : 261,5 Ko

**kaitlyn** · 02/03/2023, 20h56

Envoyé par Vincent PETIT

Il y a très peu de chance que quelqu'un ouvre ton fichier zip, postes plutôt ton programme entre les balises adaptées [CODE][/CODE] ou en cliquant sur le bouton # dans l'éditeur de message

Vous avez demandé les codes sources, ne quittez pas!

Edit: Ça m'a tellement fait rire que j'ai oublié les salutations. Salut.

Sinon bravo @Valim, tes +1 sont bien mérités.

**Jay M** · 02/03/2023, 23h04

Envoyé par Valim

je vous demande donc un peu d'aide pour m'expliquer comment fonctionne le programme.

Vous souhaitez une explication ligne par ligne ?

**Guesset** · 03/03/2023, 17h12

Bonjour,

Enfin un sujet où on s'amuse.

Je ne crois pas qu'expliquer les bibliothèques non dédiées à ce seul projet soit très opportun.

J'ai un peu regardé le code. Je suis loin d'avoir tout compris, mais j'ai quelques remarques :

theaterChaseRainbow() utilise un % 255 (donc compris entre 0 et 254) alors que partout ailleurs est utilisé un & 255 équivalent performant du %256. Erreur ?
il y a beaucoup de divisions évitables dans des boucles simplement incrémentées, par exemple dans rainbowFade2White() on trouve i * 256 / numPixels()) + j avec i allant de 0 à numPixels()-1. Bresenham au secours !
il y a un retour en force du goto (dans keyEventHandle() ). Je pense que key_value est un volatile mis à jour dans une interruption mais il est mis à 0 juste avant d'être testé à '3' ce qui laisse quelques us à l'interruption pour se manifester. Pertinent ?
il y a beaucoup de switches dont nombre pourraient disparaître assez facilement avec des tableaux des structures.
les commentaires en chinois sont très intéressants certainement mais c'est du ch... latin pour moi.
dans la loop(), start_time est comparé à millis() sans jamais être affecté.
dans la loop(), print_time est comparé à millis() sans avoir été affecté préalablement.

Je pense que le plus intéressant est le code d'équilibre mais je ne l'ai pas détaillé : vraisemblablement PDI avec Kalman.
Je ne crois pas que j'aurai le courage de le désosser ligne par ligne.
Salutations

Invité · 03/03/2023, 20h30

Merci beaucoup Guesset ton commentaire, tu m'as déjà bien aider. Mais c'est vraie que le plus important pour le projet est le code d'équilibre, mais comme vous pouvez le voir le code est énorme. A la fin du projet je vais effectivement devoir expliquer ligne par ligne (à peut prêt) comment fonctionne mon code final.

**Jay M** · 03/03/2023, 21h14

Envoyé par Valim

A la fin du projet je vais effectivement devoir expliquer ligne par ligne (à peut prêt) comment fonctionne mon code final.

dans ce cas ne prenez pas leur code... écrivez le votre de A à Z

prenez chaque fonction du robot indépendamment au départ, faites bouger les moteurs, allumez les LEDs, faites fonctionnner le détecteur à ultrasons, etc...

une fois que chaque fonction est bien maitrisée vous pourrez passer à l'assemblage des fonctions pour un mode automatique

Vous êtes en quelle classe?

il faudra vous renseigner sur ce qu'est un régulateur PID et étudier le principe des filtres de Kalman... Suivant votre niveau d'étude je pense que c'est OK si vous vous appuyez sur une bibliothèque (par exemple SimpleKalmanFilter)

Projet Robot self-balancing Elegoo Trumbller arduino nano

Arduino

Discussions similaires

Partager

Partager