4WD Rover with Sonar, Mega, RF24, L298N(W/ SELF DRIVE MODE)

1// 4  wheel rover, Elegoo Mega, RF24+, L298N x2
2// This rover is self   driving if you assign data[4] as an input and set it equal to the value of 1 
3
4#include   <nRF24L01.h>
5#include <RF24.h>
6#include <RF24_config.h>
7#include <SPI.h>   
8#include <printf.h>
9
10#include <Servo.h>   // radar servo 
11
12RF24   radio(9, 10);   // CE, CSN
13//const uint64_t pipe[][32] = {"0xE8E8F0F0E1LL",   "0xF0F0F0F0E8E8"};
14const byte addresses[][32] = {"13572" , "13513"};
15
16int   data[8] = { "001", "002", "003", "004", "005", "006", "007" , "008"   };
17int dataIn[2] = {"", ""}; 
18
19int spdA = 2;
20int out1 = 29;   //   MOTOR A  (front)
21int out2 = 27;
22
23int spdB = 3;
24int out3 = 25;    //   MOTOR B   (front)
25int out4 = 23;
26
27int spdC = 4;
28int out5 = 22;    //   MOTOR C   (back)       
29int out6 = 24;                        
30                                                                                                   
31int   spdD = 5;
32int out7 = 26;     // MOTOR D   (back)
33int out8 = 28;
34
35const   int trigPin = 47;
36const int echoPin = 45;
37
38long duration;
39int distance;
40int   distance2; 
41
42int i = 90;   // integer for angle of servo
43int mode = 0;   // int mode for self driving mode vs control mode. 
44
45Servo RadarServo;        
46
47                                        // radar integer 
48int calculateDistance()   {             // Function for calculating the distance measured by the Ultrasonic   sensor
49  
50  digitalWrite(trigPin, LOW); 
51  delayMicroseconds(2);
52  //   Sets the trigPin on HIGH state for 10 micro seconds
53  digitalWrite(trigPin, HIGH);   
54  delayMicroseconds(10);
55  digitalWrite(trigPin, LOW);
56  
57  duration   = pulseIn(echoPin, HIGH); // Reads the echoPin, returns the sound wave travel time   in microseconds
58  distance= duration*0.034/2;
59  
60  return distance;
61
62
63      }  
64     
65///////////////////////////////////////SETUP//////////////////////////////////////////
66
67void   setup() {
68
69  
70  Serial.begin(9600);
71  
72  radio.begin();
73  radio.openWritingPipe(addresses[1]);   
74  radio.openReadingPipe(1, addresses[0]);
75  radio.setPALevel(RF24_PA_HIGH);
76   radio.setDataRate(RF24_250KBPS);
77 // radio.enableDynamicPayloads();   
78   radio.setChannel(0x76); 
79 
80  pinMode(spdA, OUTPUT);    
81  pinMode(spdB,   OUTPUT);
82  pinMode(spdC, OUTPUT);    
83  pinMode(spdD, OUTPUT);
84  
85  pinMode(out1,   OUTPUT);    
86  pinMode(out2, OUTPUT);     
87  pinMode(out3, OUTPUT);     
88   pinMode(out4, OUTPUT);
89  pinMode(out5, OUTPUT);    
90  pinMode(out6, OUTPUT);      
91  pinMode(out7, OUTPUT);     
92  pinMode(out8, OUTPUT);
93
94  pinMode(trigPin,   OUTPUT);            // Sets the trigPin as an Output
95  pinMode(echoPin, INPUT);             // Sets the echoPin as an Input
96  
97  RadarServo.attach(49);             //   Defines on which pin is the servo motor attached
98  RadarServo.write(90); 
99   
100  }
101
102////////////////////////////////////////LOOP/////////////////////////////////////////////
103
104   void loop() { 
105    
106     distance = calculateDistance();            // lets   calculate distance again 
107   
108    Serial.print(i);                  // print   angle 
109    Serial.print(",");                // comma for processing string   read
110    Serial.print(distance);          // distance 
111    Serial.println(".");              // period for processing string read
112    
113    delay(5); 
114     radio.startListening();              // start listening
115    
116/**************************************************************************************/
117     
118    if (radio.available() ) {            // if the radio is available      
119       
120      while (radio.available() ) {              // while it is available   (required) 
121   
122     distance = calculateDistance();            // lets calculate   distance again 
123    
124    
125    Serial.print(i); 
126    Serial.print(",");
127     Serial.print(distance);
128    Serial.print(".");
129    
130//////////////////////////////////////////////////////////////////////////////////////////
131
132
133     
134    delay(5); 
135    radio.startListening();             // start listening   again              
136    
137    radio.read(&data, sizeof(data));    // read all   in the dataset, and size
138
139  
140    Serial.print("Drive mode"); 
141   Serial.print("\	");   
142     
143    Serial.print ("J1x: ");
144   Serial.print( data[0]);      
145    Serial.print("\	"); 
146  
147    Serial.print ("J1y: ");
148  Serial.println(data[1]);
149   Serial.print("\	");
150  
151    Serial.print ("J2x: ");
152  Serial.print(   data[2]);           // read the data for debugging, is in drive mode
153  Serial.print("\	");               // follow this format for easy data reading and testing
154  
155    Serial.print ("J2y: ");
156  Serial.print(data[3]);
157  Serial.print("\	");   
158  
159   Serial.print ("B3: ");
160  Serial.print(data[4]);
161  Serial.print("\	");   
162      
163   Serial.print ("B4: ");
164  Serial.print(data[5]);
165  Serial.print("\	");
166
167    Serial.print ("Pot2: ");
168  Serial.print(data[6]);
169  Serial.print("\	");
170
171    Serial.print ("Jpush1: ");
172  Serial.print(data[7]);
173  Serial.print("\	");
174   
175 ////////////////////////////////DRIVING MODE/////////////////////////////////
176   
177    
178    RadarServo.write(data[6]);    // use potentiometer for calibrating   initial direction of servo                                   
179   
180    if ((   data[1] == 129) || (data[1] == 128 ))   {      
181      
182      analogWrite(spdA,   0);           // added y axis here too 
183      analogWrite(spdB, 0);           
184       analogWrite(spdC, 0);    
185      analogWrite(spdD, 0);
186      
187      digitalWrite(out1,   LOW);         // Idle, (not moving)   
188      digitalWrite(out2, LOW);         //   See data received(CTRL + M.
189      digitalWrite(out3, LOW);
190      digitalWrite(out4,   LOW);                     
191      digitalWrite(out5, LOW);    
192      digitalWrite(out6,   LOW);     
193      digitalWrite(out7, LOW);     
194      digitalWrite(out8, LOW);
195     
196     }
197
198    if ( data[2] == 0  )  {        
199                          
200       analogWrite(spdA, 150);           
201      analogWrite(spdB, 150);              
202       analogWrite(spdC, 150);    
203      analogWrite(spdD, 150);                        
204       
205      digitalWrite(out1, HIGH);         // TURN LEFT   
206      digitalWrite(out2,   LOW);         
207      digitalWrite(out3, LOW);
208      digitalWrite(out4, HIGH);                      
209      digitalWrite(out5, HIGH);    
210      digitalWrite(out6,   LOW);     
211      digitalWrite(out7, LOW);     
212      digitalWrite(out8, HIGH);          
213    
214    }
215
216    if ( data[2] == 255 ) {      
217     
218       analogWrite(spdA, 150);           
219      analogWrite(spdB, 150);              
220       analogWrite(spdC, 150);    
221      analogWrite(spdD, 150);                        
222       
223      digitalWrite(out1, LOW);         // TURN RIGHT 
224      digitalWrite(out2,   HIGH);         
225      digitalWrite(out3, HIGH);
226      digitalWrite(out4, LOW);                      
227      digitalWrite(out5, LOW);    
228      digitalWrite(out6,   HIGH);     
229      digitalWrite(out7, HIGH);     
230      digitalWrite(out8,   LOW);         
231    
232    }
233   
234   
235
236    if ( data[1] == 255  )   {      
237     
238      analogWrite(spdA, 200);           
239      analogWrite(spdB,   200);              
240      analogWrite(spdC, 200);    
241      analogWrite(spdD,   200);                        
242      
243      digitalWrite(out1, HIGH);         
244       digitalWrite(out2, LOW);          // GO MOTORS
245      digitalWrite(out3,   HIGH);
246      digitalWrite(out4, LOW);                     
247      digitalWrite(out5,   HIGH);   
248      digitalWrite(out6, LOW);     
249      digitalWrite(out7, HIGH);      
250      digitalWrite(out8, LOW);         
251    
252    }
253
254   if (   data[1] == 0  ) {      
255     
256      analogWrite(spdA, 255);           
257       analogWrite(spdB, 255);              
258      analogWrite(spdC, 255);    
259       analogWrite(spdD, 255);                        
260      
261      digitalWrite(out1,   LOW);         
262      digitalWrite(out2, HIGH);          // BACKWARDS
263      digitalWrite(out3,   LOW);
264      digitalWrite(out4, HIGH);                     
265      digitalWrite(out5,   LOW);   
266      digitalWrite(out6, HIGH);     
267      digitalWrite(out7, LOW);      
268      digitalWrite(out8, HIGH);         
269    
270          }
271    
272
273   
274    
275   
276
277      
278
279    //////////////////////////////COLLISION   PREVENTION/////////////////////////////////////
280
281  
282    if ( distance <=   25 ) {      // if robot gets too close (10 cm) 
283 
284      analogWrite(spdA,   200);           
285      analogWrite(spdB, 200);              
286      analogWrite(spdC,   200);    
287      analogWrite(spdD, 200);                        
288                
289       digitalWrite(out1, LOW);         
290      digitalWrite(out2, HIGH);      //   BACKWARDS, with 6v motors you can drive full speed into walls
291      digitalWrite(out3,   LOW);
292      digitalWrite(out4, HIGH);                     
293      digitalWrite(out5,   LOW);   
294      digitalWrite(out6, HIGH);     
295      digitalWrite(out7, LOW);      
296      digitalWrite(out8, HIGH);  
297    
298     delay(5); 
299     radio.stopListening();   
300
301    for ( int i=89; i<=92; i++ )   {     // rotates the servo motor from   89 to 92 degrees
302                                         // a short scan range   between 89 to 92 because 
303     RadarServo.write(i);                     // we   want to be able to control again
304     delay(30);
305  
306     distance = calculateDistance();       // Calls a function for calculating the distance measured by the Ultrasonic   sensor for each degree
307  
308        dataIn[0] = i; 
309        dataIn[1] = distance;     // defined data array components, i is angle
310
311      radio.write(&dataIn,   sizeof(dataIn));  // write the radar data to the controller
312
313    
314
315           }
316
317     
318    }
319
320////////////////////////////SCAN RADAR   IF B3 IS PRESSED//////////////////////////////////
321
322      if ( data[4] ==   1 ) {                // if button 3 is pressed 
323
324    delay(5); 
325    radio.stopListening();
326    
327    for ( int i=90; i<=120; i++ )   {     // rotates the servo motor from   90 to 120 degrees
328
329     RadarServo.write(i++);
330     delay(30);
331  
332      distance = calculateDistance();         // Calls a function for calculating   the distance measured by the Ultrasonic sensor for each degree
333  
334        dataIn[0]   = i; 
335        dataIn[1] = distance; 
336
337      radio.write(&dataIn, sizeof(dataIn));   
338
339     }
340    
341    for ( int i=120; i>=60; i-- )   {     // rotates   the servo motor from 120 to 60 degrees
342
343     RadarServo.write(i--);
344     delay(30);
345   
346     distance = calculateDistance();         // Calls a function for calculating   the distance measured by the Ultrasonic sensor for each degree
347  
348        dataIn[0]   = i; 
349        dataIn[1] = distance; 
350
351      radio.write(&dataIn, sizeof(dataIn));   
352    
353         }
354
355   for ( int i=60; i<=90; i++ )   {     // rotates   the servo motor from 60 to 90 degrees
356
357     RadarServo.write(i++);
358     delay(30);
359   
360     distance = calculateDistance();         // Calls a function for calculating   the distance measured by the Ultrasonic sensor for each degree
361  
362        dataIn[0]   = i; 
363        dataIn[1] = distance; 
364
365      radio.write(&dataIn, sizeof(dataIn));   
366      delay(10); 
367
368        }
369     }
370    
371 ////////////////////////////SCAN   RADAR WIDER IF BUTTON 4 IS PRESSED ///////////////////  
372     
373 if ( data[5]   == 1 ) {         // if button 4 is pressed (longer radar scan)        
374
375    delay(5);   
376    radio.stopListening(); 
377  
378  for ( int i=90; i<=165; i++ )   {     //   rotates the servo motor from 90 to 165 degrees
379
380     RadarServo.write(i++);
381      delay(30);
382  
383     distance = calculateDistance();         // Calls a   function for calculating the distance measured by the Ultrasonic sensor for each   degree
384  
385        dataIn[0] = i; 
386        dataIn[1] = distance; 
387
388       radio.write(&dataIn, sizeof(dataIn));  
389      
390
391     
392     }
393      
394    for ( int i=165; i>=20; i-- )   {     // rotates the servo motor from   165 to 20 degrees
395
396     RadarServo.write(i--);
397     delay(30);
398  
399      distance = calculateDistance();         // Calls a function for calculating   the distance measured by the Ultrasonic sensor for each degree
400  
401        dataIn[0]   = i; 
402        dataIn[1] = distance; 
403
404      radio.write(&dataIn, sizeof(dataIn));   
405    
406
407     
408     }
409
410     
411    for ( int i=20; i<=90; i++   )   {     // rotates the servo motor from 20 to 90 degrees
412    
413     RadarServo.write(i);
414      delay(30);
415  
416     distance = calculateDistance();         // Calls a   function for calculating the distance measured by the Ultrasonic sensor for each   degree
417  
418        dataIn[0] = i; 
419        dataIn[1] = distance; 
420
421       radio.write(&dataIn, sizeof(dataIn));  
422      
423      
424
425           }   
426         }
427
428      
429     
430    
431    
432///////////////////SMART   CAR////////////////////////////////////////////////////////////////
433////////////////////////////////AUTO   MODE////////////////////////////////////////////////
434////////////////////////////////////////////////////////////////////////////////////////
435
436     if ( data[7] == 0 )        {               // if i press the first joystick   down like a button 
437   
438         mode = 1;              // changes mode to   1 
439   
440            while (mode == 1) {      // while the mode is 1 
441      
442                 distance = calculateDistance();         // Calls a function for   calculating the distance measured by the Ultrasonic sensor for each degre
443       
444   ///////////////////////////////////////////////////////////////////////////////////////
445   
446      
447                  
448       for (int i=90; i>=30; i-=30) {          //   scans 90 to 30 
449                  
450              Serial.print(i); 
451              RadarServo.write(i);   
452              delay(1000); 
453            
454            distance = calculateDistance();
455
456///////////////////////////////////////////////////////////////////////////////
457
458          while ( i == 90 ) {     // while it scans at 90 degrees.... 
459                 
460                  if ( distance>=40) {  // now we add if statements for distances
461                   
462                  distance = calculateDistance();
463                  RadarServo.write(90);   
464                  
465                  analogWrite(spdA, 200);           
466                   analogWrite(spdB, 200);              
467                  analogWrite(spdC,   200);    
468                  analogWrite(spdD, 200);                        
469                   
470                  digitalWrite(out1, HIGH);         
471                  digitalWrite(out2,   LOW);          // GO MOTORS
472                  digitalWrite(out3, HIGH);
473                  digitalWrite(out4,   LOW);                     
474                  digitalWrite(out5, HIGH);   
475                   digitalWrite(out6, LOW);     
476                  digitalWrite(out7,   HIGH);     
477                  digitalWrite(out8, LOW);       
478                  
479                   
480                              }   // end if dist > 40
481                        
482            if ( distance <= 25 ) {
483                
484                   analogWrite(spdA, 200);           
485                  analogWrite(spdB,   200);              
486                  analogWrite(spdC, 200);    
487                  analogWrite(spdD,   200);                        
488                            
489                  digitalWrite(out1,   LOW);         
490                  digitalWrite(out2, HIGH);      // BACKWARDS,   with 6v motors you can drive full speed into walls
491                  digitalWrite(out3,   LOW);
492                  digitalWrite(out4, HIGH);                     
493                  digitalWrite(out5,   LOW);   
494                  digitalWrite(out6, HIGH);     
495                  digitalWrite(out7,   LOW);     
496                  digitalWrite(out8, HIGH);  
497                  delay(400);   
498                
499                  analogWrite(spdA, 0);           // added   y axis here too 
500                  analogWrite(spdB, 0);           
501                  analogWrite(spdC,   0);    
502                  analogWrite(spdD, 0);
503                  
504                  digitalWrite(out1,   LOW);         // Idle, (not moving)   
505                  digitalWrite(out2, LOW);          // See data received(CTRL + M.
506                  digitalWrite(out3,   LOW);
507                  digitalWrite(out4, LOW);                     
508                  digitalWrite(out5,   LOW);    
509                  digitalWrite(out6, LOW);     
510                  digitalWrite(out7,   LOW);     
511                  digitalWrite(out8, LOW);
512                  delay(500);   
513                
514                     }  // close if d <25
515              
516              
517                   
518           if   (distance <= 40)   {
519                       
520                      analogWrite(spdA, 0);           //   added y axis here too 
521                      analogWrite(spdB, 0);           
522                       analogWrite(spdC, 0);    
523                      analogWrite(spdD,   0);
524                      
525                      digitalWrite(out1, LOW);          // Idle, (not moving)   
526                      digitalWrite(out2, LOW);          // See data received(CTRL + M.
527                      digitalWrite(out3,   LOW);
528                      digitalWrite(out4, LOW);                     
529                       digitalWrite(out5, LOW);    
530                      digitalWrite(out6,   LOW);     
531                      digitalWrite(out7, LOW);     
532                      digitalWrite(out8,   LOW);
533                      delay(500);  
534                      break;
535                       
536                      }
537                  } // end   while 90 
538                  
539//////////////////////////////////////////////////////////////////////////
540              
541        while ( i == 30 ) {   // lots of rinsing and repeating.   
542                 
543              if (distance>=40) {
544                      
545                       analogWrite(spdA, 150);           
546                      analogWrite(spdB,   150);              
547                      analogWrite(spdC, 150);    
548                      analogWrite(spdD,   150);                        
549                      
550                      digitalWrite(out1,   LOW);         // TURN RIGHT 
551                      digitalWrite(out2, HIGH);          
552                      digitalWrite(out3, HIGH);
553                      digitalWrite(out4,   LOW);                     
554                      digitalWrite(out5, LOW);    
555                       digitalWrite(out6, HIGH);     
556                      digitalWrite(out7,   HIGH);     
557                      digitalWrite(out8, LOW); 
558                      delay(500);
559                       
560                      analogWrite(spdA, 0);          
561                       analogWrite(spdB, 0);           
562                      analogWrite(spdC,   0);    
563                      analogWrite(spdD, 0);
564                      
565                       digitalWrite(out1, LOW);         // Idle, (not moving)   
566                       digitalWrite(out2, LOW);         // See data received(CTRL   + M.
567                      digitalWrite(out3, LOW);
568                      digitalWrite(out4,   LOW);                     
569                      digitalWrite(out5, LOW);    
570                       digitalWrite(out6, LOW);     
571                      digitalWrite(out7,   LOW);     
572                      digitalWrite(out8, LOW);
573                      delay(500);   
574                      break;
575                      
576                         }
577                          
578              if   (distance<=40)   {
579
580                      analogWrite(spdA,   0);           // added y axis here too 
581                      analogWrite(spdB,   0);           
582                      analogWrite(spdC, 0);    
583                      analogWrite(spdD,   0);
584                      
585                      digitalWrite(out1, LOW);          // Idle, (not moving)   
586                      digitalWrite(out2, LOW);          // See data received(CTRL + M.
587                      digitalWrite(out3,   LOW);
588                      digitalWrite(out4, LOW);                     
589                       digitalWrite(out5, LOW);    
590                      digitalWrite(out6,   LOW);     
591                      digitalWrite(out7, LOW);     
592                      digitalWrite(out8,   LOW);
593                      delay(500);  
594                      break;
595                       
596                             }         
597             
598               
599                      }       // end while 
600             
601////////////////////////////////////////////////////////////////////////////
602
603                }         // end for int 
604               
605///////////////////////////////////////////////////////////////////////////////
606                   
607       for (int i=30; i<=150; i+=30) {      // 30 degrees   to 150 
608                  
609              Serial.print(i); 
610              RadarServo.write(i);   
611              delay(1000); 
612            
613            distance = calculateDistance();
614
615//////////////////////////////////////////////////////////////////////////////
616
617     while ( i == 30 ) {
618                
619              if (distance>=40) {
620                       
621                      analogWrite(spdA, 150);           
622                       analogWrite(spdB, 150);              
623                      analogWrite(spdC,   150);    
624                      analogWrite(spdD, 150);                        
625                       
626                      digitalWrite(out1, LOW);         //   TURN RIGHT 
627                      digitalWrite(out2, HIGH);         
628                      digitalWrite(out3,   HIGH);
629                      digitalWrite(out4, LOW);                     
630                       digitalWrite(out5, LOW);    
631                      digitalWrite(out6,   HIGH);     
632                      digitalWrite(out7, HIGH);     
633                      digitalWrite(out8,   LOW); 
634                      delay(500);
635                      
636                      analogWrite(spdA,   0);          
637                      analogWrite(spdB, 0);           
638                      analogWrite(spdC,   0);    
639                      analogWrite(spdD, 0);
640                      
641                       digitalWrite(out1, LOW);         // Idle, (not moving)   
642                       digitalWrite(out2, LOW);         // See data received(CTRL   + M.
643                      digitalWrite(out3, LOW);
644                      digitalWrite(out4,   LOW);                     
645                      digitalWrite(out5, LOW);    
646                       digitalWrite(out6, LOW);     
647                      digitalWrite(out7,   LOW);     
648                      digitalWrite(out8, LOW);
649                      delay(500);   
650                      break;
651                      
652                         }
653                          
654              if   (distance<=40)   {
655
656                      analogWrite(spdA,   0);           // added y axis here too 
657                      analogWrite(spdB,   0);           
658                      analogWrite(spdC, 0);    
659                      analogWrite(spdD,   0);
660                      
661                      digitalWrite(out1, LOW);          // Idle, (not moving)   
662                      digitalWrite(out2, LOW);          // See data received(CTRL + M.
663                      digitalWrite(out3,   LOW);
664                      digitalWrite(out4, LOW);                     
665                       digitalWrite(out5, LOW);    
666                      digitalWrite(out6,   LOW);     
667                      digitalWrite(out7, LOW);     
668                      digitalWrite(out8,   LOW);
669                      delay(500);  
670                      break;
671                       
672                             }  
673                      }   // end while 30       
674     
675//////////////////////////////////////////////////////////////////////////////////////////////////
676      
677       while ( i == 90 ) {
678                 
679              if ( distance>=40)   {
680                  
681                  distance = calculateDistance();
682                   RadarServo.write(90); 
683                  
684                  analogWrite(spdA,   200);           
685                  analogWrite(spdB, 200);              
686                   analogWrite(spdC, 200);    
687                  analogWrite(spdD,   200);                        
688                  
689                  digitalWrite(out1,   HIGH);         
690                  digitalWrite(out2, LOW);          // GO MOTORS
691                   digitalWrite(out3, HIGH);
692                  digitalWrite(out4,   LOW);                     
693                  digitalWrite(out5, HIGH);   
694                   digitalWrite(out6, LOW);     
695                  digitalWrite(out7,   HIGH);     
696                  digitalWrite(out8, LOW);       
697                  
698                   
699                              }   // end if dist > 40
700                        
701            if ( distance <= 25 ) {
702                
703                   analogWrite(spdA, 200);           
704                  analogWrite(spdB,   200);              
705                  analogWrite(spdC, 200);    
706                  analogWrite(spdD,   200);                        
707                            
708                  digitalWrite(out1,   LOW);         
709                  digitalWrite(out2, HIGH);      // BACKWARDS,   with 6v motors you can drive full speed into walls
710                  digitalWrite(out3,   LOW);
711                  digitalWrite(out4, HIGH);                     
712                  digitalWrite(out5,   LOW);   
713                  digitalWrite(out6, HIGH);     
714                  digitalWrite(out7,   LOW);     
715                  digitalWrite(out8, HIGH);  
716                  delay(400);   
717                
718                  analogWrite(spdA, 0);           // added   y axis here too 
719                  analogWrite(spdB, 0);           
720                  analogWrite(spdC,   0);    
721                  analogWrite(spdD, 0);
722                  
723                  digitalWrite(out1,   LOW);         // Idle, (not moving)   
724                  digitalWrite(out2, LOW);          // See data received(CTRL + M.
725                  digitalWrite(out3,   LOW);
726                  digitalWrite(out4, LOW);                     
727                  digitalWrite(out5,   LOW);    
728                  digitalWrite(out6, LOW);     
729                  digitalWrite(out7,   LOW);     
730                  digitalWrite(out8, LOW);
731                  delay(500);   
732                 
733                     }  // close if d <25
734              
735              
736                   
737           if   (distance <= 40)   {
738                       
739                      analogWrite(spdA, 0);           //   added y axis here too 
740                      analogWrite(spdB, 0);           
741                       analogWrite(spdC, 0);    
742                      analogWrite(spdD,   0);
743                      
744                      digitalWrite(out1, LOW);          // Idle, (not moving)   
745                      digitalWrite(out2, LOW);          // See data received(CTRL + M.
746                      digitalWrite(out3,   LOW);
747                      digitalWrite(out4, LOW);                     
748                       digitalWrite(out5, LOW);    
749                      digitalWrite(out6,   LOW);     
750                      digitalWrite(out7, LOW);     
751                      digitalWrite(out8,   LOW);
752                      delay(500);  
753                      break;
754                       
755                      }
756                 } // end while   90
757                                      
758                      
759//////////////////////////////////////////////////////////////////////////////////////
760                 
761          while (i ==150) { 
762 
763                      distance   = calculateDistance();
764            
765                    if ( distance>=40)   {
766                    
767       
768                          analogWrite(spdA,   150);           
769                          analogWrite(spdB, 150);              
770                           analogWrite(spdC, 150);    
771                          analogWrite(spdD,   150);                        
772                          
773                          digitalWrite(out1,   HIGH);         // TURN LEFT   
774                          digitalWrite(out2, LOW);          
775                          digitalWrite(out3, LOW);
776                          digitalWrite(out4,   HIGH);                     
777                          digitalWrite(out5, HIGH);     
778                          digitalWrite(out6, LOW);     
779                          digitalWrite(out7,   LOW);     
780                          digitalWrite(out8, HIGH); 
781                          delay(500);
782                          
783                         
784                          analogWrite(spdA,   0);           // added y axis here too 
785                          analogWrite(spdB,   0);           
786                          analogWrite(spdC, 0);    
787                          analogWrite(spdD,   0);
788                          
789                          digitalWrite(out1,   LOW);         // Idle, (not moving)   
790                          digitalWrite(out2,   LOW);         // See data received(CTRL + M.
791                          digitalWrite(out3,   LOW);
792                          digitalWrite(out4, LOW);                     
793                           digitalWrite(out5, LOW);    
794                          digitalWrite(out6,   LOW);     
795                          digitalWrite(out7, LOW);     
796                          digitalWrite(out8,   LOW);
797                          delay(500);  
798                          break;
799                       
800                                         
801                  
802                      }   // end if dist > 40
803                
804                  
805             if ( distance<=40) {
806                      
807                      analogWrite(spdA,   150);           
808                      analogWrite(spdB, 150);              
809                       analogWrite(spdC, 150);    
810                      analogWrite(spdD,   150);                        
811                      
812                      digitalWrite(out1,   LOW);         // TURN AROUND  
813                      digitalWrite(out2, HIGH);          
814                      digitalWrite(out3, HIGH);
815                      digitalWrite(out4,   LOW);                     
816                      digitalWrite(out5, LOW);    
817                       digitalWrite(out6, HIGH);     
818                      digitalWrite(out7,   HIGH);     
819                      digitalWrite(out8, LOW); 
820                      delay(2000);
821                       
822                      analogWrite(spdA, 0);          
823                       analogWrite(spdB, 0);           
824                      analogWrite(spdC,   0);    
825                      analogWrite(spdD, 0);
826                      
827                       digitalWrite(out1, LOW);         // Idle, (not moving)   
828                       digitalWrite(out2, LOW);         // See data received(CTRL   + M.
829                      digitalWrite(out3, LOW);
830                      digitalWrite(out4,   LOW);                     
831                      digitalWrite(out5, LOW);    
832                       digitalWrite(out6, LOW);     
833                      digitalWrite(out7,   LOW);     
834                      digitalWrite(out8, LOW);
835                      delay(500);   
836                      break;
837                  
838                  }
839      
840               }   // end while 150 
841               
842////////////////////////////////////////////////////////////////////////   
843           
844           }    // end for int  
845                
846///////////////////////////////////////////////////////////////////////
847
848        for (int i=150; i>=90; i-=30) {
849                  
850              Serial.print(i);   
851              RadarServo.write(i); 
852              delay(1000); 
853            
854             distance = calculateDistance();
855            
856  //////////////////////////////////////////////////////////////////////////       
857        
858        while ( i == 90 ) {
859                 
860                 if   ( distance>=40) {
861                  
862                  distance = calculateDistance();
863                   RadarServo.write(90); 
864                  
865                  analogWrite(spdA,   200);           
866                  analogWrite(spdB, 200);              
867                   analogWrite(spdC, 200);    
868                  analogWrite(spdD,   200);                        
869                  
870                  digitalWrite(out1,   HIGH);         
871                  digitalWrite(out2, LOW);          // GO MOTORS
872                   digitalWrite(out3, HIGH);
873                  digitalWrite(out4,   LOW);                     
874                  digitalWrite(out5, HIGH);   
875                   digitalWrite(out6, LOW);     
876                  digitalWrite(out7,   HIGH);     
877                  digitalWrite(out8, LOW);       
878                  
879                   
880                              }   // end if dist > 40
881                        
882            if ( distance <= 25 ) {
883                
884                   analogWrite(spdA, 200);           
885                  analogWrite(spdB,   200);              
886                  analogWrite(spdC, 200);    
887                  analogWrite(spdD,   200);                        
888                            
889                  digitalWrite(out1,   LOW);         
890                  digitalWrite(out2, HIGH);      // BACKWARDS,   with 6v motors you can drive full speed into walls
891                  digitalWrite(out3,   LOW);
892                  digitalWrite(out4, HIGH);                     
893                  digitalWrite(out5,   LOW);   
894                  digitalWrite(out6, HIGH);     
895                  digitalWrite(out7,   LOW);     
896                  digitalWrite(out8, HIGH);  
897                  delay(400);   
898                
899                  analogWrite(spdA, 0);           // added   y axis here too 
900                  analogWrite(spdB, 0);           
901                  analogWrite(spdC,   0);    
902                  analogWrite(spdD, 0);
903                  
904                  digitalWrite(out1,   LOW);         // Idle, (not moving)   
905                  digitalWrite(out2, LOW);          // See data received(CTRL + M.
906                  digitalWrite(out3,   LOW);
907                  digitalWrite(out4, LOW);                     
908                  digitalWrite(out5,   LOW);    
909                  digitalWrite(out6, LOW);     
910                  digitalWrite(out7,   LOW);     
911                  digitalWrite(out8, LOW);
912                  delay(500);   
913                
914                     }  // close if d <25
915              
916              
917                   
918           if   (distance <= 40)   {
919                       
920                      analogWrite(spdA, 0);           //   added y axis here too 
921                      analogWrite(spdB, 0);           
922                       analogWrite(spdC, 0);    
923                      analogWrite(spdD,   0);
924                      
925                      digitalWrite(out1, LOW);          // Idle, (not moving)   
926                      digitalWrite(out2, LOW);          // See data received(CTRL + M.
927                      digitalWrite(out3,   LOW);
928                      digitalWrite(out4, LOW);                     
929                       digitalWrite(out5, LOW);    
930                      digitalWrite(out6,   LOW);     
931                      digitalWrite(out7, LOW);     
932                      digitalWrite(out8,   LOW);
933                      delay(500);  
934                      break;
935                       
936                      }
937                }  // end while   
938                
939////////////////////////////////////////////////////////////////////////              
940             
941             }  // end for int 
942      
943/////////////////////////////////////////////////////////////////////////                    
944                       
945             }  // while mode   =1
946            } // data 7
947         
948/////////////////////////////////////////////////////////////////////////
949                          
950                       }   // while rad avail
951                     }      // if rad avail
952                   }      // end loop
953                         
954                  
955               
956            
957  
958         
959          
960         
961                     
962                  
963               
964  
965    

1#include <nRF24L01.h>
2#include <printf.h>
3#include <RF24.h>
4#include  <RF24_config.h>
5#include <SPI.h>
6
7RF24 radio(9, 10);                        
8
9
10const  byte addresses[][32] = {"13572" , "13513"};
11
12// int data[10] = { "001",  "002", "003", "004", "005", "006", "007", "008", "009", "010"  };
13int data[6] = { "001", "002", "003", "004", "005", "006" };
14int  dataIn[2] = {" ", " "}; 
15
16const int J1xPin = A0;     
17const int J1yPin  = A1;      
18const int J2xPin = A2;     
19const int J2yPin = A3;
20
21//const  int B1Pin = 2;       
22//const int B2Pin = 3;
23const int B3Pin = 4;
24const  int B4Pin = 5;
25
26//const int Pot1Pin = A6; 
27//const int Pot2Pin = A7;
28
29//const  int Jpush1 = A4;
30//const int Jpush2 = A5;
31
32void setup() {
33  
34  Serial.begin(9600);
35  
36  radio.begin();                       
37  radio.setPALevel(RF24_PA_HIGH);     
38  radio.openWritingPipe(addresses[0]); 
39  radio.openReadingPipe(1, addresses[1]);  
40  radio.setDataRate(RF24_250KBPS);        
41  //radio.enableDynamicPayloads();             
42  radio.setChannel(0x76); 
43 
44  
45//  pinMode(Jpush1, INPUT);  
46 // pinMode(Jpush2, INPUT); 
47  
48 // pinMode(B1Pin,  INPUT_PULLUP);   
49//  pinMode(B2Pin,  INPUT_PULLUP);
50  pinMode(B3Pin,  INPUT_PULLUP);
51  pinMode(B4Pin,  INPUT_PULLUP); 
52  
53//  pinMode(Pot1Pin,  INPUT);
54//  pinMode(Pot2Pin,  INPUT);
55  
56}
57
58void loop() {
59
60 
61 delay(5); 
62 radio.stopListening();    
63   
64  data[0] = map(analogRead(J1xPin), 0,1023, 255, 0);
65  data[1]  = map(analogRead(J1yPin), 0,1023, 255, 0);
66  data[2] = map(analogRead(J2xPin),  0,1023, 255, 0);
67  data[3] = map(analogRead(J2yPin), 0,1023, 255, 0);
68  data[4]  = map(digitalRead(B3Pin), 1, 0, 0, 1);
69  data[5] = map(digitalRead(B4Pin), 1,  0, 0, 1);
70
71  radio.write(&data, sizeof(data));
72
73  //Serial.println("Sending  Joystick Data");      // use to debug, enable to verify data sent
74  delay(10);  
75
76   
77    radio.startListening(); 
78
79  while (radio.available() )  { 
80    
81    radio.read(&dataIn, sizeof(dataIn));  
82    
83      Serial.print(dataIn[0]);
84      Serial.print(",");
85      Serial.print(dataIn[1]);
86      Serial.print(".");  
87    delay(10); 
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   

1// to add sound file for blip https://poanchen.github.io/blog/2016/11/15/how-to-add-background-music-in-processing-3.0#:~:text=First%2C%20you%20need%20to%20have,wait%20for%20a%20few%20moments.
2
3
4import  processing.serial.*;               // imports library for serial communication
5import  java.awt.event.KeyEvent; // imports library for reading the data from the serial  port
6import java.io.IOException;
7Serial myPort;                         //  defines Object for Serial
8
9String ang="";
10String distance="";
11String  data="";
12
13int angle, dist;
14
15void setup() {
16  
17   size (600,  400); 
18   
19   
20   myPort = new Serial(this, Serial.list()[1], 9600);                     //  starts the serial communication
21   // myPort = new Serial(this, Serial.list()[0],  9600);  // 0 for other port          
22   myPort.bufferUntil('.');    // reads  the data from the serial port up to the character '.' before calling serialEvent
23   
24  background(0);
25}
26
27void draw() {
28  
29                              //for  the blur effect
30      fill(0,5);              //colour,opacity
31      noStroke();
32      rect(0, 0, width, height*0.93); 
33      
34      noStroke();
35      fill(0,255);
36      rect(0,height*0.93,width,height);                   // so that the text having  angle and distance doesnt blur out
37      
38      
39      drawRadar(); 
40      drawLine();
41      drawObject();
42      drawText();
43}
44
45
46void  serialEvent (Serial myPort) {                                                     //  starts reading data from the Serial Port
47                                                                                      //  reads the data from the Serial Port up to the character '.' and puts it into the  String variable "data".
48      data = myPort.readStringUntil('.');
49      data  = data.substring(0,data.length()-1);
50      
51      int index1 = data.indexOf(",");                                                    
52      ang= data.substring(0,  index1);                                                 
53      distance= data.substring(index1+1,  data.length());                            
54      
55      angle = int(ang);
56      dist = int(distance);
57      System.out.println(angle);
58}
59
60void  drawRadar()
61{
62    pushMatrix();
63    noFill();
64    stroke(10,255,10);        //green
65    strokeWeight(3);
66    
67    translate(width/2,height-height*0.06);
68    
69    line(-width/2,0,width/2,0);
70    
71     arc(0,0,(width*0.125),(width*0.125),PI,TWO_PI);
72     arc(0,0,(width*0.25),(width*0.25),PI,TWO_PI);
73     arc(0,0,(width*0.375),(width*0.375),PI,TWO_PI);
74     arc(0,0,(width*0.5),(width*0.5),PI,TWO_PI);
75     arc(0,0,(width*0.625),(width*0.625),PI,TWO_PI);
76     arc(0,0,(width*0.75),(width*0.75),PI,TWO_PI);
77     arc(0,0,(width*0.875),(width*0.875),PI,TWO_PI);
78     arc(0,0,(width*0.995),(width*0.995),PI,TWO_PI);
79    
80    line(0,0,(-width/2)*cos(radians(30)),(-width/2)*sin(radians(30)));
81    line(0,0,(-width/2)*cos(radians(60)),(-width/2)*sin(radians(60)));
82    line(0,0,(-width/2)*cos(radians(90)),(-width/2)*sin(radians(90)));
83    line(0,0,(-width/2)*cos(radians(120)),(-width/2)*sin(radians(120)));
84    line(0,0,(-width/2)*cos(radians(150)),(-width/2)*sin(radians(150)));
85    
86    stroke(175,255,175); 
87    strokeWeight(1);
88    line(0,0,(-width/2)*cos(radians(15)),(-width/2)*sin(radians(15)));
89    line(0,0,(-width/2)*cos(radians(45)),(-width/2)*sin(radians(45)));
90    line(0,0,(-width/2)*cos(radians(75)),(-width/2)*sin(radians(75)));
91    line(0,0,(-width/2)*cos(radians(105)),(-width/2)*sin(radians(105)));
92    line(0,0,(-width/2)*cos(radians(135)),(-width/2)*sin(radians(135)));
93    line(0,0,(-width/2)*cos(radians(165)),(-width/2)*sin(radians(165)));
94
95    popMatrix();
96}
97
98void drawLine() {
99  
100    pushMatrix();
101    
102    strokeWeight(9);
103    stroke(0,255,0);
104    translate(width/2,height-height*0.06);  
105    
106   line(0,0,(width/2)*cos(radians(angle)),(-width/2)*sin(radians(angle)));
107   
108
109    popMatrix();
110    
111}
112
113
114void drawObject() {
115  
116    pushMatrix();
117    
118    strokeWeight(9);
119    stroke(255,0,0);
120    translate(width/2,height-height*0.06);
121   
122    float pixleDist = (dist/80.0)*(width/2.0);                        //  covers the distance from the sensor from cm to pixels
123    float pd=(width/2)-pixleDist;
124    
125             
126    float x=-pixleDist*cos(radians(angle));
127    float  y=-pixleDist*sin(radians(angle));
128    
129    if(dist<=80)                                                  //  limiting the range to 40 cms
130    {                               
131       //line(0,0,pixleDist,0);  
132       line(-x,y,-x+(pd*cos(radians(angle))),y-(pd*sin(radians(angle))));
133    }
134    popMatrix();
135}
136
137void drawText()
138{
139    pushMatrix();
140    
141    fill(192,192,192);
142    textSize(10);
143    
144    text("10cm",(width/2.3)+(width*0.115),height*0.93);
145    text("20cm",(width/2.3)+(width*0.1775),height*0.93);
146    text("30cm",(width/2.3)+(width*0.24),height*0.93);
147    text("40cm",(width/2.3)+(width*0.3025),height*0.93);
148    text("50cm",(width/2.3)+(width*0.365),height*0.93);
149    text("60cm",(width/2.3)+(width*0.4275),height*0.93);
150    text("70cm",(width/2.3)+(width*0.49),height*0.93);
151    text("80cm",(width/2.3)+(width*0.5525),height*0.93);
152    
153    
154   
155    
156    
157    textSize(15);
158    text("Radar",width*0.08,height*0.99);
159    text("Angle :"+angle,width*0.45,height*0.99);
160    
161    if(dist<=80)  {
162      text("Distance :"+dist,width*0.7,height*0.99);
163    }
164      
165   translate(width/2,height-height*0.06);
166   textSize(15);
167   
168   text("  30°",(width/2)*cos(radians(30)),(-width/2)*sin(radians(30)));
169   text(" 60°",(width/2)*cos(radians(60)),(-width/2)*sin(radians(60)));
170   text("90°",(width/2)*cos(radians(91)),(-width/2)*sin(radians(90)));
171   text("120°",(width/2)*cos(radians(123)),(-width/2)*sin(radians(118)));
172   text("150°",(width/2)*cos(radians(160)),(-width/2)*sin(radians(150)));
173    
174    popMatrix();  
175  
176}

1// 4  wheel rover, Elegoo Mega, RF24+, L298N x2
2
3#include <nRF24L01.h>
4#include  <RF24.h>
5#include <RF24_config.h>   // add these RF24 libraries
6#include <SPI.h>  
7#include <printf.h>
8
9#include <Servo.h>   // include radar servo 
10
11RF24  radio(9, 10);   // CE, CSN pins on Mega or Uno etc
12
13const byte addresses[][32]  = {"13572" , "13513"}; // create 2 addresses,
14                                                   //  32 bytes max 
15                                                   
16// create  6 piece data array to receive from controller 
17int data[6] = { "001", "002",  "003", "004", "005", "006" }; 
18// create two piece array so we can send  radar data to controller
19int dataIn[2] = {"", ""}; 
20
21int spdA = 2;
22int  out1 = 29;   // MOTOR A  (front)
23int out2 = 27;
24
25int spdB = 3;
26int out3  = 25;    // MOTOR B   (front)
27int out4 = 23;                              //  these are motor control pins
28                                          // and  speed control pins on L298N
29int spdC = 4;
30int out5 = 22;    // MOTOR C   (back)       
31int out6 = 24;                        
32                                                                                                   
33int  spdD = 5;
34int out7 = 26;     // MOTOR D   (back)
35int out8 = 28;
36
37const  int trigPin = 47;
38const int echoPin = 45;           // pins for HC-SR04 Ultrasonic  module
39
40long duration;      // integer for time (for sound wave)
41int distance;       // distance is measured by bouncing sound wave (doppler)
42
43int i = 90;     // integer for angle of servo (we start here, facing forward)
44
45Servo  RadarServo;      // Servo (space) I named it "RadarServo"  
46
47
48// Notice  we can identify the next integer within curly brackets 
49
50int calculateDistance()  {   // Function for calculating the                                              //  distance measured by the Ultrasonic sensor
51  
52                         //ultrasonic  module will now perform distance test 
53  digitalWrite(trigPin, LOW); 
54  delayMicroseconds(2);
55  
56  // Sets the trigPin on HIGH state for 10 micro seconds
57  digitalWrite(trigPin,  HIGH); 
58  delayMicroseconds(10);
59  digitalWrite(trigPin, LOW);
60  
61  duration  = pulseIn(echoPin, HIGH); // Reads the echoPin, returns the sound                                     //  wave travel time in microseconds
62  distance= duration*0.034/2;  // divide by  2 for there and back 
63  
64  return distance;        // returns the distance  value
65
66
67     }  
68     
69////////////////////SETUP///////////////////////////////
70
71void  setup() {
72
73  
74  Serial.begin(9600);   // starts the serial monitor 
75  
76  radio.begin();         // starts the radio
77  radio.openWritingPipe(addresses[1]);     // need both, writes on address 1
78  radio.openReadingPipe(1, addresses[0]);  // reads on address 0
79  radio.setPALevel(RF24_PA_HIGH);          // sets power  level of rf24
80  radio.setDataRate(RF24_250KBPS);        // data rate is essential  
81  //radio.enableDynamicPayloads();   // not needed here 
82  radio.setChannel(0x76);           // setting the channel helps 
83  
84 
85  pinMode(spdA, OUTPUT);    
86  pinMode(spdB, OUTPUT);       // sets spd PWM pins as outputs
87  pinMode(spdC,  OUTPUT);    
88  pinMode(spdD, OUTPUT);
89  
90  pinMode(out1, OUTPUT);    
91  pinMode(out2, OUTPUT);     
92  pinMode(out3, OUTPUT);     
93  pinMode(out4,  OUTPUT);       // sets output pins on L298N for driving motors
94  pinMode(out5,  OUTPUT);    
95  pinMode(out6, OUTPUT);     
96  pinMode(out7, OUTPUT);     
97  pinMode(out8, OUTPUT);
98
99  pinMode(trigPin, OUTPUT);       // Sets the trigPin  as an Output
100  pinMode(echoPin, INPUT);        // Sets the echoPin as an Input
101  
102  RadarServo.attach(49); // Defines on which pin is the servo motor attached
103  RadarServo.write(90);  // writes servo to forward at start (forward)
104  
105  }
106
107////////////////////////////LOOP/////////////////////////////////////////////
108//////////////////CALCULATING  DISTANCE FROM HC-SR04////////////////////
109  void loop() { 
110    
111    distance  = calculateDistance();   // lets calculate distance with HCSR04
112    
113  
114    
115    Serial.print(i);             // now let's read those values
116    Serial.print(",");          // i is the angle
117    Serial.print(distance);     // distance is distance  
118    Serial.println(".");
119     
120    delay(5);                  // wait  5 ms
121    radio.startListening();   // start listening 
122
123    if (radio.available()  ) {     // if radio is available 
124      
125    
126      while (radio.available()  ) {  // while it is available 
127   
128     distance = calculateDistance();   //  calculate distance here too 
129    
130    Serial.print(i); 
131    Serial.print(",");           // print these values 
132    Serial.print(distance);
133    Serial.print(".");
134
135    if (distance >= 10) {      // if the distance is more than 10cm
136      
137    
138    delay(5);                  // wait 5ms
139    radio.startListening();   // start listening 
140    radio.read(&data, sizeof(data));    // read all in  the dataset, and size
141
142  
143    Serial.print("Drive mode"); 
144   Serial.print("\	");  
145     
146    Serial.print ("J1x: ");
147   Serial.print( data[0]);      
148   Serial.print("\	"); 
149  
150    Serial.print ("J1y: ");
151  Serial.println(data[1]);  // print remote control data sent from controller
152  Serial.print("\	");
153  
154    Serial.print ("J2x: ");
155  Serial.print( data[2]);      
156  Serial.print("\	");  
157  
158   Serial.print ("J2y: ");
159  Serial.print(data[3]);
160  Serial.print("\	");  
161  
162   Serial.print ("B3: ");
163  Serial.print(data[4]);
164  Serial.print("\	");  
165      
166   Serial.print ("B4: ");
167  Serial.print(data[5]);
168  Serial.print("\	");
169
170
171  delay(10);    // delay 10 ms for receiving 
172
173 /////////////////////////DRIVING  THE ROVER////////////////////////////////// 
174  
175 if ( data[1] == 129 )   {      // joystick1 y data as it comes in untouched
176      
177      analogWrite(spdA,  0);           
178      analogWrite(spdB, 0);           // no speed, no motion.  
179      analogWrite(spdC, 0);    
180      analogWrite(spdD, 0);
181      
182      digitalWrite(out1, LOW);         // Idle, (not moving)   
183      digitalWrite(out2,  LOW);         // See data received(CTRL + M.
184      digitalWrite(out3, LOW);
185      digitalWrite(out4, LOW);                     
186      digitalWrite(out5,  LOW);    
187      digitalWrite(out6, LOW);     
188      digitalWrite(out7, LOW);     
189      digitalWrite(out8, LOW);
190    
191     }
192
193    if ( data[2]  == 0  )  {        // if joystick 2 x data is left
194                          
195      analogWrite(spdA, 255);           
196      analogWrite(spdB, 255);              
197      analogWrite(spdC, 255);    
198      analogWrite(spdD, 255);                        
199      
200      digitalWrite(out1, HIGH);         // TURN LEFT   
201      digitalWrite(out2,  LOW);         
202      digitalWrite(out3, LOW);
203      digitalWrite(out4, HIGH);                     
204      digitalWrite(out5, HIGH);    
205      digitalWrite(out6,  LOW);     
206      digitalWrite(out7, LOW);     
207      digitalWrite(out8, HIGH);         
208    
209    }
210
211    if ( data[2] == 255 ) {      // if joystick  2 x data is right
212     
213      analogWrite(spdA, 255);           
214      analogWrite(spdB,  255);              
215      analogWrite(spdC, 255);    
216      analogWrite(spdD,  255);                        
217      
218      digitalWrite(out1, LOW);         //  TURN RIGHT 
219      digitalWrite(out2, HIGH);         
220      digitalWrite(out3,  HIGH);
221      digitalWrite(out4, LOW);                     
222      digitalWrite(out5,  LOW);    
223      digitalWrite(out6, HIGH);     
224      digitalWrite(out7, HIGH);     
225      digitalWrite(out8, LOW);         
226    
227    }
228  
229
230    if ( data[1] == 255  ) {      // if joystick 1 y data is forward
231     
232      analogWrite(spdA, 255);           
233      analogWrite(spdB, 255);              
234      analogWrite(spdC, 255);    
235      analogWrite(spdD, 255);                        
236      
237      digitalWrite(out1, HIGH);         
238      digitalWrite(out2, LOW);          // GO MOTORS
239      digitalWrite(out3, HIGH);
240      digitalWrite(out4,  LOW);                     
241      digitalWrite(out5, HIGH);   
242      digitalWrite(out6,  LOW);     
243      digitalWrite(out7, HIGH);     
244      digitalWrite(out8, LOW);         
245    
246    }
247
248if ( data[1] == 0  ) {             // if joystick  1 y data is down
249     
250      analogWrite(spdA, 255);           
251      analogWrite(spdB,  255);              
252      analogWrite(spdC, 255);    
253      analogWrite(spdD,  255);                        
254      
255      digitalWrite(out1, LOW);         
256      digitalWrite(out2, HIGH);          // BACKWARDS
257      digitalWrite(out3,  LOW);
258      digitalWrite(out4, HIGH);                     
259      digitalWrite(out5,  LOW);   
260      digitalWrite(out6, HIGH);     
261      digitalWrite(out7, LOW);     
262      digitalWrite(out8, HIGH);         
263    
264          }
265//////////////////////USE  BUTTON TO CONTROL RADAR ///////////////////////
266
267      if ( data[4] == 1 )  {    // IF BUTTON3 IS PRESSED, RUN RADAR, THEN STOP
268                               //  SO WE CAN DRIVE :)
269    delay(5); 
270    radio.stopListening(); 
271
272    for  ( int i=60; i<=120; i++ )   {     // rotates the servo motor from 60 to 120 degrees
273
274     RadarServo.write(i++);
275     delay(30);
276  
277     distance = calculateDistance();         // Calls a function for calculating the distance measured by the Ultrasonic  sensor for each degree
278  
279        dataIn[0] = i; 
280        dataIn[1] = distance;  
281
282      radio.write(&dataIn, sizeof(dataIn));  
283      RadarServo.write(90);
284
285
286     }
287     
288            
289         }
290      
291    }
292    
293    
294////////////////////////IF  DISTANCE <10CM BACK UP////////////////////////////
295
296  
297    if (distance  <= 10) {
298
299 
300      analogWrite(spdA, 255);           
301      analogWrite(spdB,  255);              
302      analogWrite(spdC, 255);    
303      analogWrite(spdD,  255);                        
304      
305      digitalWrite(out1, LOW);         
306      digitalWrite(out2, HIGH);          // BACKWARDS
307      digitalWrite(out3,  LOW);
308      digitalWrite(out4, HIGH);                     
309      digitalWrite(out5,  LOW);   
310      digitalWrite(out6, HIGH);     
311      digitalWrite(out7, LOW);     
312      digitalWrite(out8, HIGH);  
313        
314
315      distance = calculateDistance();
316      
317    delay(5); 
318    radio.stopListening(); 
319
320    for ( int i=89;  i<=92; i++ )   {     // rotates the servo motor from 89 to 92 degrees
321
322     RadarServo.write(i);
323     delay(30);
324  
325     distance = calculateDistance();         // Calls a  function for calculating the distance measured by the Ultrasonic sensor for each  degree
326  
327        dataIn[0] = i; 
328        dataIn[1] = distance; 
329
330      radio.write(&dataIn, sizeof(dataIn));  
331
332      RadarServo.write(90);  
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