Goliath Fall 2017
Color Sensor Line Following
Mark Huffman (Project Manager)
Line Following Algorithm
By Mark Huffman (Project Manager)
Figure 1: Different sensor detections and corrections followed
The theory behind line following with color sensors is the same reasoning used when using IR sensors. A sensor is placed on each side of the centerline and both sensors are constantly read back to adjust heading direction as based on which sensor is detecting.
Both Sensors White: On-the-line, continue forward straight line.
Right Sensor Black: Off-the-line, veer to the right.
Left Sensor Black: Off-the-line, veer to the left.
Both Sensors Black: On an intersection, detect it then continue straight.
To keep things looking smooth a really simple PID element was added. When correcting for a turn the motor speed is first influenced gradually based on continuous time of the error. This method is very basic and can be built upon to build an even more graceful correction algorithm.
Optimal Setup
By Mark Huffman (Project Manager)
Figure 2: Ideal sensor location with respect to drive wheels
For line following to look smooth and consistent, the color sensors should be placed just on either side of the line. The line in our case is (1 cm) so having the sensors between 1.5 cm and 3 cm apart will provide the best results. Secondly, for correction, the sensors should be placed in front of the main drive wheels. This allows corrects to be made as travelling forward and allows smaller corrections to keep close to the line.
Installation for Linefollower
By Mark Huffman (Project Manager)
For simplification of integration into other projects, the control code was organized into two simple files. Requiring both just “lineFollower.h” and “lineFollower.ino” to be located within the project. The code is designed to be called from a single line statement (taking in both translated sensor readings) and returns a flag stating when an intersection is observed. Plus a method the user defines called “atIntersection()” is called once everytime an intersection is first observed. Also, note that the internal function called “motorfControl()” will need to be modified depending on a particular robots movement method.
Installation Steps:
-
- Download the Raw Files here
- Add the file “lineFollower.h” and “lineFollower.ino” into your project folder then open your project
- Make sure you have #include “lineFollower.h”
- Initialize the linefollower using this (for non 3DOT)
lineFollower follower = lineFollower(); - OR Initialize the linefollower using this (for 3DOT)
lineFollower follower = lineFollower(true); - Within the main loop define
static boolean intersection = false;(if you wish to know when over an intersection and for intersection detection to work) - Call within the main loop to follow the line
(sensor values must be given as booleans Black = True & White = False)
intersection = follower.lineFollow(leftSensor, rightSensor, intersection); - Add the method:
void atIntersection() {
// Only called once per intersection and is called when a intersection is first observed
// *Note: Set another flag, to call a "turn" action method to process turing
}
9. Add the method:
void motorControl(uint8_t param[]){
// The main motor control method (Must be placed in the main file where motors are defined
// param[0] defines direction of motorA 0-4
// param[1] defines speed of motorA 0-255
// param[2] defines direction of motorB 0-4
// param[3] defines speed of motorB 0-255
motorA.go(param[0],param[1]);
motorB.go(param[2],param[3]);
}
Source Material
Code for Linefollower
By Mark Huffman (Project Manager)
lineFollower_example
#define __PROG_TYPES_COMPAT__
#include
#include
#include "Adafruit_TCS34725.h"
#include "lineFollower.h"
#include
#include
#include
#include #include
#include
#include
#include
//Make 3Dot Robot
Robot3DoTBoard Robot3Dot;
// Pins for all inputs, for TB6612 Motor Driver
#define IR1 A2
#define IR2 A1
#define IR3 A0
#define IR4 A3
#define AIN1 8
#define AIN2 9
#define PWMA 5
#define BIN1 7
#define BIN2 4
#define PWMB 6
#define STBY 16
// Address for I2C multiplexer
#define TCAADDR 0x70
//Define Color sensors using library
Adafruit_TCS34725 colorSensor = Adafruit_TCS34725();
//Import follower
lineFollower follower = lineFollower();
//Define 3Dot Motors
Motor motorA;
Motor motorB;
//Gobal variable to keep track of intersections
int intersections = 0;
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
//Initalize 3Dot
//Robot3Dot.begin();
//Configure Pins
InitPins();
}
void loop() {
// Flag set when over the location of an intersection
static boolean intersection = false;
// Main 3Dot Loop
// Robot3Dot.loop();
// read color from sensors
// translate to (Black or white)
// BLACK == TRUE
// WHITE == FALSE
boolean leftSensor = readSensor(0);
boolean rightSensor = readSensor(1);
// Call LineFollower Code
// Returns if a intersection is present
intersection = follower.lineFollow(leftSensor, rightSensor, intersection);
Serial.println(intersection);
}
void tcaselect(uint8_t i) {
//Selecting an address on I2C Mutiplexer
if (i > 7) return;
Wire.beginTransmission(TCAADDR);
Wire.write(1 << i);
Wire.endTransmission();
}
boolean readSensor(uint8_t index) {
// Read value from color sensor and determine reading
// if black reading = true
// if white reading = false
// Needs calibration for particular maze material
tcaselect(index);
colorSensor.begin();
uint16_t r, g, b, c, colorTemp, lux;
colorSensor.getRawData(&r, &g, &b, &c);
// Uncomment This part to calibrate for color reading
//Serial.print("R: "); Serial.print(r, DEC); Serial.print(" ");
//Serial.print("G: "); Serial.print(g, DEC); Serial.print(" ");
//Serial.print("B: "); Serial.print(b, DEC); Serial.print(" ");
//Serial.print("C: "); Serial.print(c, DEC); Serial.print(" ");Serial.println();
if (r < 70 || g < 70 || b < 70) // A color detectected that isn't white (Assume black)
return true;
return false;
}
void atIntersection() {
// Only called once per intersection and is called when a intersection is first observed
// *Note: Set another flag, to call a "turn" action method to process turing
// Example code follows:
intersections++;
Serial.println("Intersection!");
if (intersections == 3) {
digitalWrite(STBY, 0); // Operate
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, LOW);
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, LOW);
}
}
void atIntersection() {
// Only called once per intersection and is called when a intersection is first observed
// *Note: Set another flag, to call a "turn" action method to process turing
}
void InitPins()
{
// 3Dot Configuration
/*DDRD &= ~((1 << PD0) | (1 << PD1));
*DDRD |= ((1<<pd4)|(1<<pd6)|(1<<pd7));
*DDRF &= ~((1<<pf4)|(1<<pf5));
*DDRB |= ((1<<pb4)|(1<<pb5)|(1<<pb6));
*DDRC |= (1<<pc6);
*
*/
// Custom Setup
DDRF &= ~((1<<pf7)|(1<<pf6)|(1<<pf5)|(1<<pf4));
DDRB &= ~((1<<pb1)|(1<<pb2));
DDRC |= (1<<pc6);
DDRD |= (1<<pd7);
DDRE |= (1<<pe6);
DDRB |= ((1<<pb4)|(1<<pb5)|(1<<pb6));
// I2C Setup
PORTD |= (1<<pd1);
DDRD |= (1<<pd1);
}
lineFollower.h
//***************************************************************************//
// Name : Color-LineFollower.h //
// Author : Mark Huffman //
// Date : 10/30/17 //
// Version : 1.0 //
//***************************************************************************//
// 3Dot Library REQUIRED
#include
//Class Definition
class lineFollower {
public:
lineFollower();
lineFollower(Motor a, Motor b);
boolean lineFollow(boolean leftSensor, boolean rightSensor, boolean flag);
private:
int _intersections;
long _timeOfError;
long _intersectionDetect;
boolean _errorStart;
boolean _3Dot;
uint16_t _motorASpeed;
uint16_t _motorBSpeed;
Motor _motorA;
Motor _motorB;
void motorfControl(String dir);
};
lineFollower
//***************************************************************************//
// Name : Color-LineFollower Class //
// Author : Mark Huffman //
// Date : 10/30/17 //
// Version : 1.0 //
//***************************************************************************//
// 3Dot Library REQUIRED
#include
#ifndef lineFollower_H
#define lineFollower_H
/*========================================================================*/
/* CONSTRUCTORS */
/*========================================================================*/
lineFollower::lineFollower(){
_intersections = 0;
_timeOfError = 0;
_intersectionDetect = 0;
_errorStart = false;
_motorASpeed = 60;
_motorBSpeed = 60;
_3Dot = false;
motorfControl("Go");
}
lineFollower::lineFollower(Motor a, Motor b){
_intersections = 0;
_timeOfError = 0;
_intersectionDetect = 0;
_errorStart = false;
_motorASpeed = 60;
_motorBSpeed = 60;
_3Dot = true;
_motorA = a;
_motorB = b;
}
/*========================================================================*/
/* PUBLIC METHODS */
/*========================================================================*/
boolean lineFollower::lineFollow(boolean leftSensor, boolean rightSensor, boolean flag) {
// The line Following Algorithm
// Look at both the left and right sensor and determine type:
// - Left see black = Veer Left
// - Right see black = Veer Right
// - Both see white = following line Straight
// - Both see black = intersection
// Returns: True if both sensors see black (Intersection)
if (!leftSensor && !rightSensor) {
// White Space detected
flag = false;
_errorStart = false;
motorfControl("Foward");
}
else if (!leftSensor && rightSensor) {
if(!_errorStart)
_timeOfError = millis();
_errorStart = true;
if(_intersectionDetect > 0 && millis() - _intersectionDetect > 500){
flag = false;
_intersectionDetect = 0;
}
motorfControl("Veer Right");
}
else if (leftSensor && !rightSensor) {
if(!_errorStart)
_timeOfError = millis();
_errorStart = true;
if(_intersectionDetect > 0 && millis() - _intersectionDetect > 500){
flag = false;
_intersectionDetect = 0;
}
motorfControl("Veer Left");
}
else if (leftSensor && rightSensor) {
//On Cross line
motorfControl("Foward");
_errorStart = false;
if (!flag) {
_intersectionDetect = millis();
atIntersection();
flag = true;
}
}
return flag;
}
/*========================================================================*/
/* PRIVATE METHODS */
/*========================================================================*/
void lineFollower::motorfControl(String dir) {
// This method controls the motors independently by direction and speed
// *Note: MUST be reconfigured for particular bot motor configuration
// Assumes foward is foward for motors
// Param follows definiton for 3Dot motor control
uint8_t param[4];
if (dir == "Go") {
if(!_3Dot)
digitalWrite(STBY, 1); // Operate
} else if (dir == "Foward") {
if(!_3Dot){
digitalWrite(AIN1, HIGH);
digitalWrite(AIN2, LOW);
digitalWrite(BIN1, HIGH);
digitalWrite(BIN2, LOW);
}else{
param[0] = 0x01;
param[2] = 0x01;
}
_motorASpeed = 60;
_motorBSpeed = 60;
} else if (dir == "Veer Right") {
// PID method
// speed up B
if(millis() - _timeOfError > 30 && _errorStart)
_motorBSpeed -= 1;
else if(millis() - _timeOfError > 60 && _errorStart)
_motorBSpeed -= 2;
else if(millis() - _timeOfError > 100 && _errorStart){
_motorBSpeed = 57;
/* Hard Turn */
if(!_3Dot){
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, HIGH);
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, LOW);
}else{
param[0] = 0x03;
param[2] = 0x01;
}
}
_motorASpeed += 3;
} else if (dir == "Veer Left") {
// PID Method
// speed up A
if(millis() - _timeOfError > 30 && _errorStart)
_motorASpeed -= 1;
else if(millis() - _timeOfError > 60 && _errorStart)
_motorASpeed -= 2;
else if(millis() - _timeOfError > 100 && _errorStart){
_motorASpeed = 57;
/* Hard Turn */
if(!_3Dot){
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, HIGH);
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, LOW);
}else{
param[0] = 0x01;
param[2] = 0x03;
}
}
_motorBSpeed += 3;
} else if (dir == "Stop") {
if(!_3Dot){
digitalWrite(STBY, 0); // Operate
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, LOW);
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, LOW);
}else{
param[0] = 0x03;
param[2] = 0x03;
}
}
// Define motor speed
if(!_3Dot){
analogWrite(PWMA, _motorASpeed);
analogWrite(PWMB, _motorBSpeed);
}else{
param[1] = _motorASpeed;
param[3] = _motorBSpeed;
_motorA.go(param[0],param[1]);
_motorB.go(param[2],param[3]);
}
}
#endif