/**********************************************
   e-Gizmo PBot Uno Controller - SUMO.ino

   This is for collsion sample sketch to avoid
   lines and stays in a ring with 3 channel collsion
   sensors to bump the targets if detected.

   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 BumpSpeedLeft = 110;
int BumpSpeedCenter = 130;
int BumpSpeedRight = 110;
int ReverseSpeed = 100;
int RotateSpeed = 90;
int SafeSpeed = 80;
int LineSense = 0;
int GetBack = 0;

void setup() {

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

void loop() {

  //Attacking the opponent if the sensors detects it
  if (PBot.COL2_CENTER() == HIGH) {

    PBot.FORWARD(BumpSpeedCenter);
  }

  if (PBot.COL3_LEFT() == HIGH) {

    PBot.SPINLEFT(BumpSpeedLeft);
  }

  if (PBot.COL1_RIGHT() == HIGH) {

    PBot.SPINRIGHT(BumpSpeedRight);
  }

  if (PBot.COL1_RIGHT() == HIGH && PBot.COL3_LEFT() == HIGH) {

    PBot.FORWARD(BumpSpeedCenter);
  }

  //Avoiding the outside black line
  if (PBot.LS1_LEFT() == HIGH && PBot.LS3_RIGHT() == LOW) {

    PBot.TURNRIGHT(SafeSpeed);
    delay(100);
  }

  if (PBot.LS1_LEFT() == LOW && PBot.LS3_RIGHT() == HIGH) {

    PBot.TURNLEFT(SafeSpeed);
    delay(100);
  }

  if (PBot.LS1_LEFT() == HIGH && PBot.LS2_CENTER() == HIGH && PBot.LS3_RIGHT() == HIGH) {

    PBot.REVERSE();
    delay(200);
    PBot.SPINRIGHT(SafeSpeed);
    delay(900);
  }

  // If all sensors detects nothing
  if (PBot.COL3_LEFT() == LOW && PBot.COL2_CENTER() == LOW && PBot.COL1_RIGHT() == LOW) {

    PBot.FORWARD(SafeSpeed);
  }

  if (PBot.LS2_CENTER() == LOW) LineSense = 1;

  if (LineSense == 2) {
    PBot.SPINRIGHT();
    delay(900);
    GetBack = 0;
  }
  else
  {
    GetBack = 0;
    PBot.FORWARD(SafeSpeed);

  }

}