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
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