From 6e2bfef2f6099793f8a59b6331366b2ca6e09247 Mon Sep 17 00:00:00 2001
From: Daniel Qu <danielq987@gmail.com>
Date: Mon, 16 Oct 2023 03:00:23 -0400
Subject: [PATCH] Refactor color sensor code to separate file
---
include/color.h | 37 ++++++++++++
include/settings.h | 22 +------
src/color.cpp | 119 ++++++++++++++++++++++++++++++++++++
src/main.cpp | 147 ++-------------------------------------------
4 files changed, 163 insertions(+), 162 deletions(-)
create mode 100644 include/color.h
create mode 100644 src/color.cpp
diff --git a/include/color.h b/include/color.h
new file mode 100644
index 0000000..a56a205
--- /dev/null
+++ b/include/color.h
@@ -0,0 +1,37 @@
+#include <Arduino.h>
+
+#define LEFT_RED_HUE_THRESHOLD 8
+#define RIGHT_RED_HUE_THRESHOLD 8
+
+typedef struct
+{
+ float r;
+ float g;
+ float b;
+} Rgb;
+
+typedef struct
+{
+ Rgb min = Rgb{0, 0, 0};
+ Rgb max = Rgb{255, 255, 255};
+} ColorSensorCal;
+
+typedef enum
+{
+ CALIBRATE_BLACK,
+ CALIBRATE_WHITE,
+ CALIBRATE_READY,
+} CalibrationState;
+
+extern CalibrationState calibration_state;
+
+void setupColorSensor();
+
+void printRGB(const Rgb &value);
+
+void getColorValues(Rgb &left_value, Rgb &right_value);
+
+float computeHue(const Rgb &rgb);
+
+void calibrateBlack();
+void calibrateWhite();
diff --git a/include/settings.h b/include/settings.h
index 4b40548..20bf354 100644
--- a/include/settings.h
+++ b/include/settings.h
@@ -9,21 +9,7 @@
#define LEFT_MOTOR_SPEED 25
#define RIGHT_MOTOR_SPEED 25
-typedef struct {
- float R;
- float G;
- float B;
-} Rgb;
-
-typedef struct {
- 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,10 +20,4 @@ void motorRightBackward(int speed, bool mode = 0);
typedef enum {
MODE_BREAK,
MODE_COAST
-} MotorMode;
-
-typedef enum {
- CALIBRATE_BLACK,
- CALIBRATE_WHITE,
- CALIBRATE_READY,
-} CalibrationState;
+} MotorMode;
\ No newline at end of file
diff --git a/src/color.cpp b/src/color.cpp
new file mode 100644
index 0000000..2a3021b
--- /dev/null
+++ b/src/color.cpp
@@ -0,0 +1,119 @@
+#include <Arduino.h>
+#include "color.h"
+#include "Adafruit_TCS34725.h"
+
+// Initiate color sensor instances
+static Adafruit_TCS34725 color_sensor_left = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_24MS, TCS34725_GAIN_4X);
+static Adafruit_TCS34725 color_sensor_right = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_24MS, TCS34725_GAIN_4X);
+
+static TwoWire Wire2 = TwoWire(PC9, PA8); // I2C channel 3 for right sensor
+
+// Change this to CALIBRATE_READY if no calibration is required
+CalibrationState calibration_state = CALIBRATE_BLACK;
+
+static ColorSensorCal color_cal_left;
+static ColorSensorCal color_cal_right;
+
+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));
+}
+
+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;
+}
+
+void setupColorSensor()
+{
+ // 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)
+ ;
+ }
+
+ // 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)
+ ;
+ }
+};
+
+void calibrateBlack()
+{
+ getColorValues(color_cal_left.min, color_cal_right.max);
+}
+
+void calibrateWhite()
+{
+ getColorValues(color_cal_left.max, color_cal_right.max);
+}
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index 32da901..e52ebd4 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -2,24 +2,12 @@
#include <Wire.h>
#include "settings.h"
#include "Adafruit_TCS34725.h"
-
-// Color sensor
-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
+#include "color.h"
bool prev_button_state = true; // nominal high
int prev_button_press_time = 0;
bool enable_line_follow = false;
-bool enable_calibration = false; // Applies calibration on the rgb values
-CalibrationState calibration_state = CALIBRATE_BLACK;
-
-int left_red_threshold = 8;
-int right_red_threshold = 8;
-
-ColorSensorCal color_cal_left;
-ColorSensorCal color_cal_right;
void setup()
{
@@ -31,49 +19,13 @@ void setup()
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!
- }
+ setupColorSensor();
}
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 = {};
@@ -95,33 +47,12 @@ void loop()
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)
+ if (left_red_error > LEFT_RED_HUE_THRESHOLD)
{
motorLeftForward(LEFT_MOTOR_SPEED);
}
@@ -130,7 +61,7 @@ void loop()
motorLeftForward(0);
}
- if (right_red_error > right_red_threshold)
+ if (right_red_error > RIGHT_RED_HUE_THRESHOLD)
{
motorRightForward(RIGHT_MOTOR_SPEED);
}
@@ -141,16 +72,6 @@ void loop()
}
}
-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()
{
@@ -171,7 +92,7 @@ void checkButtonPress()
{
// Calibrate black
Serial.print("Calibrating black");
- getColorValues(color_cal_left.min, color_cal_right.min);
+ calibrateBlack();
// Transition to calibrate white
calibration_state = CALIBRATE_WHITE;
}
@@ -179,7 +100,7 @@ void checkButtonPress()
{
// Calibrate white
Serial.print("Calibrating white");
- getColorValues(color_cal_left.max, color_cal_right.max);
+ calibrateWhite();
// Transition to ready state
calibration_state = CALIBRATE_READY;
}
@@ -187,62 +108,6 @@ void checkButtonPress()
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 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)
{
--
GitLab