/**********************************************
 * e-Gizmo PBot Uno Controller - MAZE.ino
 * 
 * This enables PBot to avoid obstacles and may
 * also solve mazes.
 * 
 * Reading the 3channel Collision sensors:
 * PBot.COL1_RIGHT();
 * PBot.COL2_CENTER();
 * PBot.COL3_LEFT();
 * Output reads:
 * No object detection = 0 or LOW;
 * Object detected     = 1 or HIGH;
 * 
 * Codes by
 * e-Gizmo Mechatronix Central
 * http://www.e-gizmo.net
 * March 17,2020
 ************************************************/
#include "eGizmo_PBot_Uno.h"
#include <Wire.h>


eGizmo_PBot_Uno PBot;


int NormalSpeed = 90;
int RotateSpeed = 90;
int LowSpeed = 120;
int Pause = 200;

void setup(){

  PBot.BEGIN(); //PBot begin and turn on all motors
}

void loop(){

  if(PBot.COL3_LEFT() == 0 && PBot.COL2_CENTER() == 0 && PBot.COL1_RIGHT() == 0){

    PBot.FORWARD(NormalSpeed);
  }
  if(PBot.COL3_LEFT() == 1 && PBot.COL2_CENTER() == 1 && PBot.COL1_RIGHT() == 1){

    PBot.REVERSE(NormalSpeed);
    delay(Pause);
    PBot.SPINRIGHT(RotateSpeed);
    delay(Pause);
  }
  if(PBot.COL3_LEFT() == 1 && PBot.COL2_CENTER() == 1){

    PBot.SPINRIGHT(LowSpeed);
    delay(500);
    PBot.FORWARD(NormalSpeed);
    delay(Pause);
  }
  if(PBot.COL2_CENTER() == 1  && PBot.COL1_RIGHT() == 1){
    
    PBot.SPINLEFT(LowSpeed);
    delay(500);
    PBot.FORWARD(NormalSpeed);
    delay(Pause);
  }
}