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 { ...@@ -16,13 +16,14 @@ typedef struct {
} Rgb; } Rgb;
typedef struct { typedef struct {
Rgb min = Rgb{255, 255, 255}; Rgb min = Rgb{0, 0, 0};
Rgb max = Rgb{0, 0, 0}; Rgb max = Rgb{255, 255, 255};
} ColorSensorCal; } ColorSensorCal;
void printRGB(const Rgb& value); void printRGB(const Rgb& value);
void checkButtonPress(); void checkButtonPress();
void getColorValues(Rgb& left_value, Rgb& right_value); void getColorValues(Rgb& left_value, Rgb& right_value);
float computeHue(const Rgb& rgb);
// mode 0(default) - break, 1 - coast // mode 0(default) - break, 1 - coast
void motorLeftForward(int speed, bool mode = 0); void motorLeftForward(int speed, bool mode = 0);
...@@ -34,3 +35,9 @@ typedef enum { ...@@ -34,3 +35,9 @@ typedef enum {
MODE_BREAK, MODE_BREAK,
MODE_COAST MODE_COAST
} MotorMode; } MotorMode;
typedef enum {
CALIBRATE_BLACK,
CALIBRATE_WHITE,
CALIBRATE_READY,
} CalibrationState;
src/main.cpp
+ 273
− 151
View file @ 26f124c2
...@@ -4,185 +4,307 @@ ...@@ -4,185 +4,307 @@
#include "Adafruit_TCS34725.h" #include "Adafruit_TCS34725.h"
// Color sensor // Color sensor
Adafruit_TCS34725 color_sensor_left = 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_4X); Adafruit_TCS34725 color_sensor_right = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_24MS, TCS34725_GAIN_60X);
TwoWire Wire2 = TwoWire(PC9, PA8); // I2C channel 3 for right sensor TwoWire Wire2 = TwoWire(PC9, PA8); // I2C channel 3 for right sensor
bool prev_button_state = true; // nominal high bool prev_button_state = true; // nominal high
int prev_button_press_time = 0; int prev_button_press_time = 0;
bool enable_line_follow = false; 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 left_red_threshold = 8;
int right_red_threshold = 95; int right_red_threshold = 8;
ColorSensorCal color_cal_left; ColorSensorCal color_cal_left;
ColorSensorCal color_cal_right; ColorSensorCal color_cal_right;
void setup() { void setup()
Serial.begin(115200); {
Serial.begin(115200);
pinMode(MOTOR_LEFT_PIN_1, OUTPUT);
pinMode(MOTOR_LEFT_PIN_2, OUTPUT); pinMode(MOTOR_LEFT_PIN_1, OUTPUT);
pinMode(MOTOR_RIGHT_PIN_1, OUTPUT); pinMode(MOTOR_LEFT_PIN_2, OUTPUT);
pinMode(MOTOR_RIGHT_PIN_2, OUTPUT); pinMode(MOTOR_RIGHT_PIN_1, OUTPUT);
pinMode(USER_BTN, INPUT); 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) // Color sensor setup
if (color_sensor_left.begin()) { // Left sensor on pins SDA=D14 SCK=D15 (default I2C channel 1)
Serial.println("Found left color sensor"); if (color_sensor_left.begin())
} else { {
Serial.println("Left color sensor not found"); Serial.println("Found left color sensor");
while (1); // halt! }
} else
{
// Right sensor on pins SDA=PC9 SCK=PA8 Serial.println("Left color sensor not found");
if (color_sensor_right.begin(TCS34725_ADDRESS, &Wire2)) { while (1)
Serial.println("Found right color sensor"); ; // halt!
} else { }
Serial.println("Right 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() { void loop()
checkButtonPress(); {
checkButtonPress();
// For testing motor
// int motor_speed = map(ir_left_value, 0, 1023, 0, 255); // For testing motor
// Serial.println(motor_speed); // int motor_speed = map(ir_left_value, 0, 1023, 0, 255);
// Serial.println(motor_speed);
// For testing motor
// if (!enable_line_follow) { // For testing motor
// motorLeftForward(LEFT_MOTOR_SPEED); // if (!enable_line_follow) {
// motorRightForward(RIGHT_MOTOR_SPEED); // motorLeftForward(LEFT_MOTOR_SPEED);
// } else { // motorRightForward(RIGHT_MOTOR_SPEED);
// motorLeftBackward(LEFT_MOTOR_SPEED); // } else {
// motorRightBackward(RIGHT_MOTOR_SPEED); // motorLeftBackward(LEFT_MOTOR_SPEED);
// } // motorRightBackward(RIGHT_MOTOR_SPEED);
// }
// Color sensor code
Rgb left_color = {}; // Color sensor code
Rgb right_color = {}; Rgb left_color = {};
getColorValues(left_color, right_color); Rgb right_color = {};
getColorValues(left_color, right_color);
// Serial.print("Left ");
// printRGB(left_color); float left_hue = computeHue(left_color);
// Serial.print("\t"); float right_hue = computeHue(right_color);
// 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);
}
}
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) { void printRGB(const Rgb &value)
Serial.print("R:"); Serial.print(int(value.R)); {
Serial.print("\tG:"); Serial.print(int(value.G)); Serial.print("R:");
Serial.print("\tB:"); Serial.print(int(value.B)); 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 // Check user button press
void checkButtonPress() { void checkButtonPress()
bool current_button_state = digitalRead(USER_BTN); {
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)) { // Trigger on falling edge
prev_button_press_time = millis(); if (prev_button_state && !current_button_state && (millis() - prev_button_press_time > DEBOUNCE_TIME_MS))
Serial.print("Pressed: "); {
enable_line_follow = !enable_line_follow; prev_button_press_time = millis();
} Serial.print("Pressed: ");
prev_button_state = current_button_state; if (calibration_state == CALIBRATE_READY)
{
// calibration code enable_line_follow = !enable_line_follow;
if(calibration_state == 0) { }
// Calibrate black
Serial.print("Calibrating black; "); // calibration code
getColorValues(color_cal_left.min, color_cal_right.min); if (calibration_state == CALIBRATE_BLACK)
// Transition to calibrate white {
calibration_state = 1; // Calibrate black
} else if (calibration_state == 1) { Serial.print("Calibrating black");
// Calibrate white getColorValues(color_cal_left.min, color_cal_right.min);
getColorValues(color_cal_left.max, color_cal_right.max); // Transition to calibrate white
// Transition back to calibrate black next button press calibration_state = CALIBRATE_WHITE;
calibration_state = 0; }
} 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) { void getColorValues(Rgb &left_value, Rgb &right_value)
// Turn on LEDs {
color_sensor_left.setInterrupt(false); // Turn on LEDs
color_sensor_right.setInterrupt(false); color_sensor_left.setInterrupt(false);
// Delay 24ms to read color_sensor_right.setInterrupt(false);
delay(24); // Delay 50ms to read
color_sensor_left.getRGB(&left_value.R, &left_value.G, &left_value.B); delay(24);
color_sensor_right.getRGB(&right_value.R, &right_value.G, &right_value.B); color_sensor_left.getRGB(&left_value.R, &left_value.G, &left_value.B);
// Turn off LEDs color_sensor_right.getRGB(&right_value.R, &right_value.G, &right_value.B);
color_sensor_left.setInterrupt(true); // Turn off LEDs
color_sensor_right.setInterrupt(true); 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 // Left motor
void motorLeftForward(int speed, bool mode) { void motorLeftForward(int speed, bool mode)
if (!mode) { {
// Break if (!mode)
analogWrite(MOTOR_LEFT_PIN_1, 255); {
analogWrite(MOTOR_LEFT_PIN_2, 255-speed); // Break
} else { analogWrite(MOTOR_LEFT_PIN_1, 255);
// Coast analogWrite(MOTOR_LEFT_PIN_2, 255 - speed);
analogWrite(MOTOR_LEFT_PIN_1, 0); }
analogWrite(MOTOR_LEFT_PIN_2, speed); else
} {
// Coast
analogWrite(MOTOR_LEFT_PIN_1, 0);
analogWrite(MOTOR_LEFT_PIN_2, speed);
}
} }
void motorLeftBackward(int speed, bool mode) { void motorLeftBackward(int speed, bool mode)
if (!mode) { {
// Break if (!mode)
analogWrite(MOTOR_LEFT_PIN_2, 255); {
analogWrite(MOTOR_LEFT_PIN_1, 255-speed); // Break
} else { analogWrite(MOTOR_LEFT_PIN_2, 255);
// Coast analogWrite(MOTOR_LEFT_PIN_1, 255 - speed);
analogWrite(MOTOR_LEFT_PIN_2, 0); }
analogWrite(MOTOR_LEFT_PIN_1, speed); else
} {
// Coast
analogWrite(MOTOR_LEFT_PIN_2, 0);
analogWrite(MOTOR_LEFT_PIN_1, speed);
}
} }
// Right motor // Right motor
void motorRightForward(int speed, bool mode) { void motorRightForward(int speed, bool mode)
if (!mode) { {
// Break if (!mode)
analogWrite(MOTOR_RIGHT_PIN_1, 255); {
analogWrite(MOTOR_RIGHT_PIN_2, 255-speed); // Break
} else { analogWrite(MOTOR_RIGHT_PIN_1, 255);
// Coast analogWrite(MOTOR_RIGHT_PIN_2, 255 - speed);
analogWrite(MOTOR_RIGHT_PIN_1, 0); }
analogWrite(MOTOR_RIGHT_PIN_2, speed); else
} {
// Coast
analogWrite(MOTOR_RIGHT_PIN_1, 0);
analogWrite(MOTOR_RIGHT_PIN_2, speed);
}
} }
void motorRightBackward(int speed, bool mode) { void motorRightBackward(int speed, bool mode)
if (!mode) { {
// Break if (!mode)
analogWrite(MOTOR_RIGHT_PIN_2, 255); {
analogWrite(MOTOR_RIGHT_PIN_1, 255-speed); // Break
} else { analogWrite(MOTOR_RIGHT_PIN_2, 255);
// Coast analogWrite(MOTOR_RIGHT_PIN_1, 255 - speed);
analogWrite(MOTOR_RIGHT_PIN_2, 0); }
analogWrite(MOTOR_RIGHT_PIN_1, 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