So I did calibrate it at the red tape but I was getting some pretty high numbers for some reason, so I switched to a different room with better lighting and got more reasonable numbers. Here is my full code:
/***************************************************************
Project: Autonomous Car
Function: A car that is able to drive by itself within a track and avoid
hitting obstacles and stopping for stop signs
****************************************************************/
// Library for the Ultrasonic Sensor
#include <NewPing.h>
// UltraSonic Sensor Pins
#define TRIGGER_PIN 12
#define ECHO_PIN 13
#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters)
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
// Libraries for Color Sensor
#include <Wire.h> //library for communicating with L2C device
#include <Adafruit_TCS34725.h> //library of commands for color sensor
// Instance of Color Sensor and interrupt variables
Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_2_4MS, TCS34725_GAIN_1X);
#define interruptPin 3
volatile boolean changeDetected = false;
// RGB Light
int redPin = 10;
int greenPin = 5;
int bluePin = 6;
// Motor Pins
#define PWM_A_Pin 9
#define Dir_A_Pin 8
#define PWM_B_Pin 11
#define Dir_B_Pin 4
// Directions values
const bool forward = HIGH;
const bool backward = LOW;
const int Speed = 140;
const int FowardTurnSpeed = 75;
const int backwardTurnSpeed = 75;
const int noSpeed = 0;
// Labels for Track and Off Track
#define black HIGH // The Ir sensor will get a high Signal if it is over black
#define white LOW // The Ir sensor will get a low Signal if it is over white
// Tracking Sensor Pins
const int rightTrackingPin = 2;
const int leftTrackingPin = 7;
// Tracking Sensors
bool leftSensor;
bool rightSensor;
void goForward(int delayTime)
{
digitalWrite(Dir_A_Pin, forward); // makes motor A spin forward
digitalWrite(Dir_B_Pin, forward); // makes motor B spin forward
analogWrite(PWM_A_Pin, Speed); // makes motor A spin at a given speed
analogWrite(PWM_B_Pin, Speed); // makes motor B spin at a given speed
delay(delayTime);
}
void goBackward(int delayTime)
{
Serial.println("Vehicle Moving Backward");
// Channel A and B spin backwards @ full speed
digitalWrite(Dir_A_Pin, backward); // makes motor A spin forward
digitalWrite(Dir_B_Pin, backward); // makes motor B spin forward
analogWrite(PWM_A_Pin, Speed); // makes motor A spin at a given speed
analogWrite(PWM_B_Pin, Speed); // makes motor B spin at a given speed
delay(delayTime);
}
void stop(int delayTime)
{
// Channel A and B spin forward but @ at no speed
digitalWrite(Dir_A_Pin, forward); // makes motor A spin forward
digitalWrite(Dir_B_Pin, forward); // makes motor B spin forward
digitalWrite(PWM_A_Pin, noSpeed); // motor A will not spin with a PWM of 0
digitalWrite(PWM_B_Pin, noSpeed); // motor B will not spin with a PWM of 0
delay(delayTime);
Serial.println("Vehicle STOPPED");
}
void turnLeft(int delayTime)
{
Serial.println("Vehicle turning Left");
digitalWrite(Dir_A_Pin, backward); // makes motor A spin backward
digitalWrite(Dir_B_Pin, forward); // makes motor B spin forward
analogWrite(PWM_A_Pin, backwardTurnSpeed); // motor A will spin at half speed
analogWrite(PWM_B_Pin, FowardTurnSpeed); // motor B will spin at half speed
delay(delayTime);
}
void turnRight(int delayTime)
{
Serial.println("Vehicle turning Right");
digitalWrite(Dir_A_Pin, forward); // makes motor A spin forward
digitalWrite(Dir_B_Pin, backward); // makes motor B spin forward
analogWrite(PWM_A_Pin, FowardTurnSpeed); // motor A will spin at half speed
analogWrite(PWM_B_Pin, backwardTurnSpeed); // motor B will spin backward at half speed
delay(delayTime);
}
// For RGB light
void color(unsigned char red, unsigned char green, unsigned char blue)
{
analogWrite(redPin, red);
analogWrite(greenPin, green);
analogWrite(bluePin, blue);
}
void getRawData_noDelay(uint16_t *r, uint16_t *g, uint16_t *b, uint16_t *c)
{
*c = tcs.read16(TCS34725_CDATAL);
*r = tcs.read16(TCS34725_RDATAL);
*g = tcs.read16(TCS34725_GDATAL);
*b = tcs.read16(TCS34725_BDATAL);
}
void Interrupt()
{
changeDetected = true;
}
void setup() // In setup() we will set the pin mode for all of the pins that we made and begin the Serial monitor
{
// Set up motor pin, pin mode
pinMode(Dir_A_Pin, OUTPUT);
pinMode(Dir_B_Pin, OUTPUT);
pinMode(PWM_A_Pin, OUTPUT);
pinMode(PWM_B_Pin, OUTPUT);
// RGB Light
pinMode(redPin, OUTPUT);
pinMode(greenPin, OUTPUT);
pinMode(bluePin, OUTPUT);
// Set up for the Track Sensor Pin, pin mode
pinMode(leftTrackingPin, INPUT); // Should we use INPUT_PULLUP here, for the tracking Pins?
pinMode(rightTrackingPin, INPUT);
// Begin Serial monitor
Serial.begin(9600);
// Color Sensor
pinMode(interruptPin, INPUT);
if (tcs.begin())
{
//Serial.println("Found sensor");
}
else
{
Serial.println("No TCS34725 found ... check your connections");
while (1); // halt!
}
attachInterrupt(digitalPinToInterrupt(interruptPin), Interrupt, FALLING);
//tcs.setInterrupt(true);
tcs.setIntLimits(170, 111);
//tcs.clearInterrupt();
}
void loop()
{
// Get Color Sensor Values
delay(100);
uint16_t r, g, b, c;
tcs.getRawData(&r, &g, &b, &c);
//getRawData_noDelay(&r, &g, &b, &c);
// Read values from tracking sensors
leftSensor = digitalRead(leftTrackingPin);
rightSensor = digitalRead(rightTrackingPin);
// Ultra Sonic ping
//delay(100); //delay 100ms
unsigned int uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).
// Convert ping time to distance in cm and print result (0 = outside set distance range)
int distance = uS / US_ROUNDTRIP_CM; // US_ROUNDTRIP_CM is not defined by the code anywhere, must be built in function to convert to cm
Serial.print(distance); // Print the distance to the serial monitor
Serial.println("cm");
// Print Color Sensor Values
Serial.print("Red: "); Serial.print(r);
Serial.print(", Green: "); Serial.print(g);
Serial.print(", Blue: "); Serial.println(b);
Serial.print(", Clear: "); Serial.println(c);
if(changeDetected == true)
{
if (r > g || r > b )
{
Serial.println("Red Detcted");
color(0, 255, 255); // Red for RGB light
stop(0);
}
tcs.clearInterrupt();
changeDetected = false;
}
else
{
if (distance == 0 || distance > 15) // checking for 0 because, if there is nothing for the ultrasonic to bounce sound off of, then it will default to 0
{
if (leftSensor == black && rightSensor == black) // Higher signal = lower light reflected back, meaning a darker surface
{
color(255, 0, 255); // green
goForward(0);
Serial.println();
Serial.println("Going Forward, both sensors on track");
Serial.println(leftSensor);
Serial.println(rightSensor);
}
else if (leftSensor == black && rightSensor == white) // Error where the car is turning the wrong way, unsure how to fix
{
color(0, 225, 255); // Orange
turnLeft(0);
Serial.println();
Serial.println("turning Left, one sensor off track");
Serial.println(leftSensor);
Serial.println(rightSensor);
}
// If the left sensor is off the track, turn right
else if (leftSensor == white && rightSensor == black)
{
color(255, 255, 0); // blue
turnRight(0);
Serial.println();
Serial.println("turning right, one sensor off track");
Serial.println(leftSensor);
Serial.println(rightSensor);
}
else if (leftSensor == white && rightSensor == white)
{
color(220, 255, 0); // Indigo
goBackward(0);
Serial.print("Off the track");
Serial.println(leftSensor);
Serial.println(rightSensor);
}
}
else if (distance <= 15)
{
Serial.println("Object Detected");
color(0, 255, 255); // red
stop(0);
}
}
}