mod24

/* Modulo 24 - Seguidor de linhas
 *  Curso de Robotica
 *  Sketch para seguidor de linhas
 *  Author: Gerson Ferreira
 *  03-08-2019
 */
#include  <NewPing.h>                                                // ultrasonic library

#define   BAUD      9600                                             // serial baud rate
#define   REF       11                                               // right engine forward
#define   REB       10                                               // right engine backward
#define   LEB       9                                                // left  engine backward
#define   LEF       6                                                // left  engine forward
#define   TRIGGER   2                                                // ultrasonic trigger pin
#define   ECHO      4                                                // ultrasonic echo pin
#define   MAX_DIST  500                                              // max distance we want to ping for
#define   MIN_DIST  15                                               // min distance in this project
#define   TOO_CLOSE 5                                                // very close distance from an object
#define   RANDOM    A0                                               // aleatory pin for randomSeed function
#define   IRright   8                                                // right reflexive sensor
#define   IRleft    7                                                // left reflexive sensor
#define   BACKSPEED 100                                              // reverse speed
#define   NORMSPEED 80                                               // normal speed


NewPing   sonar(TRIGGER,  ECHO, MAX_DIST);                           // creates object for ultrasonic device

void  engineForwardAll(int engineSpeed);                             // both engines ahead with a specific speed
void  engineReversesAll(int engineSpeed, int msWait);               // both engine backwards with a specific speed
void  engineStopAll(int   msWait);                                   // stops all engines
void  engineStopRight(int msWait);                                   // stops right engine
void  engineStopLeft(int  msWait);                                   // stops left  engine
void  engineTurnsRight(int engineSpeed,   int msWait);               // turns right at specific speed
void  engineTurnsLeft(int engineSpeed,    int msWait);               // turns left  at specific speed
void  engineRotatesRight(int engineSpeed, int msWait);               // rotates right at specific speed
void  engineRotatesLeft(int engineSpeed,  int msWait);               // rotates left at specific speed
void  engineRandomTurn(int engineSpeed,   int msWait);               // turns left or right according to random

void setup() {
  pinMode(REF,  OUTPUT);                                         // output setting
  pinMode(REB,  OUTPUT);
  pinMode(LEB,  OUTPUT);
  pinMode(LEF,  OUTPUT);

  randomSeed(analogRead(RANDOM));                                // read value

  engineStopAll(0);                                              // all wheels are parked at start
  Serial.begin(BAUD);
}

void loop() {
/*  According to the exercise at class module #24:
 *   1- engine advances according to black line
 *   2- engine stops when it founds a cross-path
 *   3- engine turns left or turn right according to random function and
 *   4- loop
 */
/*
  if (sonar.ping_cm() < MIN_DIST && sonar.ping_cm()) {        // any object in front of me ?
    Serial.println("Turns Left");
    engineStopAll(800);                                          // engine stop
    if (sonar.ping_cm() < TOO_CLOSE && sonar.ping_cm()) {    // too close
      Serial.print("Backing up ... ");
      Serial.println(sonar.ping_cm());
      engineReversesAll(100, 100);                                // backwards to get some space
    }
    engineStopAll(50);
    engineRandomTurn(80, 400);                                       // turns right or left according to random                                                  // give me a few time to turn
  }
  if (sonar.ping_cm() >= MIN_DIST || sonar.ping_cm() == 0) {
    engineForwardAll(80);                                      // go straight
  }
*/
  if (!digitalRead(IRright) &&                                 // no black line
      !digitalRead(IRleft)) {                                  // no black line
    engineForwardAll(NORMSPEED);                               // go ahead
  }
  if (digitalRead(IRright) &&                                  // black line at right
      !digitalRead(IRleft)) {                                  // no black line at left
    Serial.print("Black Line Right IR="); Serial.print(IRright); Serial.println( " ->left turn");
    engineTurnsRight(NORMSPEED,  0);                            // turns left
  }
  if (digitalRead(IRleft) &&                                 // black line at left
      !digitalRead(IRright)) {
    Serial.print("Black Line Left IR="); Serial.print(IRleft); Serial.println( " ->right turn");
    engineTurnsLeft(NORMSPEED,  0);                           // turns right
  }
  if (digitalRead(IRright) &&                                  // a cross-path was found
      digitalRead(IRleft)) {
    Serial.print("Cross-Path IR="); Serial.print(IRleft); Serial.print(IRright); Serial.println( " ->turns left 180 degrees");
    engineStopAll(2000);                                        // stop
    engineReversesAll(100, 100);                                // backwards
    engineTurnsLeft(NORMSPEED,  400);                         // turns left
  }
}

void  engineForwardAll(int engineSpeed) {
  // engine ahead with a specific speed
  Serial.print("engineForwardAll "); Serial.println(engineSpeed);
  analogWrite(REB, LOW);
  analogWrite(LEB, LOW);
  analogWrite(REF, engineSpeed);                              // right engine at a specific speed(values from LOW to HIGH)
  analogWrite(LEF, engineSpeed);                              // left  engine at a specific speed
}

void  engineReversesAll(int engineSpeed, int msWait) {
  // engine backups with a specific speed
  Serial.print("engineReversesAll "); Serial.print(msWait); Serial.println("ms");
  analogWrite(REF, LOW);
  analogWrite(LEF, LOW);
  analogWrite(REB, engineSpeed);                              // right engine at a specific speed(values from LOW to HIGH)
  analogWrite(LEB, engineSpeed);                              // left  engine at a specific speed
  if (msWait) { delay(msWait); }
}

void  engineStopAll(int msWait) {
  // stops all engines
  Serial.print("engineStopAll "); Serial.print(msWait); Serial.println("ms");
    analogWrite(REF, LOW);
    analogWrite(REB, LOW);
    analogWrite(LEB, LOW);
    analogWrite(LEF, LOW);
    if (msWait) { delay(msWait); }
}

void  engineStopRight(int msWait) {
  // stops right engine
  Serial.print("engineStopRight "); Serial.print(msWait); Serial.println("ms");
    analogWrite(REF, LOW);
    analogWrite(REB, LOW);
    if (msWait) { delay(msWait); }
}

void  engineStopLeft(int msWait) {
  // stops left engine
  Serial.print("engineStopLeft "); Serial.print(msWait); Serial.println("ms");
    analogWrite(LEF, LOW);
    analogWrite(LEB, LOW);
    if (msWait) { delay(msWait); }
}

void  engineTurnsRight(int engineSpeed, int msWait) {
  // turns Right at specific speed
  engineStopRight(0);
  Serial.print("turnsRight "); Serial.print(engineSpeed); Serial.print(" "); Serial.println(msWait);
  analogWrite(LEF, engineSpeed);
  analogWrite(LEB, LOW);
  if (msWait) { delay(msWait); }
}

void  engineTurnsLeft(int engineSpeed,  int msWait) {
  // turns left at specific speed
  engineStopLeft(0);
  Serial.print("turnsLeft "); Serial.print(engineSpeed); Serial.print(" "); Serial.println(msWait);
  analogWrite(REF, engineSpeed);
  analogWrite(REB, LOW);
  if (msWait) { delay(msWait); }
}

void  engineRotatesRight(int engineSpeed, int msWait) {
  // rotates Right at specific speed
  // rotate means move wheels in different directions
  Serial.print("rotatesRight "); Serial.print(engineSpeed); Serial.print(" "); Serial.println(msWait);
  analogWrite(LEB, LOW);
  analogWrite(REF, LOW);
  analogWrite(REB, engineSpeed);
  analogWrite(LEF, engineSpeed);
  if (msWait) { delay(msWait); }
}

void  engineRotatesLeft(int engineSpeed,  int msWait) {
  // rotates Left at specific speed
  // rotate means move wheels in different directions
  Serial.print("rotatesLeft "); Serial.print(engineSpeed); Serial.print(" "); Serial.println(msWait);
  analogWrite(LEF, LOW);
  analogWrite(REB, LOW);
  analogWrite(LEB, engineSpeed);
  analogWrite(REF, engineSpeed);
  if (msWait) { delay(msWait); }
}

void  engineRandomTurn(int engineSpeed, int msWait) {
  int numberDrawn = random(2);                             // draws a number between 0 and 1
  if (numberDrawn) {
    engineTurnsLeft(engineSpeed,  0);                                         // turns left if nember == 1
    Serial.print("randomTurn Left "); Serial.print(engineSpeed); Serial.print(" "); Serial.println(msWait);
  } else {
    engineTurnsRight(engineSpeed, 0);                                        // turns right if number == 0
    Serial.print("randomTurn Right "); Serial.print(engineSpeed); Serial.print(" "); Serial.println(msWait);
  }
  if (msWait) { delay(msWait); }
}