API tools faq
paste
Advertisement
gersonfer

mod24

gersonfer
Aug 5th, 2019
108
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 9.68 KB | None | 0 0
raw download clone embed print report
  1. /* Modulo 24 - Seguidor de linhas
  2.  *  Curso de Robotica
  3.  *  Sketch para seguidor de linhas
  4.  *  Author: Gerson Ferreira
  5.  *  03-08-2019
  6.  */
  7. #include  <NewPing.h>                                                // ultrasonic library
  8.  
  9. #define   BAUD      9600                                             // serial baud rate
  10. #define   REF       11                                               // right engine forward
  11. #define   REB       10                                               // right engine backward
  12. #define   LEB       9                                                // left  engine backward
  13. #define   LEF       6                                                // left  engine forward
  14. #define   TRIGGER   2                                                // ultrasonic trigger pin                                            
  15. #define   ECHO      4                                                // ultrasonic echo pin
  16. #define   MAX_DIST  500                                              // max distance we want to ping for
  17. #define   MIN_DIST  15                                               // min distance in this project
  18. #define   TOO_CLOSE 5                                                // very close distance from an object
  19. #define   RANDOM    A0                                               // aleatory pin for randomSeed function
  20. #define   IRright   8                                                // right reflexive sensor
  21. #define   IRleft    7                                                // left reflexive sensor
  22. #define   BACKSPEED 100                                              // reverse speed
  23. #define   NORMSPEED 80                                               // normal speed
  24.  
  25.  
  26. NewPing   sonar(TRIGGER,  ECHO, MAX_DIST);                           // creates object for ultrasonic device
  27.  
  28. void  engineForwardAll(int engineSpeed);                             // both engines ahead with a specific speed
  29. void  engineReversesAll(int engineSpeed, int msWait);               // both engine backwards with a specific speed
  30. void  engineStopAll(int   msWait);                                   // stops all engines
  31. void  engineStopRight(int msWait);                                   // stops right engine
  32. void  engineStopLeft(int  msWait);                                   // stops left  engine
  33. void  engineTurnsRight(int engineSpeed,   int msWait);               // turns right at specific speed
  34. void  engineTurnsLeft(int engineSpeed,    int msWait);               // turns left  at specific speed
  35. void  engineRotatesRight(int engineSpeed, int msWait);               // rotates right at specific speed
  36. void  engineRotatesLeft(int engineSpeed,  int msWait);               // rotates left at specific speed
  37. void  engineRandomTurn(int engineSpeed,   int msWait);               // turns left or right according to random                          
  38.  
  39. void setup() {
  40.   pinMode(REF,  OUTPUT);                                         // output setting
  41.   pinMode(REB,  OUTPUT);
  42.   pinMode(LEB,  OUTPUT);
  43.   pinMode(LEF,  OUTPUT);
  44.  
  45.   randomSeed(analogRead(RANDOM));                                // read value
  46.  
  47.   engineStopAll(0);                                              // all wheels are parked at start
  48.   Serial.begin(BAUD);
  49. }
  50.  
  51. void loop() {
  52. /*  According to the exercise at class module #24:
  53.  *   1- engine advances according to black line
  54.  *   2- engine stops when it founds a cross-path
  55.  *   3- engine turns left or turn right according to random function and
  56.  *   4- loop
  57.  */
  58. /*  
  59.   if (sonar.ping_cm() < MIN_DIST && sonar.ping_cm()) {        // any object in front of me ?
  60.     Serial.println("Turns Left");
  61.     engineStopAll(800);                                          // engine stop
  62.     if (sonar.ping_cm() < TOO_CLOSE && sonar.ping_cm()) {    // too close
  63.       Serial.print("Backing up ... ");
  64.       Serial.println(sonar.ping_cm());
  65.       engineReversesAll(100, 100);                                // backwards to get some space
  66.     }
  67.     engineStopAll(50);
  68.     engineRandomTurn(80, 400);                                       // turns right or left according to random                                                  // give me a few time to turn
  69.   }
  70.   if (sonar.ping_cm() >= MIN_DIST || sonar.ping_cm() == 0) {                                                  
  71.     engineForwardAll(80);                                      // go straight
  72.   }
  73. */
  74.   if (!digitalRead(IRright) &&                                 // no black line
  75.       !digitalRead(IRleft)) {                                  // no black line
  76.     engineForwardAll(NORMSPEED);                               // go ahead    
  77.   }
  78.   if (digitalRead(IRright) &&                                  // black line at right
  79.       !digitalRead(IRleft)) {                                  // no black line at left
  80.     Serial.print("Black Line Right IR="); Serial.print(IRright); Serial.println( " ->left turn");
  81.     engineTurnsRight(NORMSPEED,  0);                            // turns left    
  82.   }
  83.   if (digitalRead(IRleft) &&                                 // black line at left
  84.       !digitalRead(IRright)) {
  85.     Serial.print("Black Line Left IR="); Serial.print(IRleft); Serial.println( " ->right turn");
  86.     engineTurnsLeft(NORMSPEED,  0);                           // turns right
  87.   }
  88.   if (digitalRead(IRright) &&                                  // a cross-path was found
  89.       digitalRead(IRleft)) {
  90.     Serial.print("Cross-Path IR="); Serial.print(IRleft); Serial.print(IRright); Serial.println( " ->turns left 180 degrees");
  91.     engineStopAll(2000);                                        // stop
  92.     engineReversesAll(100, 100);                                // backwards
  93.     engineTurnsLeft(NORMSPEED,  400);                         // turns left      
  94.   }
  95. }
  96.  
  97. void  engineForwardAll(int engineSpeed) {
  98.   // engine ahead with a specific speed
  99.   Serial.print("engineForwardAll "); Serial.println(engineSpeed);
  100.   analogWrite(REB, LOW);
  101.   analogWrite(LEB, LOW);
  102.   analogWrite(REF, engineSpeed);                              // right engine at a specific speed(values from LOW to HIGH)
  103.   analogWrite(LEF, engineSpeed);                              // left  engine at a specific speed
  104. }
  105.  
  106. void  engineReversesAll(int engineSpeed, int msWait) {
  107.   // engine backups with a specific speed
  108.   Serial.print("engineReversesAll "); Serial.print(msWait); Serial.println("ms");
  109.   analogWrite(REF, LOW);
  110.   analogWrite(LEF, LOW);
  111.   analogWrite(REB, engineSpeed);                              // right engine at a specific speed(values from LOW to HIGH)
  112.   analogWrite(LEB, engineSpeed);                              // left  engine at a specific speed
  113.   if (msWait) { delay(msWait); }
  114. }
  115.  
  116. void  engineStopAll(int msWait) {
  117.   // stops all engines
  118.   Serial.print("engineStopAll "); Serial.print(msWait); Serial.println("ms");                                      
  119.     analogWrite(REF, LOW);                                        
  120.     analogWrite(REB, LOW);
  121.     analogWrite(LEB, LOW);
  122.     analogWrite(LEF, LOW);
  123.     if (msWait) { delay(msWait); }
  124. }
  125.  
  126. void  engineStopRight(int msWait) {
  127.   // stops right engine
  128.   Serial.print("engineStopRight "); Serial.print(msWait); Serial.println("ms");                                      
  129.     analogWrite(REF, LOW);                                        
  130.     analogWrite(REB, LOW);
  131.     if (msWait) { delay(msWait); }
  132. }
  133.  
  134. void  engineStopLeft(int msWait) {
  135.   // stops left engine
  136.   Serial.print("engineStopLeft "); Serial.print(msWait); Serial.println("ms");                                    
  137.     analogWrite(LEF, LOW);                                        
  138.     analogWrite(LEB, LOW);
  139.     if (msWait) { delay(msWait); }
  140. }
  141.  
  142. void  engineTurnsRight(int engineSpeed, int msWait) {
  143.   // turns Right at specific speed
  144.   engineStopRight(0);
  145.   Serial.print("turnsRight "); Serial.print(engineSpeed); Serial.print(" "); Serial.println(msWait);
  146.   analogWrite(LEF, engineSpeed);
  147.   analogWrite(LEB, LOW);
  148.   if (msWait) { delay(msWait); }
  149. }
  150.  
  151. void  engineTurnsLeft(int engineSpeed,  int msWait) {
  152.   // turns left at specific speed
  153.   engineStopLeft(0);
  154.   Serial.print("turnsLeft "); Serial.print(engineSpeed); Serial.print(" "); Serial.println(msWait);
  155.   analogWrite(REF, engineSpeed);
  156.   analogWrite(REB, LOW);
  157.   if (msWait) { delay(msWait); }
  158. }
  159.  
  160. void  engineRotatesRight(int engineSpeed, int msWait) {
  161.   // rotates Right at specific speed
  162.   // rotate means move wheels in different directions
  163.   Serial.print("rotatesRight "); Serial.print(engineSpeed); Serial.print(" "); Serial.println(msWait);
  164.   analogWrite(LEB, LOW);
  165.   analogWrite(REF, LOW);  
  166.   analogWrite(REB, engineSpeed);
  167.   analogWrite(LEF, engineSpeed);
  168.   if (msWait) { delay(msWait); }
  169. }
  170.  
  171. void  engineRotatesLeft(int engineSpeed,  int msWait) {
  172.   // rotates Left at specific speed
  173.   // rotate means move wheels in different directions
  174.   Serial.print("rotatesLeft "); Serial.print(engineSpeed); Serial.print(" "); Serial.println(msWait);
  175.   analogWrite(LEF, LOW);
  176.   analogWrite(REB, LOW);
  177.   analogWrite(LEB, engineSpeed);
  178.   analogWrite(REF, engineSpeed);
  179.   if (msWait) { delay(msWait); }  
  180. }
  181.  
  182. void  engineRandomTurn(int engineSpeed, int msWait) {
  183.   int numberDrawn = random(2);                             // draws a number between 0 and 1
  184.   if (numberDrawn) {
  185.     engineTurnsLeft(engineSpeed,  0);                                         // turns left if nember == 1
  186.     Serial.print("randomTurn Left "); Serial.print(engineSpeed); Serial.print(" "); Serial.println(msWait);    
  187.   } else {
  188.     engineTurnsRight(engineSpeed, 0);                                        // turns right if number == 0
  189.     Serial.print("randomTurn Right "); Serial.print(engineSpeed); Serial.print(" "); Serial.println(msWait);
  190.   }
  191.   if (msWait) { delay(msWait); }
  192. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!