Skip to content
Snippets Groups Projects
Commit 6e2bfef2 authored by Daniel Qu's avatar Daniel Qu
Browse files

Refactor color sensor code to separate file

parent 26f124c2
No related branches found
No related tags found
1 merge request!1Refactor color sensor code to separate file
Hide whitespace changes
Inline Side-by-side
include/color.h 0 → 100644
+ 37
− 0
View file @ 6e2bfef2
#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();
include/settings.h
+ 1
− 21
View file @ 6e2bfef2
......@@ -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
src/color.cpp 0 → 100644
+ 119
− 0
View file @ 6e2bfef2
#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
src/main.cpp
+ 6
− 141
View file @ 6e2bfef2
......@@ -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)
{
......
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