Kleurensensor op het Robotonderstel!

Vorige week expirimenteerde ik met de TCS230 TCS3200 Color sensor. Deze week heb ik die sensor gemonteerd op het robotonderstel van de bluetooth tank om zo een robot te maken die een gekleurde lijn kan volgen.

Schema

Voor het volledige schema van het rijdende onderstel verwijs ik je naar het artikel over de bluetooth tank
Dit schema laat alleen zien hoe ik de kleurensensor heb aangesloten (klik om te vergroten)

Sketch 1, alles draait om de eenvoud

In deze eerste sketch heb ik alles zo eenvoudig mogelijk gehouden.
// // Bart Venneker 2016 // Robot with color sensor, line follower // see https://www.bartvenneker.nl/Arduino/index.php?art=0022 for details // released into the public domain // #define L298N_IN1 7 #define L298N_IN2 6 #define L298N_IN3 5 #define L298N_IN4 4 #define C_out A1 #define C_s2 A2 #define C_s3 A3 void setup() { pinMode(C_s3,OUTPUT); pinMode(C_s2,OUTPUT); pinMode(C_out,INPUT); pinMode(L298N_IN1,OUTPUT); pinMode(L298N_IN2,OUTPUT); pinMode(L298N_IN3,OUTPUT); pinMode(L298N_IN4,OUTPUT); } void loop() { // folow the red track if (get_color() == 1) { driveTracks(1,0,1,0); // drive straight ahead while the line is red delay(50); } if (get_color() != 1) { // take action if the line is not red driveTracks(0,1,0,1);delay(50); // back up just a bit driveTracks(1,0,0,1); delay (200);driveTracks(0,0,0,0); // Look right if (get_color() != 1) { driveTracks(0,1,1,0);delay (400);driveTracks(0,0,0,0); // Look left } } } void driveTracks(bool IN1,bool IN2,bool IN3,bool IN4) { digitalWrite(L298N_IN1,IN1); digitalWrite(L298N_IN2,IN2); digitalWrite(L298N_IN3,IN3); digitalWrite(L298N_IN4,IN4); } int get_color() { byte color = 0 ; int red = 0; int green = 0; int blue = 0; // S2 S3 Filter // 0 0 red // 0 1 blue // 1 1 green digitalWrite(C_s2, LOW); // set filter to red digitalWrite(C_s3, LOW); // set filter to red while(digitalRead(C_out)){}; // wait for output to go LOW red = pulseIn(C_out, LOW); // get the value for red digitalWrite(C_s3, HIGH); // set filter to blue while(digitalRead(C_out)){}; // wait for output to go LOW blue = pulseIn(C_out, LOW); // get the value for blue digitalWrite(C_s2, HIGH); // set filter to green while(digitalRead(C_out)){}; // wait for output to go LOW green = pulseIn(C_out, LOW); // get the value for green if ((red<600) && (green<600) && (blue <600)) {color=4;} // color 4 = white else if (red < green && red < blue) {color = 1;} // red else if (green < red && green < blue) {color = 2;} // green else if (blue < red && blue < green) {color = 3;} // blue return color; }

Sketch 2, Beter in de bochten

Het nadeel van de eerste sketch is dat alle bochten benaderd worden als bochten naar RECHTS. Bochten naar links worden dus minder elegant genomen.
De volgende sketch lost dat op door te onthouden naar welke kant de bocht af lijkt te buigen en dat voortaan als standaard aan te houden.
// // Bart Venneker 2016 // Robot with color sensor, line follower // see https://www.bartvenneker.nl/Arduino/index.php?art=0022 for details // released into the public domain // #define L298N_IN1 7 #define L298N_IN2 6 #define L298N_IN3 5 #define L298N_IN4 4 #define C_out A1 #define C_s2 A2 #define C_s3 A3 void setup() { pinMode(C_s3,OUTPUT); pinMode(C_s2,OUTPUT); pinMode(C_out,INPUT); pinMode(L298N_IN1,OUTPUT); pinMode(L298N_IN2,OUTPUT); pinMode(L298N_IN3,OUTPUT); pinMode(L298N_IN4,OUTPUT); Serial.begin(9600); mySerial.begin(9600); } bool rightTurn=1; void loop() { // folow the red track if (get_color() == 1) { driveTracks(1,0,1,0); delay(50); } if (get_color() != 1) { driveTracks(0,1,0,1);delay(50); // backup a small bit if (rightTurn) { // Look right first driveTracks(1,0,0,1); delay (200);driveTracks(0,0,0,0); // Look right if (get_color() != 1) { driveTracks(0,1,1,0);delay (400);driveTracks(0,0,0,0); // Look left rightTurn=0; } } else { // Look left first driveTracks(0,1,1,0);delay (200);driveTracks(0,0,0,0); // Look left if (get_color() != 1) { driveTracks(1,0,0,1);delay (400);driveTracks(0,0,0,0); // Look right rightTurn=1; } } } } void driveTracks(bool IN1,bool IN2,bool IN3,bool IN4) { digitalWrite(L298N_IN1,IN1); digitalWrite(L298N_IN2,IN2); digitalWrite(L298N_IN3,IN3); digitalWrite(L298N_IN4,IN4); } int get_color() { byte color = 0 ; int red = 0; int green = 0; int blue = 0; // S2 S3 Filter // 0 0 red // 0 1 blue // 1 1 green digitalWrite(C_s2, LOW); // set filter to red digitalWrite(C_s3, LOW); // set filter to red while(digitalRead(C_out)){}; // wait for output to go LOW red = pulseIn(C_out, LOW); // get the value for red digitalWrite(C_s3, HIGH); // set filter to blue while(digitalRead(C_out)){}; // wait for output to go LOW blue = pulseIn(C_out, LOW); // get the value for blue digitalWrite(C_s2, HIGH); // set filter to green while(digitalRead(C_out)){}; // wait for output to go LOW green = pulseIn(C_out, LOW); // get the value for green if ((red<600) && (green<600) && (blue <600)) {color=4;} // color 4 = white else if (red < green && red < blue) {color = 1;} // red else if (green < red && green < blue) {color = 2;} // green else if (blue < red && blue < green) {color = 3;} // blue return color; }

Sketch 3, Blauw is beter dan Rood

Nu heb ik een tweede (blauw) parcours gemaakt die de rode kruist. Ik wil nu dat de robot het rode parcours volgt totdat hij het blauwe heeft gevonden en daarna alleen nog maar het blauwe volgt.


// // Bart Venneker 2016 // Robot with color sensor, line follower // see https://www.bartvenneker.nl/Arduino/index.php?art=0022 for details // released into the public domain // #define L298N_IN1 7 #define L298N_IN2 6 #define L298N_IN3 5 #define L298N_IN4 4 #define C_out A1 #define C_s2 A2 #define C_s3 A3 #define horn 8 void setup() { pinMode(C_s3,OUTPUT); pinMode(C_s2,OUTPUT); pinMode(C_out,INPUT); pinMode(L298N_IN1,OUTPUT); pinMode(L298N_IN2,OUTPUT); pinMode(L298N_IN3,OUTPUT); pinMode(L298N_IN4,OUTPUT); } bool rightTurn=1; byte targetColor = 1; void loop() { // folow the red track if (get_color() == targetColor) { driveTracks(1,0,1,0); delay(50); } if (targetColor ==1 && get_color() ==3) { driveTracks(0,0,0,0); delay(500); tone(horn,1000);delay(60); tone(horn,800);delay(80); noTone(horn); delay(500); targetColor = 3; } if (get_color() != targetColor) { driveTracks(0,1,0,1);delay(50); // backup a small bit if (rightTurn) { // Look right first driveTracks(1,0,0,1); delay (200);driveTracks(0,0,0,0); // Look right if (get_color() != targetColor) { driveTracks(0,1,1,0);delay (400);driveTracks(0,0,0,0); // Look left rightTurn=0; } } else { // Look left first driveTracks(0,1,1,0);delay (200);driveTracks(0,0,0,0); // Look left if (get_color() != targetColor) { driveTracks(1,0,0,1);delay (400);driveTracks(0,0,0,0); // Look right rightTurn=1; } } } } void driveTracks(bool IN1,bool IN2,bool IN3,bool IN4) { digitalWrite(L298N_IN1,IN1); digitalWrite(L298N_IN2,IN2); digitalWrite(L298N_IN3,IN3); digitalWrite(L298N_IN4,IN4); } int get_color() { byte color = 0 ; int red = 0; int green = 0; int blue = 0; // S2 S3 Filter // 0 0 red // 0 1 blue // 1 1 green digitalWrite(C_s2, LOW); // set filter to red digitalWrite(C_s3, LOW); // set filter to red while(digitalRead(C_out)){}; // wait for output to go LOW red = pulseIn(C_out, LOW); // get the value for red digitalWrite(C_s3, HIGH); // set filter to blue while(digitalRead(C_out)){}; // wait for output to go LOW blue = pulseIn(C_out, LOW); // get the value for blue digitalWrite(C_s2, HIGH); // set filter to green while(digitalRead(C_out)){}; // wait for output to go LOW green = pulseIn(C_out, LOW); // get the value for green if ((red<600) && (green<600) && (blue <600)) {color=4;} // color 4 = white else if (red < green && red < blue) {color = 1;} // red else if (green < red && green < blue) {color = 2;} // green else if (blue < red && blue < green) {color = 3;} // blue return color; }

Bekijk het op Youtube

In mijn video op youtube bespreek ik de code in meer detail. Kijk, Like en Subscribe! :-)