#include <Servo.h>
#include <NewPing.h>
#include <Adafruit_TCS34725.h>
Servo servoLeft;
Servo servoRight;
Servo servoGrab;
#define TRIGGER_PIN 7
#define ECHO_PIN 7
#define MAX_DISTANCE 200
#define commonAnode true
byte gammatable[256];
Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_4X);
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
bool grab = false;
void setup() {
Serial.begin(9600);
servoLeft.attach(5);
servoRight.attach(6);
servoGrab.attach(4);
servoGrab.write(90);
Serial.println("Color View Test!");
if (tcs.begin()) {
//Serial.println("Found sensor");
} else {
//Serial.println("No TCS34725 found ... check your connections");
while (1); // halt!
}
for (int i = 0; i < 256; i++) {
float x = i;
x /= 255;
x = pow(x, 2.5);
x *= 255;
if (commonAnode) {
gammatable[i] = 255 - x;
} else {
gammatable[i] = x;
}
//Serial.println(gammatable[i]);
}
}
void loop() {
aton();
}
void aton() {
Serial.print("Ping: ");
Serial.print(sonar.ping_cm()); // Send ping, get distance in cm and print result (0 = outside set distance range)
Serial.println("cm");
uint16_t clear, red, green, blue;
uint32_t sum = clear;
delay(60); // takes 50ms to read
tcs.getRawData(&red, &green, &blue, &clear);
// Serial.print("C:\t"); Serial.print(clear);
// Serial.print("\tR:\t"); Serial.print(red);
// Serial.print("\tG:\t"); Serial.print(green);
// Serial.print("\tB:\t"); Serial.print(blue);
// Figure out some basic hex code for visualization
float r, g, b;
r = red; r /= sum;
g = green; g /= sum;
b = blue; b /= sum;
r *= 256; g *= 256; b *= 256;
// Serial.print("\t");
// Serial.print((int)r, HEX); Serial.print((int)g, HEX); Serial.print((int)b, HEX);
// Serial.println();
if (red > 350 || green > 350 || blue > 350) {
servoLeft.writeMicroseconds(1500);
servoRight.writeMicroseconds(1500);
servoGrab.write(180);
delay(300);
grab = true;
}
else {
servoGrab.write(90);
}
while (grab == true) {
if (sonar.ping_cm() < 10) {
servoLeft.writeMicroseconds(1300);
servoRight.writeMicroseconds(1300);
delay(900);
if (sonar.ping_cm() < 10) {
servoLeft.writeMicroseconds(1500);
servoRight.writeMicroseconds(1500);
servoGrab.write(90);
delay(900);
servoLeft.writeMicroseconds(1300);
servoRight.writeMicroseconds(1300);
delay(1200)
grab = false;
}
} else if (sonar.ping_cm() > 10) {
servoLeft.writeMicroseconds(1700);
servoRight.writeMicroseconds(1300);
}
}
while (grab == false) {
if (sonar.ping_cm() > 10) {
servoLeft.writeMicroseconds(1700);
servoRight.writeMicroseconds(1300);
} else if (sonar.ping_cm() < 10) {
servoLeft.writeMicroseconds(1300);
servoRight.writeMicroseconds(1300);
delay(900);
}
}
}
Above is my code. I'm working on a shield bot for school and it needs to be able to pick up colored toy chickens. Whenever the colored chicken goes inside of my robot's grippers the Adafruit_TCS34725 color sensor activates. this bot is autonomous The bot stops itself from running into walls with a parallax sonar sensor. Whenever the color sensor activates, the sonar sensor goes crazy. I get readings that go from over 1000cm to 0cm back and fourth.