Read_IRSensors.ino

Read_IRSensors.ino

#include "src/CCCDrive/CCCDrive.h"
#include "src/Pololu/QTRSensors.h"
#include "src/Button/Button.h"

CCCDrive Robot;
Button start_btn(7); //Connect momentary to GPIO 7
QTRSensors qtr;

int Running = 1;      //Set up variable to determine if the robot should be moving

void setup() {
  Serial.begin(115200);
  delay(30000);
  Serial.println("- Cyber City Circuits -");
  Serial.println("Robot Online and Ready");
  Serial.println("Waiting on button to begin");
  Wire.setSDA(12);
  Wire.setSCL(13);
  Wire.begin();
  Robot.begin();
  start_btn.begin();
  qtr.setTypeRC();
  qtr.setSensorPins((const uint8_t[]){15, 16, 17, 18, 19, 20, 21, 22}, 8);
  for (uint8_t i = 0; i < 250; i++)
  {
    qtr.calibrate();
    delay(20);
  }
  Serial.println("Calibration complete.");
}


void loop() {
  CheckforButton();   //Main command, do not remove! This monitors button press for robot state
  
  if(Running == 1) {  //If the robot should be moving, do things here
    uint16_t sensors[8];
    int16_t position = qtr.readLineWhite(sensors);
    int16_t error = position - 1000;
    Serial.println(error);
    if (error < 1000) {
      Serial.println("TURN LEFT");
      Robot.allstop();
      delay(200);
      Robot.leftturn(25);
      delay(200);
      Robot.allstop();
    }
    else if (error > 3000) {
      Serial.println("TURN RIGHT");
      Robot.allstop();
      delay(200);
      Robot.rightturn(25);
      delay(200);
      Robot.allstop();
    }
    else {
      Serial.println("OK");
      delay(200);
      Robot.driveforward(25);
      delay(200);
    }
    /**
    Robot.driveforward(50); 
    delay(5000);
    Robot.allstop();
    delay(5000);
    Robot.rightturn(50);
    delay(1000);
    Robot.drivereverse(50);
    delay(5000);
    Robot.diagonal(2,50);
    delay(1000);
    Robot.strafe(1, 60);
    delay(5000);
    Robot.allstop();
    */
  }
 
 
}
void CheckforButton() {
  switch (Running) {
    case 0:
      if(start_btn.pressed()) {
        Serial.println("Button Pressed, Beginning Routine");
        Running = 1;  
       
      }
      break;
    case 1:
      if(start_btn.pressed()) {
        Serial.println("Button Pressed, Stopping Routine");
        Running = 0;
        
      }
      break;
  }
}