diff --git a/.vscode/extensions.json b/.vscode/extensions.json
deleted file mode 100644
index 080e70d08b9811fa743afe5094658dba0ed6b7c2..0000000000000000000000000000000000000000
--- a/.vscode/extensions.json
+++ /dev/null
@@ -1,10 +0,0 @@
-{
- // 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"
- ]
-}
diff --git a/.vscode/settings.json b/.vscode/settings.json
deleted file mode 100644
index dc21fb8fd05fcdae6b30dbb054561d3409cc93b7..0000000000000000000000000000000000000000
--- a/.vscode/settings.json
+++ /dev/null
@@ -1,13 +0,0 @@
-{
- "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
diff --git a/include/settings.h b/include/settings.h
index 76575f25fa433c30d876e67afc3aa5b990bbdb51..4b40548bcdd25bee074fb35577ff20e828976bc9 100644
--- a/include/settings.h
+++ b/include/settings.h
@@ -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;
diff --git a/src/main.cpp b/src/main.cpp
index 455b0e0b3467747c4c84df8c917e69a7c14e1c3e..32da9013f26e8de0a5c3ff73a884ae06e073cb4a 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -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