Skip to content
Snippets Groups Projects
Commit 26f124c2 authored by Daniel Ye's avatar Daniel Ye
Browse files

Complete initial draft of line following with hue

parent 2660f372
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
.vscode/extensions.json deleted 100644 → 0
+ 0
− 10
View file @ 2660f372
{
// See http://go.microsoft.com/fwlink/?LinkId=827846
// for the documentation about the extensions.json format
"recommendations": [
"platformio.platformio-ide"
],
"unwantedRecommendations": [
"ms-vscode.cpptools-extension-pack"
]
}
.vscode/settings.json deleted 100644 → 0
+ 0
− 13
View file @ 2660f372
{
"files.associations": {
"optional": "cpp",
"istream": "cpp",
"ostream": "cpp",
"system_error": "cpp",
"array": "cpp",
"functional": "cpp",
"tuple": "cpp",
"type_traits": "cpp",
"utility": "cpp"
}
}
\ No newline at end of file
include/settings.h
+ 9
− 2
View file @ 26f124c2
......@@ -16,13 +16,14 @@ typedef struct {
} Rgb;
typedef struct {
Rgb min = Rgb{255, 255, 255};
Rgb max = Rgb{0, 0, 0};
Rgb min = Rgb{0, 0, 0};
Rgb max = Rgb{255, 255, 255};
} ColorSensorCal;
void printRGB(const Rgb& value);
void checkButtonPress();
void getColorValues(Rgb& left_value, Rgb& right_value);
float computeHue(const Rgb& rgb);
// mode 0(default) - break, 1 - coast
void motorLeftForward(int speed, bool mode = 0);
......@@ -34,3 +35,9 @@ typedef enum {
MODE_BREAK,
MODE_COAST
} MotorMode;
typedef enum {
CALIBRATE_BLACK,
CALIBRATE_WHITE,
CALIBRATE_READY,
} CalibrationState;
src/main.cpp
+ 273
− 151
View file @ 26f124c2
......@@ -4,185 +4,307 @@
#include "Adafruit_TCS34725.h"
// Color sensor
Adafruit_TCS34725 color_sensor_left = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_24MS, TCS34725_GAIN_4X);
Adafruit_TCS34725 color_sensor_right = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_24MS, TCS34725_GAIN_4X);
Adafruit_TCS34725 color_sensor_left = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_24MS, TCS34725_GAIN_60X);
Adafruit_TCS34725 color_sensor_right = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_24MS, TCS34725_GAIN_60X);
TwoWire Wire2 = TwoWire(PC9, PA8); // I2C channel 3 for right sensor
bool prev_button_state = true; // nominal high
int prev_button_press_time = 0;
bool enable_line_follow = false;
int calibration_state = -1; // 0 - black, 1 - white, -1 - calibration disabled
bool enable_calibration = false; // Applies calibration on the rgb values
CalibrationState calibration_state = CALIBRATE_BLACK;
int left_red_threshold = 95;
int right_red_threshold = 95;
int left_red_threshold = 8;
int right_red_threshold = 8;
ColorSensorCal color_cal_left;
ColorSensorCal color_cal_right;
void setup() {
Serial.begin(115200);
pinMode(MOTOR_LEFT_PIN_1, OUTPUT);
pinMode(MOTOR_LEFT_PIN_2, OUTPUT);
pinMode(MOTOR_RIGHT_PIN_1, OUTPUT);
pinMode(MOTOR_RIGHT_PIN_2, OUTPUT);
pinMode(USER_BTN, INPUT);
// Color sensor setup
// Left sensor on pins SDA=D14 SCK=D15 (default I2C channel 1)
if (color_sensor_left.begin()) {
Serial.println("Found left color sensor");
} else {
Serial.println("Left color sensor not found");
while (1); // halt!
}
// Right sensor on pins SDA=PC9 SCK=PA8
if (color_sensor_right.begin(TCS34725_ADDRESS, &Wire2)) {
Serial.println("Found right color sensor");
} else {
Serial.println("Right color sensor not found");
while (1); // halt!
}
void setup()
{
Serial.begin(115200);
pinMode(MOTOR_LEFT_PIN_1, OUTPUT);
pinMode(MOTOR_LEFT_PIN_2, OUTPUT);
pinMode(MOTOR_RIGHT_PIN_1, OUTPUT);
pinMode(MOTOR_RIGHT_PIN_2, OUTPUT);
pinMode(USER_BTN, INPUT);
// Color sensor setup
// Left sensor on pins SDA=D14 SCK=D15 (default I2C channel 1)
if (color_sensor_left.begin())
{
Serial.println("Found left color sensor");
}
else
{
Serial.println("Left color sensor not found");
while (1)
; // halt!
}
// Right sensor on pins SDA=PC9 SCK=PA8
if (color_sensor_right.begin(TCS34725_ADDRESS, &Wire2))
{
Serial.println("Found right color sensor");
}
else
{
Serial.println("Right color sensor not found");
while (1)
; // halt!
}
}
void loop() {
checkButtonPress();
// For testing motor
// int motor_speed = map(ir_left_value, 0, 1023, 0, 255);
// Serial.println(motor_speed);
// For testing motor
// if (!enable_line_follow) {
// motorLeftForward(LEFT_MOTOR_SPEED);
// motorRightForward(RIGHT_MOTOR_SPEED);
// } else {
// motorLeftBackward(LEFT_MOTOR_SPEED);
// motorRightBackward(RIGHT_MOTOR_SPEED);
// }
// Color sensor code
Rgb left_color = {};
Rgb right_color = {};
getColorValues(left_color, right_color);
// Serial.print("Left ");
// printRGB(left_color);
// Serial.print("\t");
// Serial.print("Right ");
// printRGB(right_color);
// Serial.print("\n");
// Enable line following
if (enable_line_follow) {
if (left_color.R < left_red_threshold) {
motorLeftForward(LEFT_MOTOR_SPEED);
} else {
motorLeftForward(0);
}
if (right_color.R < right_red_threshold) {
motorRightForward(RIGHT_MOTOR_SPEED);
} else {
motorRightForward(0);
}
}
void loop()
{
checkButtonPress();
// For testing motor
// int motor_speed = map(ir_left_value, 0, 1023, 0, 255);
// Serial.println(motor_speed);
// For testing motor
// if (!enable_line_follow) {
// motorLeftForward(LEFT_MOTOR_SPEED);
// motorRightForward(RIGHT_MOTOR_SPEED);
// } else {
// motorLeftBackward(LEFT_MOTOR_SPEED);
// motorRightBackward(RIGHT_MOTOR_SPEED);
// }
// Color sensor code
Rgb left_color = {};
Rgb right_color = {};
getColorValues(left_color, right_color);
float left_hue = computeHue(left_color);
float right_hue = computeHue(right_color);
float left_red_error = left_hue > 180 ? 360 - left_hue : left_hue;
float right_red_error = right_hue > 180 ? 360 - right_hue : right_hue;
Serial.print("Left ");
printRGB(left_color);
Serial.print(" Hue: ");
Serial.print(int(left_hue));
Serial.print(" ");
Serial.print("Right ");
Serial.print("Hue: ");
Serial.print(int(right_hue));
printRGB(right_color);
// Serial.print(" Left min ");
// printRGB(color_cal_left.min);
// Serial.print(" Left max ");
// printRGB(color_cal_left.max);
// Serial.print(" Right min ");
// printRGB(color_cal_right.min);
// Serial.print(" Right max ");
// printRGB(color_cal_right.max);
Serial.print("\n");
// Enable line following
if (enable_line_follow)
{
// if (left_color.R < left_red_threshold) {
// motorLeftForward(LEFT_MOTOR_SPEED);
// } else {
// motorLeftForward(0);
// }
// if (right_color.R < right_red_threshold) {
// motorRightForward(RIGHT_MOTOR_SPEED);
// } else {
// motorRightForward(0);
// }
if (left_red_error > left_red_threshold)
{
motorLeftForward(LEFT_MOTOR_SPEED);
}
else
{
motorLeftForward(0);
}
if (right_red_error > right_red_threshold)
{
motorRightForward(RIGHT_MOTOR_SPEED);
}
else
{
motorRightForward(0);
}
}
}
void printRGB(const Rgb& value) {
Serial.print("R:"); Serial.print(int(value.R));
Serial.print("\tG:"); Serial.print(int(value.G));
Serial.print("\tB:"); Serial.print(int(value.B));
void printRGB(const Rgb &value)
{
Serial.print("R:");
Serial.print(int(value.R));
Serial.print(" G:");
Serial.print(int(value.G));
Serial.print(" B:");
Serial.print(int(value.B));
}
// Check user button press
void checkButtonPress() {
bool current_button_state = digitalRead(USER_BTN);
// Trigger on falling edge
if(prev_button_state && !current_button_state && (millis() - prev_button_press_time > DEBOUNCE_TIME_MS)) {
prev_button_press_time = millis();
Serial.print("Pressed: ");
enable_line_follow = !enable_line_follow;
}
prev_button_state = current_button_state;
// calibration code
if(calibration_state == 0) {
// Calibrate black
Serial.print("Calibrating black; ");
getColorValues(color_cal_left.min, color_cal_right.min);
// Transition to calibrate white
calibration_state = 1;
} else if (calibration_state == 1) {
// Calibrate white
getColorValues(color_cal_left.max, color_cal_right.max);
// Transition back to calibrate black next button press
calibration_state = 0;
}
void checkButtonPress()
{
bool current_button_state = digitalRead(USER_BTN);
// Trigger on falling edge
if (prev_button_state && !current_button_state && (millis() - prev_button_press_time > DEBOUNCE_TIME_MS))
{
prev_button_press_time = millis();
Serial.print("Pressed: ");
if (calibration_state == CALIBRATE_READY)
{
enable_line_follow = !enable_line_follow;
}
// calibration code
if (calibration_state == CALIBRATE_BLACK)
{
// Calibrate black
Serial.print("Calibrating black");
getColorValues(color_cal_left.min, color_cal_right.min);
// Transition to calibrate white
calibration_state = CALIBRATE_WHITE;
}
else if (calibration_state == CALIBRATE_WHITE)
{
// Calibrate white
Serial.print("Calibrating white");
getColorValues(color_cal_left.max, color_cal_right.max);
// Transition to ready state
calibration_state = CALIBRATE_READY;
}
}
prev_button_state = current_button_state;
}
void getColorValues(Rgb& left_value, Rgb& right_value) {
// Turn on LEDs
color_sensor_left.setInterrupt(false);
color_sensor_right.setInterrupt(false);
// Delay 24ms to read
delay(24);
color_sensor_left.getRGB(&left_value.R, &left_value.G, &left_value.B);
color_sensor_right.getRGB(&right_value.R, &right_value.G, &right_value.B);
// Turn off LEDs
color_sensor_left.setInterrupt(true);
color_sensor_right.setInterrupt(true);
void getColorValues(Rgb &left_value, Rgb &right_value)
{
// Turn on LEDs
color_sensor_left.setInterrupt(false);
color_sensor_right.setInterrupt(false);
// Delay 50ms to read
delay(24);
color_sensor_left.getRGB(&left_value.R, &left_value.G, &left_value.B);
color_sensor_right.getRGB(&right_value.R, &right_value.G, &right_value.B);
// Turn off LEDs
color_sensor_left.setInterrupt(true);
color_sensor_right.setInterrupt(true);
if (calibration_state == CALIBRATE_READY)
{
// Calibrate left color value
left_value.R = constrain(map(left_value.R, color_cal_left.min.R, color_cal_left.max.R, 0, 255), 0, 255);
left_value.G = constrain(map(left_value.G, color_cal_left.min.G, color_cal_left.max.G, 0, 255), 0, 255);
left_value.B = constrain(map(left_value.B, color_cal_left.min.B, color_cal_left.max.B, 0, 255), 0, 255);
// Calibrate right color value
right_value.R = constrain(map(right_value.R, color_cal_right.min.R, color_cal_right.max.R, 0, 255), 0, 255);
right_value.G = constrain(map(right_value.G, color_cal_right.min.G, color_cal_right.max.G, 0, 255), 0, 255);
right_value.B = constrain(map(right_value.B, color_cal_right.min.B, color_cal_right.max.B, 0, 255), 0, 255);
}
}
float computeHue(const Rgb &rgb)
{
float red = rgb.R / 255;
float green = rgb.G / 255;
float blue = rgb.B / 255;
float max_component = max(max(red, green), blue);
float min_component = min(min(red, green), blue);
float hue;
float diff = max_component - min_component;
if (max_component == red)
{
hue = (green - blue) / (diff) + (green < blue ? 6.0 : 0);
}
else if (max_component == green)
{
hue = (blue - red) / (diff) + 2.0;
}
else
{
hue = (red - green) / (diff) + 4.0;
}
return hue * 60;
}
// Left motor
void motorLeftForward(int speed, bool mode) {
if (!mode) {
// Break
analogWrite(MOTOR_LEFT_PIN_1, 255);
analogWrite(MOTOR_LEFT_PIN_2, 255-speed);
} else {
// Coast
analogWrite(MOTOR_LEFT_PIN_1, 0);
analogWrite(MOTOR_LEFT_PIN_2, speed);
}
void motorLeftForward(int speed, bool mode)
{
if (!mode)
{
// Break
analogWrite(MOTOR_LEFT_PIN_1, 255);
analogWrite(MOTOR_LEFT_PIN_2, 255 - speed);
}
else
{
// Coast
analogWrite(MOTOR_LEFT_PIN_1, 0);
analogWrite(MOTOR_LEFT_PIN_2, speed);
}
}
void motorLeftBackward(int speed, bool mode) {
if (!mode) {
// Break
analogWrite(MOTOR_LEFT_PIN_2, 255);
analogWrite(MOTOR_LEFT_PIN_1, 255-speed);
} else {
// Coast
analogWrite(MOTOR_LEFT_PIN_2, 0);
analogWrite(MOTOR_LEFT_PIN_1, speed);
}
void motorLeftBackward(int speed, bool mode)
{
if (!mode)
{
// Break
analogWrite(MOTOR_LEFT_PIN_2, 255);
analogWrite(MOTOR_LEFT_PIN_1, 255 - speed);
}
else
{
// Coast
analogWrite(MOTOR_LEFT_PIN_2, 0);
analogWrite(MOTOR_LEFT_PIN_1, speed);
}
}
// Right motor
void motorRightForward(int speed, bool mode) {
if (!mode) {
// Break
analogWrite(MOTOR_RIGHT_PIN_1, 255);
analogWrite(MOTOR_RIGHT_PIN_2, 255-speed);
} else {
// Coast
analogWrite(MOTOR_RIGHT_PIN_1, 0);
analogWrite(MOTOR_RIGHT_PIN_2, speed);
}
void motorRightForward(int speed, bool mode)
{
if (!mode)
{
// Break
analogWrite(MOTOR_RIGHT_PIN_1, 255);
analogWrite(MOTOR_RIGHT_PIN_2, 255 - speed);
}
else
{
// Coast
analogWrite(MOTOR_RIGHT_PIN_1, 0);
analogWrite(MOTOR_RIGHT_PIN_2, speed);
}
}
void motorRightBackward(int speed, bool mode) {
if (!mode) {
// Break
analogWrite(MOTOR_RIGHT_PIN_2, 255);
analogWrite(MOTOR_RIGHT_PIN_1, 255-speed);
} else {
// Coast
analogWrite(MOTOR_RIGHT_PIN_2, 0);
analogWrite(MOTOR_RIGHT_PIN_1, speed);
}
void motorRightBackward(int speed, bool mode)
{
if (!mode)
{
// Break
analogWrite(MOTOR_RIGHT_PIN_2, 255);
analogWrite(MOTOR_RIGHT_PIN_1, 255 - speed);
}
else
{
// Coast
analogWrite(MOTOR_RIGHT_PIN_2, 0);
analogWrite(MOTOR_RIGHT_PIN_1, speed);
}
}
\ No newline at end of file
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment