Bottle Detection with TI RSLK Max

#include "SimpleRSLK.h"
#include "QTRSensors.h"
#include "RSLK_MAX_Pins.h"

#include <stdio.h>

#define LED RED_LED
#define LED2 GREEN_LED

const int echo = 10;
const int trig = 9;

double duration;

int leftReading;
int rightReading;
int centerReading;


void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  delay(500);

  leftReading = 0;
  rightReading = 0;
  centerReading = 0;

  setupRSLK();

  
  pinMode(LED, OUTPUT);
  pinMode(echo, INPUT);
  pinMode(trig, OUTPUT);

  simpleCalibrate();

  
  delay(4000);
  enableMotor(BOTH_MOTORS);
  setMotorDirection(BOTH_MOTORS,MOTOR_DIR_FORWARD);
  setMotorSpeed(BOTH_MOTORS,10);
  
}

void simpleCalibrate() {
  int16_t count;
  Serial.println("Driver robot forward to calibrate sensors");

  setMotorDirection(BOTH_MOTORS,MOTOR_DIR_FORWARD);

  setMotorSpeed(BOTH_MOTORS,10);
  enableMotor(BOTH_MOTORS);

  count = getEncoderLeftCnt();
  while(getEncoderLeftCnt() < count + 200) {
    calLineSensor();
  }
  setMotorSpeed(BOTH_MOTORS,0);
  delay(1000);
  setMotorSpeed(BOTH_MOTORS,10);
  setMotorDirection(BOTH_MOTORS,MOTOR_DIR_BACKWARD);

  Serial.println("Driver robot forward to calibrate sensors");
  count = getEncoderLeftCnt();
  while(getEncoderLeftCnt() < count + 200) {
    calLineSensor();
  }  

  setMotorSpeed(BOTH_MOTORS,0);
  disableMotor(BOTH_MOTORS);
  setMotorDirection(BOTH_MOTORS,MOTOR_DIR_FORWARD);
}

void loop() {


  for(int x = 0;x<6;x++)
    {
      if(isBumpSwitchPressed(x) == true) {
        
        End();
      }
   }

 
  setMotorDirection(BOTH_MOTORS,MOTOR_DIR_FORWARD);
  setMotorSpeed(BOTH_MOTORS,10);
  bool inBound = ReadSurface();
  

  if (inBound == false)
  {
    //stop
    digitalWrite(LED, HIGH);
    setMotorSpeed(BOTH_MOTORS,0);
    setMotorDirection(BOTH_MOTORS,MOTOR_DIR_BACKWARD);
    delay(1000);

    //Back up
    while (inBound == false) 
    {
      setMotorSpeed(BOTH_MOTORS, 10);
      inBound = ReadSurface();
    }

    delay(1000);

    // Lastly Turn Around
    digitalWrite(LED, LOW);
    setMotorSpeed(LEFT_MOTOR, 0);
    int counter = 0;
    while (counter < 2750)
    {
      inBound = ReadSurface();
      delay(250);

      if (inBound == false)
      {
        counter == 4000; 
      }
      counter = counter + 250;
    }

    

    // Now Drive Forward
    setMotorSpeed(BOTH_MOTORS, 0);
    setMotorDirection(BOTH_MOTORS,MOTOR_DIR_FORWARD);
    setMotorSpeed(BOTH_MOTORS, 10);
  }
  else // Search for the bottle
  {
    centerReading = 1;
    leftReading = 1;
    rightReading = 1;
    
    //Move forward a second
    digitalWrite(LED, LOW);
    delay(500);

    setMotorSpeed(BOTH_MOTORS, 0);
    delay(500);
    centerReading = ReadDistance();

    if (centerReading == 0)
    {
      setMotorSpeed(LEFT_MOTOR, 10);
    
      delay(1000);
      setMotorSpeed(LEFT_MOTOR, 0);
      delay(500);
      rightReading = ReadDistance();

      if (rightReading == 0)
      {
        setMotorSpeed(RIGHT_MOTOR, 10);
    
        delay(2000);
        setMotorSpeed(RIGHT_MOTOR, 0);
        delay(500);
        leftReading = ReadDistance();

        if (leftReading == 0)
        {
          setMotorSpeed(LEFT_MOTOR, 10);
    
          delay(1000);
          setMotorSpeed(LEFT_MOTOR, 0);
          delay(500);
        }
        else if (leftReading < 20)
        {
          End();
        }
        
      }
      else if (rightReading < 20)
      {
        End();
      }
    }
    else if (centerReading < 20)
    {
      End();
    }
  }
}

bool ReadSurface()
{

  unsigned int lineSensor[8];
  getLineSensorCalValue(lineSensor);

  double averageReflectance = 0;
  for (int i = 0;i<8;i++) {
    averageReflectance = averageReflectance + lineSensor[i];
    //Serial.print(lineSensor[i]);
    //Serial.print(",");
  }

  averageReflectance = averageReflectance / 8;
  //Serial.println(averageReflectance);

  if (averageReflectance > 100)
  {
    return false;
  }
  else
  {
    return true;
  }
}

int ReadDistance()
{
  digitalWrite(trig, LOW);
  delayMicroseconds(2);

  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);

  duration = pulseIn(echo, HIGH);
  duration = duration*0.034/2;
   if (duration > 70)
    {
      return 0;
    }
    else
    {
      return duration;
    }
  
}

void End()
{
  while (true)
  {
    setMotorSpeed(BOTH_MOTORS, 0);
    disableMotor(BOTH_MOTORS);
    digitalWrite(LED2, HIGH);
    digitalWrite(LED, LOW);
  }
  
}