Quadruped with Arduino Nano 33 BLE Sense

How about a Spider who obeys the order to sit, dances when he sees a light and dodges obstacles? All this with the Arduino Nano 33 BLE Sense

Nov 14, 2020

•

5011 views

•

6 respects

environmental sensing

robots

Components and supplies

DC-DC Buck (Step Down) Regulator, Adjustable

Male/Female Jumper Wires

Adafruit 16-Channel 12-bit PWM/Servo Shield - I2C interface

Nano 33 BLE Sense

SG90 Micro-servo motor

Battery Holder, 18650 x 2

Tools and machines

Tape, Electrical

Plier, Long Nose

Mastech MS8217 Autorange Digital Multimeter

Project description

Code

Code Aranha

arduino

1#include <Servo.h>    // include servo library
2#include <Arduino_APDS9960.h>  // include sensor distance library
3#include <PDM.h>
4
5// calibration
6int  da =  -12,  // Left Front Pivot
7    db =   10,  // Left Back Pivot
8    dc  =  -18,  // Right Back Pivot
9    dd =   12;  // Right Front Pivot
10
11// servo  initial positions + calibration
12int a90  = (90  + da),
13    a120 = (120 + da),
14    a150 = (150 + da),
15    a180 = (180 + da);
16
17int b0   = (0   + db),
18    b30  = (30  + db),
19    b60  = (60  + db),
20    b90  = (90  + db);
21
22int  c90  = (90  + dc),
23    c120 = (120 + dc),
24    c150 = (150 + dc),
25    c180  = (180 + dc);
26
27int d0   = (0   + dd),
28    d30  = (30  + dd),
29    d60  = (60  + dd),
30    d90  = (90  + dd);
31
32// start points for servo
33int  s11 = 90; // Front Left Pivot Servo
34int s12 = 90; // Front Left Lift Servo
35int  s21 = 90; // Back Left Pivot Servo
36int s22 = 90; // Back Left Lift Servo
37int  s31 = 90; // Back Right Pivot Servo
38int s32 = 90; // Back Right Lift Servo
39int  s41 = 90; // Front Right Pivot Servo
40int s42 = 90; // Front Right Lift Servo
41
42int  f    = 0;
43int b    = 0;
44int l    = 0;
45int r    = 0;
46int spd  = 3;  //  Speed of walking motion, larger the number, the slower the speed
47int high = 0;   // How high the robot is standing
48
49// Define 8 Servos
50Servo myServo1;  // Servo piv esquerdo frontal           a4
51Servo myServo2; // Servo de elevao  frontal esquerdo    10
52Servo myServo3; // Servo do piv esquerdo traseiro        5
53Servo  myServo4; // Servo de elevao traseira esquerda    6
54
55Servo myServo5; // Servo  de piv direito traseiro         3
56Servo myServo6; // Servo de elevao traseira  direita     9
57Servo myServo7; // Servo de piv frontal direito         11
58Servo  myServo8; // Servo de elevao frontal direita     a5
59
60short sampleBuffer[256];
61
62//  number of samples read
63volatile int samplesRead;
64//===== Setup ==============================================================================
65
66void  setup()
67{
68  // Attach servos to Arduino Pins
69  myServo1.attach(A4);
70  myServo2.attach(10);
71  myServo3.attach(5);
72  myServo4.attach(6);
73
74  myServo5.attach(3);
75  myServo6.attach(9);
76  myServo7.attach(11);
77  myServo8.attach(A5);
78
79
80  Serial.begin (9600);
81 // while (!Serial);
82
83  if (!APDS.begin()) {
84    Serial.println("Error initializing APDS9960 sensor!");
85  }
86
87  PDM.onReceive(onPDMdata);
88
89  if (!PDM.begin(1, 16000)) {
90    Serial.println("Failed to start PDM!");
91    while (1);
92  }
93
94}//setup
95
96//==========================================================================================
97
98//==  Loop ==================================================================================
99
100void  loop()
101{
102  voice();
103  while (!APDS.proximityAvailable()) {
104    delay(5);
105  }
106  int proximity = APDS.readProximity();   //Read the distance
107  Serial.println(proximity);
108  if (proximity > 230) {
109    center_servos();                      // Center  all servos
110    high = 15;                            // Set hight to 15
111    spd  = 7;                              // Set speed to 3
112    voice();
113    test();                               //Test the sensors
114    forward();                            //Walk  forward function
115  }
116  else {
117    center_servos();                      //centralize  the servo leaving the spyder in the alert position
118    for (byte x = 0; x < 3;  x++) {
119      back();
120      voice();
121    }
122    test();
123    for (byte  x = 0; x < 3; x++) {
124      turn_left();
125      voice();
126    }
127    test();
128  }
129
130}//loop
131
132void dance()
133{
134  center_servos();
135  delay(100);
136  lean_left();
137  delay(300);
138  lean_right();
139  delay(300);
140  lean_left();
141  delay(300);
142  lean_right();
143  delay(300);
144  lean_left();
145  delay(300);
146  lean_right();
147  delay(300);
148  lean_left();
149  delay(300);
150  lean_right();
151  delay(800);
152  center_servos();
153  delay(300);
154  bow();
155  center_servos();
156}
157
158//==  Wave ==================================================================================
159
160void  wave()
161{
162  /*
163    myServo1 - Front Left Pivot Servo
164    myServo2 - Front  Left Lift Servo
165    myServo3 - Back Left Pivot Servo
166    myServo4 - Back Left  Lift Servo
167    myServo5 - Back Right Pivot Servo
168    myServo6 - Back Right  Lift Servo
169    myServo7 - Front Right Pivot Servo
170    myServo8 - Front Right  Lift Servo
171  */
172
173  center_servos();
174  myServo4.write(45);
175  myServo6.write(45);
176  delay(200);
177  myServo8.write(0);
178  delay(200);
179  myServo7.write(180);
180  delay(200);
181  myServo7.write(30);
182  delay(300);
183  myServo7.write(180);
184  delay(300);
185  myServo7.write(30);
186  delay(300);
187  myServo7.write(s41);
188  delay(300);
189  myServo8.write(s42);
190  center_servos();
191
192}
193
194void  sit() {
195  delay(500);
196
197  myServo2.write(20);
198  myServo4.write(20);
199  myServo6.write(20);
200  myServo8.write(20);
201
202  delay(5000);
203  center_servos();
204}
205
206//==  Bow ===================================================================================
207
208void  bow()
209{
210  center_servos();
211  delay(200);
212  myServo2.write(15);
213  myServo8.write(15);
214  delay(700);
215  myServo2.write(90);
216  myServo8.write(90);
217  delay(700);
218}
219
220//==  Lean_Left =============================================================================
221
222void  lean_left()
223{
224  myServo2.write(15);
225  myServo4.write(15);
226  myServo6.write(150);
227  myServo8.write(150);
228}
229
230//== Lean_Right ============================================================================
231
232void  lean_right()
233{
234  myServo2.write(150);
235  myServo4.write(150);
236  myServo6.write(15);
237  myServo8.write(15);
238}
239
240//== Lean_Left =============================================================================
241
242void  trim_left()
243{
244  da--; // Left Front Pivot
245  db--; // Left Back Pivot
246  dc--; // Right Back Pivot
247  dd--; // Right Front Pivot
248}
249
250//== Lean_Right  ============================================================================
251
252void  trim_right()
253{
254  da++; // Left Front Pivot
255  db++; // Left Back Pivot
256  dc++; // Right Back Pivot
257  dd++; // Right Front Pivot
258}
259
260//== Forward  ===============================================================================
261
262void  forward()
263{
264  // calculation of points
265
266  // Left Front Pivot
267  a90  = (90 + da),
268  a120 = (120 + da),
269  a150 = (150 + da),
270  a180 = (180 +  da);
271
272  // Left Back Pivot
273  b0 = (0 + db),
274  b30 = (30 + db),
275  b60  = (60 + db),
276  b90 = (90 + db);
277
278  // Right Back Pivot
279  c90 = (90 +  dc),
280  c120 = (120 + dc),
281  c150 = (150 + dc),
282  c180 = (180 + dc);
283
284  // Right Front Pivot
285  d0 = (0 + dd),
286  d30 = (30 + dd),
287  d60 = (60  + dd),
288  d90 = (90 + dd);
289
290  // set servo positions and speeds needed to  walk forward one step
291  // (LFP,  LBP, RBP,  RFP, LFL, LBL, RBL, RFL, S1, S2,  S3, S4)
292  srv(a180, b0 , c120, d60, 42,  33,  33,  42,  1,  3,  1,  1);
293  srv(  a90, b30, c90,  d30, 6,   33,  33,  42,  3,  1,  1,  1);
294  srv( a90, b30, c90,  d30, 42,  33,  33,  42,  3,  1,  1,  1);
295  srv(a120, b60, c180, d0,  42,  33,  6,   42,  1,  1,  3,  1);
296  srv(a120, b60, c180, d0,  42,  33,  33,  42,  1,  1,  3,  1);
297  srv(a150, b90, c150, d90, 42,  33,  33,  6,   1,  1,  1,  3);
298  srv(a150, b90, c150, d90, 42,  33,  33,  42,  1,  1,  1,  3);
299  srv(a180, b0,  c120, d60, 42,  6,   33,  42,  1,  3,  1,  1);
300  //srv(a180, b0,  c120, d60,  42,  15,  33,  42,  1,  3,  1,  1);
301  test();
302}
303
304//== Back ==================================================================================
305
306void  back ()
307{
308  // set servo positions and speeds needed to walk backward one  step
309  // (LFP,  LBP, RBP,  RFP, LFL, LBL, RBL, RFL, S1, S2, S3, S4)
310  srv(180,  0,  120, 60, 42, 33, 33, 42, 3,  1, 1, 1);
311  srv(150, 90, 150, 90, 42, 18, 33,  42, 1,  3, 1, 1);
312  srv(150, 90, 150, 90, 42, 33, 33, 42, 1,  3, 1, 1);
313  srv(120,  60, 180, 0,  42, 33, 33, 6,  1,  1, 1, 3);
314  srv(120, 60, 180, 0,  42, 33, 33,  42, 1,  1, 1, 3);
315  srv(90,  30, 90,  30, 42, 33, 18, 42, 1,  1, 3, 1);
316  srv(90,  30, 90,  30, 42, 33, 33, 42, 1,  1, 3, 1);
317  srv(180, 0,  120, 60, 6,  33,  33, 42, 3,  1, 1, 1);
318
319}
320
321//== Left =================================================================================
322
323void  turn_left ()
324{
325  // set servo positions and speeds needed to turn left one  step
326  // (LFP,  LBP, RBP,  RFP, LFL, LBL, RBL, RFL, S1, S2, S3, S4)
327  srv(150,  90, 90,  30, 42, 6,  33, 42, 1, 3, 1, 1);
328  srv(150,  90, 90,  30, 42, 33,  33, 42, 1, 3, 1, 1);
329  srv(120,  60, 180, 0,  42, 33, 6,  42, 1, 1, 3, 1);
330  srv(120,  60, 180, 0,  42, 33, 33, 24, 1, 1, 3, 1);
331  srv(90,   30, 150, 90,  42, 33, 33, 6,  1, 1, 1, 3);
332  srv(90,   30, 150, 90, 42, 33, 33, 42, 1, 1, 1,  3);
333  srv(180,  0,  120, 60, 6,  33, 33, 42, 3, 1, 1, 1);
334  srv(180,  0,  120,  60, 42, 33, 33, 33, 3, 1, 1, 1);
335}
336
337//== Right ================================================================================
338
339void  turn_right ()
340{
341  // set servo positions and speeds needed to turn right one  step
342  // (LFP,  LBP, RBP,  RFP, LFL, LBL, RBL, RFL, S1, S2, S3, S4)
343  srv(  90, 30, 150, 90, 6,  33, 33, 42, 3, 1, 1, 1);
344  srv( 90, 30, 150, 90, 42, 33,  33, 42, 3, 1, 1, 1);
345  srv(120, 60, 180, 0,  42, 33, 33, 6,  1, 1, 1, 3);
346  srv(120, 60, 180, 0,  42, 33, 33, 42, 1, 1, 1, 3);
347  srv(150, 90, 90,  30,  42, 33, 6,  42, 1, 1, 3, 1);
348  srv(150, 90, 90,  30, 42, 33, 33, 42, 1, 1, 3,  1);
349  srv(180, 0,  120, 60, 42, 6,  33, 42, 1, 3, 1, 1);
350  srv(180, 0,  120,  60, 42, 33, 33, 42, 1, 3, 1, 1);
351}
352
353//== Center Servos ========================================================================
354
355void  center_servos()
356{
357  myServo1.write(90);
358  myServo2.write(90);
359  myServo3.write(90);
360  myServo4.write(90);
361  myServo5.write(90);
362  myServo6.write(90);
363  myServo7.write(90);
364  myServo8.write(90);
365
366  int s11 = 90; // Front Left Pivot Servo
367  int  s12 = 90; // Front Left Lift Servo
368  int s21 = 90; // Back Left Pivot Servo
369  int s22 = 90; // Back Left Lift Servo
370  int s31 = 90; // Back Right Pivot Servo
371  int s32 = 90; // Back Right Lift Servo
372  int s41 = 90; // Front Right Pivot  Servo
373  int s42 = 90; // Front Right Lift Servo
374}
375
376//== Increase Speed  ========================================================================
377
378void  increase_speed()
379{
380  if (spd > 3)
381    spd--;
382}
383
384//== Decrease  Speed ========================================================================
385
386void  decrease_speed()
387{
388  if (spd < 50)
389    spd++;
390}
391
392//== Srv ===================================================================================
393
394void  srv( int  p11, int p21, int p31, int p41, int p12, int p22, int p32, int p42, int  sp1, int sp2, int sp3, int sp4)
395{
396  voice();
397  // p11: Front Left Pivot  Servo
398  // p21: Back Left Pivot Servo
399  // p31: Back Right Pivot Servo
400  // p41: Front Right Pivot Servo
401  // p12: Front Left Lift Servo
402  // p22:  Back Left Lift Servo
403  // p32: Back Right Lift Servo
404  // p42: Front Right  Lift Servo
405  // sp1: Speed 1
406  // sp2: Speed 2
407  // sp3: Speed 3
408  //  sp4: Speed 4
409
410  // Multiply lift servo positions by manual height adjustment
411  p12 = p12 + high * 3;
412  p22 = p22 + high * 3;
413  p32 = p32 + high * 3;
414  p42 = p42 + high * 3;
415
416  while ((s11 != p11) || (s21 != p21) || (s31 !=  p31) || (s41 != p41) || (s12 != p12) || (s22 != p22) || (s32 != p32) || (s42 !=  p42))
417  {
418
419    // Front Left Pivot Servo
420    if (s11 < p11)            //  if servo position is less than programmed position
421    {
422      if ((s11 +  sp1) <= p11)
423        s11 = s11 + sp1;      // set servo position equal to servo  position plus speed constant
424      else
425        s11 = p11;
426    }
427
428    if (s11 > p11)            // if servo position is greater than programmed position
429    {
430      if ((s11 - sp1) >= p11)
431        s11 = s11 - sp1;      // set servo  position equal to servo position minus speed constant
432      else
433        s11  = p11;
434    }
435
436    // Back Left Pivot Servo
437    if (s21 < p21)
438    {
439      if ((s21 + sp2) <= p21)
440        s21 = s21 + sp2;
441      else
442        s21  = p21;
443    }
444
445    if (s21 > p21)
446    {
447      if ((s21 - sp2) >= p21)
448        s21 = s21 - sp2;
449      else
450        s21 = p21;
451    }
452
453    //  Back Right Pivot Servo
454    if (s31 < p31)
455    {
456      if ((s31 + sp3) <=  p31)
457        s31 = s31 + sp3;
458      else
459        s31 = p31;
460    }
461
462    if (s31 > p31)
463    {
464      if ((s31 - sp3) >= p31)
465        s31 = s31  - sp3;
466      else
467        s31 = p31;
468    }
469
470    // Front Right Pivot  Servo
471    if (s41 < p41)
472    {
473      if ((s41 + sp4) <= p41)
474        s41  = s41 + sp4;
475      else
476        s41 = p41;
477    }
478
479    if (s41 > p41)
480    {
481      if ((s41 - sp4) >= p41)
482        s41 = s41 - sp4;
483      else
484        s41 = p41;
485    }
486
487    // Front Left Lift Servo
488    if (s12 <  p12)
489    {
490      if ((s12 + sp1) <= p12)
491        s12 = s12 + sp1;
492      else
493        s12 = p12;
494    }
495
496    if (s12 > p12)
497    {
498      if ((s12  - sp1) >= p12)
499        s12 = s12 - sp1;
500      else
501        s12 = p12;
502    }
503
504    // Back Left Lift Servo
505    if (s22 < p22)
506    {
507      if  ((s22 + sp2) <= p22)
508        s22 = s22 + sp2;
509      else
510        s22 =  p22;
511    }
512
513    if (s22 > p22)
514    {
515      if ((s22 - sp2) >= p22)
516        s22 = s22 - sp2;
517      else
518        s22 = p22;
519    }
520
521    //  Back Right Lift Servo
522    if (s32 < p32)
523    {
524      if ((s32 + sp3) <=  p32)
525        s32 = s32 + sp3;
526      else
527        s32 = p32;
528    }
529
530    if (s32 > p32)
531    {
532      if ((s32 - sp3) >= p32)
533        s32 = s32  - sp3;
534      else
535        s32 = p32;
536    }
537
538    // Front Right Lift  Servo
539    if (s42 < p42)
540    {
541      if ((s42 + sp4) <= p42)
542        s42  = s42 + sp4;
543      else
544        s42 = p42;
545    }
546
547    if (s42 > p42)
548    {
549      if ((s42 - sp4) >= p42)
550        s42 = s42 - sp4;
551      else
552        s42 = p42;
553    }
554
555    // Write Pivot Servo Values
556    myServo1.write(s11  + da);
557    myServo3.write(s21 + db);
558    myServo5.write(s31 + dc);
559    myServo7.write(s41  + dd);
560
561    // Write Lift Servos Values
562    myServo2.write(s12);
563    myServo4.write(s22);
564    myServo6.write(s32);
565    myServo8.write(s42);
566
567    delay(spd); // Delay  before next movement
568
569  }//while
570} //srv
571
572void test() {
573  ligth();
574  movement();
575  
576
577  /*
578         Se tiver luz{
579         center_servos();
580         Funo danar: dance();
581         }
582  */
583
584  /*
585     Se tiver  gestos{
586     center_servos();
587     Funo tchauzinho: wave();
588     }
589  */
590  /*
591     Se ouvir voz{
592     center_servos();
593     Funo sentar:  sit():
594     }
595  */
596}
597
598void ligth() {
599  while (! APDS.colorAvailable())  {
600    delay(3);
601  }
602  int r, g, b;
603
604  // read the color
605  APDS.readColor(r,  g, b);
606
607  while (r > 20 || g > 20 || b > 20) {
608    dance();
609    while  (! APDS.colorAvailable()) {
610      delay(3);
611    }
612    APDS.readColor(r,  g, b);
613  }
614  // print the values
615  /* Serial.print("r = ");
616    Serial.println(r);
617    Serial.print("g = ");
618    Serial.println(g);
619    Serial.print("b =  ");
620    Serial.println(b);
621    Serial.println();*/
622}
623
624void movement()  {
625
626}
627
628void voice() {
629  if (samplesRead) {
630
631    // print samples  to the serial monitor or plotter
632    for (int i = 0; i < samplesRead; i++) {
633     // Serial.println(sampleBuffer[i]);
634      if(sampleBuffer[i]>1000){
635       // Serial.println("Ouvi: " + String(sampleBuffer[i]));
636       center_servos();
637       sit();
638      }
639    }
640
641    // clear the read count
642    samplesRead  = 0;
643  }
644}
645
646void onPDMdata() {
647  // query the number of bytes available
648  int bytesAvailable = PDM.available();
649
650  // read into the sample buffer
651  PDM.read(sampleBuffer, bytesAvailable);
652
653  // 16-bit, 2 bytes per sample
654  samplesRead = bytesAvailable / 2;
655}
656

#include <Servo.h>    // include servo library
#include <Arduino_APDS9960.h>  // include sensor distance library
#include <PDM.h>

// calibration
int  da =  -12,  // Left Front Pivot
    db =   10,  // Left Back Pivot
    dc  =  -18,  // Right Back Pivot
    dd =   12;  // Right Front Pivot

// servo  initial positions + calibration
int a90  = (90  + da),
    a120 = (120 + da),
    a150 = (150 + da),
    a180 = (180 + da);

int b0   = (0   + db),
    b30  = (30  + db),
    b60  = (60  + db),
    b90  = (90  + db);

int  c90  = (90  + dc),
    c120 = (120 + dc),
    c150 = (150 + dc),
    c180  = (180 + dc);

int d0   = (0   + dd),
    d30  = (30  + dd),
    d60  = (60  + dd),
    d90  = (90  + dd);

// start points for servo
int  s11 = 90; // Front Left Pivot Servo
int s12 = 90; // Front Left Lift Servo
int  s21 = 90; // Back Left Pivot Servo
int s22 = 90; // Back Left Lift Servo
int  s31 = 90; // Back Right Pivot Servo
int s32 = 90; // Back Right Lift Servo
int  s41 = 90; // Front Right Pivot Servo
int s42 = 90; // Front Right Lift Servo

int  f    = 0;
int b    = 0;
int l    = 0;
int r    = 0;
int spd  = 3;  //  Speed of walking motion, larger the number, the slower the speed
int high = 0;   // How high the robot is standing

// Define 8 Servos
Servo myServo1;  // Servo piv esquerdo frontal           a4
Servo myServo2; // Servo de elevao  frontal esquerdo    10
Servo myServo3; // Servo do piv esquerdo traseiro        5
Servo  myServo4; // Servo de elevao traseira esquerda    6

Servo myServo5; // Servo  de piv direito traseiro         3
Servo myServo6; // Servo de elevao traseira  direita     9
Servo myServo7; // Servo de piv frontal direito         11
Servo  myServo8; // Servo de elevao frontal direita     a5

short sampleBuffer[256];

//  number of samples read
volatile int samplesRead;
//===== Setup ==============================================================================

void  setup()
{
  // Attach servos to Arduino Pins
  myServo1.attach(A4);
  myServo2.attach(10);
  myServo3.attach(5);
  myServo4.attach(6);

  myServo5.attach(3);
  myServo6.attach(9);
  myServo7.attach(11);
  myServo8.attach(A5);


  Serial.begin (9600);
 // while (!Serial);

  if (!APDS.begin()) {
    Serial.println("Error initializing APDS9960 sensor!");
  }

  PDM.onReceive(onPDMdata);

  if (!PDM.begin(1, 16000)) {
    Serial.println("Failed to start PDM!");
    while (1);
  }

}//setup

//==========================================================================================

//==  Loop ==================================================================================

void  loop()
{
  voice();
  while (!APDS.proximityAvailable()) {
    delay(5);
  }
  int proximity = APDS.readProximity();   //Read the distance
  Serial.println(proximity);
  if (proximity > 230) {
    center_servos();                      // Center  all servos
    high = 15;                            // Set hight to 15
    spd  = 7;                              // Set speed to 3
    voice();
    test();                               //Test the sensors
    forward();                            //Walk  forward function
  }
  else {
    center_servos();                      //centralize  the servo leaving the spyder in the alert position
    for (byte x = 0; x < 3;  x++) {
      back();
      voice();
    }
    test();
    for (byte  x = 0; x < 3; x++) {
      turn_left();
      voice();
    }
    test();
  }

}//loop

void dance()
{
  center_servos();
  delay(100);
  lean_left();
  delay(300);
  lean_right();
  delay(300);
  lean_left();
  delay(300);
  lean_right();
  delay(300);
  lean_left();
  delay(300);
  lean_right();
  delay(300);
  lean_left();
  delay(300);
  lean_right();
  delay(800);
  center_servos();
  delay(300);
  bow();
  center_servos();
}

//==  Wave ==================================================================================

void  wave()
{
  /*
    myServo1 - Front Left Pivot Servo
    myServo2 - Front  Left Lift Servo
    myServo3 - Back Left Pivot Servo
    myServo4 - Back Left  Lift Servo
    myServo5 - Back Right Pivot Servo
    myServo6 - Back Right  Lift Servo
    myServo7 - Front Right Pivot Servo
    myServo8 - Front Right  Lift Servo
  */

  center_servos();
  myServo4.write(45);
  myServo6.write(45);
  delay(200);
  myServo8.write(0);
  delay(200);
  myServo7.write(180);
  delay(200);
  myServo7.write(30);
  delay(300);
  myServo7.write(180);
  delay(300);
  myServo7.write(30);
  delay(300);
  myServo7.write(s41);
  delay(300);
  myServo8.write(s42);
  center_servos();

}

void  sit() {
  delay(500);

  myServo2.write(20);
  myServo4.write(20);
  myServo6.write(20);
  myServo8.write(20);

  delay(5000);
  center_servos();
}

//==  Bow ===================================================================================

void  bow()
{
  center_servos();
  delay(200);
  myServo2.write(15);
  myServo8.write(15);
  delay(700);
  myServo2.write(90);
  myServo8.write(90);
  delay(700);
}

//==  Lean_Left =============================================================================

void  lean_left()
{
  myServo2.write(15);
  myServo4.write(15);
  myServo6.write(150);
  myServo8.write(150);
}

//== Lean_Right ============================================================================

void  lean_right()
{
  myServo2.write(150);
  myServo4.write(150);
  myServo6.write(15);
  myServo8.write(15);
}

//== Lean_Left =============================================================================

void  trim_left()
{
  da--; // Left Front Pivot
  db--; // Left Back Pivot
  dc--; // Right Back Pivot
  dd--; // Right Front Pivot
}

//== Lean_Right  ============================================================================

void  trim_right()
{
  da++; // Left Front Pivot
  db++; // Left Back Pivot
  dc++; // Right Back Pivot
  dd++; // Right Front Pivot
}

//== Forward  ===============================================================================

void  forward()
{
  // calculation of points

  // Left Front Pivot
  a90  = (90 + da),
  a120 = (120 + da),
  a150 = (150 + da),
  a180 = (180 +  da);

  // Left Back Pivot
  b0 = (0 + db),
  b30 = (30 + db),
  b60  = (60 + db),
  b90 = (90 + db);

  // Right Back Pivot
  c90 = (90 +  dc),
  c120 = (120 + dc),
  c150 = (150 + dc),
  c180 = (180 + dc);

  // Right Front Pivot
  d0 = (0 + dd),
  d30 = (30 + dd),
  d60 = (60  + dd),
  d90 = (90 + dd);

  // set servo positions and speeds needed to  walk forward one step
  // (LFP,  LBP, RBP,  RFP, LFL, LBL, RBL, RFL, S1, S2,  S3, S4)
  srv(a180, b0 , c120, d60, 42,  33,  33,  42,  1,  3,  1,  1);
  srv(  a90, b30, c90,  d30, 6,   33,  33,  42,  3,  1,  1,  1);
  srv( a90, b30, c90,  d30, 42,  33,  33,  42,  3,  1,  1,  1);
  srv(a120, b60, c180, d0,  42,  33,  6,   42,  1,  1,  3,  1);
  srv(a120, b60, c180, d0,  42,  33,  33,  42,  1,  1,  3,  1);
  srv(a150, b90, c150, d90, 42,  33,  33,  6,   1,  1,  1,  3);
  srv(a150, b90, c150, d90, 42,  33,  33,  42,  1,  1,  1,  3);
  srv(a180, b0,  c120, d60, 42,  6,   33,  42,  1,  3,  1,  1);
  //srv(a180, b0,  c120, d60,  42,  15,  33,  42,  1,  3,  1,  1);
  test();
}

//== Back ==================================================================================

void  back ()
{
  // set servo positions and speeds needed to walk backward one  step
  // (LFP,  LBP, RBP,  RFP, LFL, LBL, RBL, RFL, S1, S2, S3, S4)
  srv(180,  0,  120, 60, 42, 33, 33, 42, 3,  1, 1, 1);
  srv(150, 90, 150, 90, 42, 18, 33,  42, 1,  3, 1, 1);
  srv(150, 90, 150, 90, 42, 33, 33, 42, 1,  3, 1, 1);
  srv(120,  60, 180, 0,  42, 33, 33, 6,  1,  1, 1, 3);
  srv(120, 60, 180, 0,  42, 33, 33,  42, 1,  1, 1, 3);
  srv(90,  30, 90,  30, 42, 33, 18, 42, 1,  1, 3, 1);
  srv(90,  30, 90,  30, 42, 33, 33, 42, 1,  1, 3, 1);
  srv(180, 0,  120, 60, 6,  33,  33, 42, 3,  1, 1, 1);

}

//== Left =================================================================================

void  turn_left ()
{
  // set servo positions and speeds needed to turn left one  step
  // (LFP,  LBP, RBP,  RFP, LFL, LBL, RBL, RFL, S1, S2, S3, S4)
  srv(150,  90, 90,  30, 42, 6,  33, 42, 1, 3, 1, 1);
  srv(150,  90, 90,  30, 42, 33,  33, 42, 1, 3, 1, 1);
  srv(120,  60, 180, 0,  42, 33, 6,  42, 1, 1, 3, 1);
  srv(120,  60, 180, 0,  42, 33, 33, 24, 1, 1, 3, 1);
  srv(90,   30, 150, 90,  42, 33, 33, 6,  1, 1, 1, 3);
  srv(90,   30, 150, 90, 42, 33, 33, 42, 1, 1, 1,  3);
  srv(180,  0,  120, 60, 6,  33, 33, 42, 3, 1, 1, 1);
  srv(180,  0,  120,  60, 42, 33, 33, 33, 3, 1, 1, 1);
}

//== Right ================================================================================

void  turn_right ()
{
  // set servo positions and speeds needed to turn right one  step
  // (LFP,  LBP, RBP,  RFP, LFL, LBL, RBL, RFL, S1, S2, S3, S4)
  srv(  90, 30, 150, 90, 6,  33, 33, 42, 3, 1, 1, 1);
  srv( 90, 30, 150, 90, 42, 33,  33, 42, 3, 1, 1, 1);
  srv(120, 60, 180, 0,  42, 33, 33, 6,  1, 1, 1, 3);
  srv(120, 60, 180, 0,  42, 33, 33, 42, 1, 1, 1, 3);
  srv(150, 90, 90,  30,  42, 33, 6,  42, 1, 1, 3, 1);
  srv(150, 90, 90,  30, 42, 33, 33, 42, 1, 1, 3,  1);
  srv(180, 0,  120, 60, 42, 6,  33, 42, 1, 3, 1, 1);
  srv(180, 0,  120,  60, 42, 33, 33, 42, 1, 3, 1, 1);
}

//== Center Servos ========================================================================

void  center_servos()
{
  myServo1.write(90);
  myServo2.write(90);
  myServo3.write(90);
  myServo4.write(90);
  myServo5.write(90);
  myServo6.write(90);
  myServo7.write(90);
  myServo8.write(90);

  int s11 = 90; // Front Left Pivot Servo
  int  s12 = 90; // Front Left Lift Servo
  int s21 = 90; // Back Left Pivot Servo
  int s22 = 90; // Back Left Lift Servo
  int s31 = 90; // Back Right Pivot Servo
  int s32 = 90; // Back Right Lift Servo
  int s41 = 90; // Front Right Pivot  Servo
  int s42 = 90; // Front Right Lift Servo
}

//== Increase Speed  ========================================================================

void  increase_speed()
{
  if (spd > 3)
    spd--;
}

//== Decrease  Speed ========================================================================

void  decrease_speed()
{
  if (spd < 50)
    spd++;
}

//== Srv ===================================================================================

void  srv( int  p11, int p21, int p31, int p41, int p12, int p22, int p32, int p42, int  sp1, int sp2, int sp3, int sp4)
{
  voice();
  // p11: Front Left Pivot  Servo
  // p21: Back Left Pivot Servo
  // p31: Back Right Pivot Servo
  // p41: Front Right Pivot Servo
  // p12: Front Left Lift Servo
  // p22:  Back Left Lift Servo
  // p32: Back Right Lift Servo
  // p42: Front Right  Lift Servo
  // sp1: Speed 1
  // sp2: Speed 2
  // sp3: Speed 3
  //  sp4: Speed 4

  // Multiply lift servo positions by manual height adjustment
  p12 = p12 + high * 3;
  p22 = p22 + high * 3;
  p32 = p32 + high * 3;
  p42 = p42 + high * 3;

  while ((s11 != p11) || (s21 != p21) || (s31 !=  p31) || (s41 != p41) || (s12 != p12) || (s22 != p22) || (s32 != p32) || (s42 !=  p42))
  {

    // Front Left Pivot Servo
    if (s11 < p11)            //  if servo position is less than programmed position
    {
      if ((s11 +  sp1) <= p11)
        s11 = s11 + sp1;      // set servo position equal to servo  position plus speed constant
      else
        s11 = p11;
    }

    if (s11 > p11)            // if servo position is greater than programmed position
    {
      if ((s11 - sp1) >= p11)
        s11 = s11 - sp1;      // set servo  position equal to servo position minus speed constant
      else
        s11  = p11;
    }

    // Back Left Pivot Servo
    if (s21 < p21)
    {
      if ((s21 + sp2) <= p21)
        s21 = s21 + sp2;
      else
        s21  = p21;
    }

    if (s21 > p21)
    {
      if ((s21 - sp2) >= p21)
        s21 = s21 - sp2;
      else
        s21 = p21;
    }

    //  Back Right Pivot Servo
    if (s31 < p31)
    {
      if ((s31 + sp3) <=  p31)
        s31 = s31 + sp3;
      else
        s31 = p31;
    }

    if (s31 > p31)
    {
      if ((s31 - sp3) >= p31)
        s31 = s31  - sp3;
      else
        s31 = p31;
    }

    // Front Right Pivot  Servo
    if (s41 < p41)
    {
      if ((s41 + sp4) <= p41)
        s41  = s41 + sp4;
      else
        s41 = p41;
    }

    if (s41 > p41)
    {
      if ((s41 - sp4) >= p41)
        s41 = s41 - sp4;
      else
        s41 = p41;
    }

    // Front Left Lift Servo
    if (s12 <  p12)
    {
      if ((s12 + sp1) <= p12)
        s12 = s12 + sp1;
      else
        s12 = p12;
    }

    if (s12 > p12)
    {
      if ((s12  - sp1) >= p12)
        s12 = s12 - sp1;
      else
        s12 = p12;
    }

    // Back Left Lift Servo
    if (s22 < p22)
    {
      if  ((s22 + sp2) <= p22)
        s22 = s22 + sp2;
      else
        s22 =  p22;
    }

    if (s22 > p22)
    {
      if ((s22 - sp2) >= p22)
        s22 = s22 - sp2;
      else
        s22 = p22;
    }

    //  Back Right Lift Servo
    if (s32 < p32)
    {
      if ((s32 + sp3) <=  p32)
        s32 = s32 + sp3;
      else
        s32 = p32;
    }

    if (s32 > p32)
    {
      if ((s32 - sp3) >= p32)
        s32 = s32  - sp3;
      else
        s32 = p32;
    }

    // Front Right Lift  Servo
    if (s42 < p42)
    {
      if ((s42 + sp4) <= p42)
        s42  = s42 + sp4;
      else
        s42 = p42;
    }

    if (s42 > p42)
    {
      if ((s42 - sp4) >= p42)
        s42 = s42 - sp4;
      else
        s42 = p42;
    }

    // Write Pivot Servo Values
    myServo1.write(s11  + da);
    myServo3.write(s21 + db);
    myServo5.write(s31 + dc);
    myServo7.write(s41  + dd);

    // Write Lift Servos Values
    myServo2.write(s12);
    myServo4.write(s22);
    myServo6.write(s32);
    myServo8.write(s42);

    delay(spd); // Delay  before next movement

  }//while
} //srv

void test() {
  ligth();
  movement();
  

  /*
         Se tiver luz{
         center_servos();
         Funo danar: dance();
         }
  */

  /*
     Se tiver  gestos{
     center_servos();
     Funo tchauzinho: wave();
     }
  */
  /*
     Se ouvir voz{
     center_servos();
     Funo sentar:  sit():
     }
  */
}

void ligth() {
  while (! APDS.colorAvailable())  {
    delay(3);
  }
  int r, g, b;

  // read the color
  APDS.readColor(r,  g, b);

  while (r > 20 || g > 20 || b > 20) {
    dance();
    while  (! APDS.colorAvailable()) {
      delay(3);
    }
    APDS.readColor(r,  g, b);
  }
  // print the values
  /* Serial.print("r = ");
    Serial.println(r);
    Serial.print("g = ");
    Serial.println(g);
    Serial.print("b =  ");
    Serial.println(b);
    Serial.println();*/
}

void movement()  {

}

void voice() {
  if (samplesRead) {

    // print samples  to the serial monitor or plotter
    for (int i = 0; i < samplesRead; i++) {
     // Serial.println(sampleBuffer[i]);
      if(sampleBuffer[i]>1000){
       // Serial.println("Ouvi: " + String(sampleBuffer[i]));
       center_servos();
       sit();
      }
    }

    // clear the read count
    samplesRead  = 0;
  }
}

void onPDMdata() {
  // query the number of bytes available
  int bytesAvailable = PDM.available();

  // read into the sample buffer
  PDM.read(sampleBuffer, bytesAvailable);

  // 16-bit, 2 bytes per sample
  samplesRead = bytesAvailable / 2;
}

Downloadable files

Circuit

Comments

Only logged in users can leave comments

sandromesquitamecatronica

0 Followers

•

0 Projects

Intro

Components and supplies

Tools and machines

Project description

Code

Downloadable files