diff --git a/include/settings.h b/include/settings.h
index 3a867aafcfdb4ef88b98143c7fc4441dc225cfe9..76575f25fa433c30d876e67afc3aa5b990bbdb51 100644
--- a/include/settings.h
+++ b/include/settings.h
@@ -6,14 +6,29 @@
#define DEBOUNCE_TIME_MS 50
-#define LEFT_MOTOR_SPEED 50
-#define RIGHT_MOTOR_SPEED 50
+#define LEFT_MOTOR_SPEED 25
+#define RIGHT_MOTOR_SPEED 25
+
+typedef struct {
+ float R;
+ float G;
+ float B;
+} Rgb;
+
+typedef struct {
+ Rgb min = Rgb{255, 255, 255};
+ Rgb max = Rgb{0, 0, 0};
+} ColorSensorCal;
+
+void printRGB(const Rgb& value);
+void checkButtonPress();
+void getColorValues(Rgb& left_value, Rgb& right_value);
// mode 0(default) - break, 1 - coast
void motorLeftForward(int speed, bool mode = 0);
-void motorLeftBackwards(int speed, bool mode = 0);
+void motorLeftBackward(int speed, bool mode = 0);
void motorRightForward(int speed, bool mode = 0);
-void motorRightBackwards(int speed, bool mode = 0);
+void motorRightBackward(int speed, bool mode = 0);
typedef enum {
MODE_BREAK,
diff --git a/src/main.cpp b/src/main.cpp
index 81f2f59116f67d14a1275139582ce5ccfb5e0629..455b0e0b3467747c4c84df8c917e69a7c14e1c3e 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -8,10 +8,18 @@ Adafruit_TCS34725 color_sensor_left = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME
Adafruit_TCS34725 color_sensor_right = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_24MS, TCS34725_GAIN_4X);
TwoWire Wire2 = TwoWire(PC9, PA8); // I2C channel 3 for right sensor
-bool forward = false;
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
+
+int left_red_threshold = 95;
+int right_red_threshold = 95;
+
+ColorSensorCal color_cal_left;
+ColorSensorCal color_cal_right;
+
void setup() {
Serial.begin(115200);
@@ -40,88 +48,117 @@ void setup() {
}
void loop() {
- 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)) {
- forward = !forward;
- prev_button_press_time = millis();
- Serial.print("Pressed: ");
- Serial.println(forward);
- }
- prev_button_state = current_button_state;
-
+ checkButtonPress();
// For testing motor
// int motor_speed = map(ir_left_value, 0, 1023, 0, 255);
// Serial.println(motor_speed);
- // Enable line following
- /*
- if (ir_left_value < LEFT_IR_THRESHOLD) {
- motorLeftForward(LEFT_MOTOR_SPEED);
- } else {
- motorLeftForward(0);
- }
-
- if (ir_right_value < RIGHT_IR_THRESHOLD) {
- motorRightForward(RIGHT_MOTOR_SPEED);
- } else {
- motorRightForward(0);
- }
- */
-
// For testing motor
- // if (forward) {
- // motorLeftForward(motor_speed);
- // motorRightForward(motor_speed);
+ // if (!enable_line_follow) {
+ // motorLeftForward(LEFT_MOTOR_SPEED);
+ // motorRightForward(RIGHT_MOTOR_SPEED);
// } else {
- // motorLeftBackwards(motor_speed);
- // motorRightBackwards(motor_speed);
+ // motorLeftBackward(LEFT_MOTOR_SPEED);
+ // motorRightBackward(RIGHT_MOTOR_SPEED);
// }
// Color sensor code
- float red_left, green_left, blue_left, red_right, green_right, blue_right;
- color_sensor_left.setInterrupt(false); // turn on LED
- color_sensor_right.setInterrupt(false); // turn on LED
- delay(24); // takes 24ms to read
- color_sensor_left.getRGB(&red_left, &green_left, &blue_left);
- color_sensor_right.getRGB(&red_right, &green_right, &blue_right);
- color_sensor_left.setInterrupt(true); // turn off LED
- color_sensor_right.setInterrupt(true); // turn off LED
+ Rgb left_color = {};
+ Rgb right_color = {};
+ getColorValues(left_color, right_color);
- Serial.print("Left R:"); Serial.print(int(red_left));
- Serial.print("\tG:"); Serial.print(int(green_left));
- Serial.print("\tB:"); Serial.print(int(blue_left));
- Serial.print("\t");
- Serial.print("Right R:"); Serial.print(int(red_right));
- Serial.print("\tG:"); Serial.print(int(green_right));
- Serial.print("\tB:"); Serial.print(int(blue_right));
- Serial.print("\n");
+ // 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 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));
+}
+
+// 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 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);
+}
// Left motor
void motorLeftForward(int speed, bool mode) {
if (!mode) {
// Break
- analogWrite(MOTOR_LEFT_PIN_2, 255);
- analogWrite(MOTOR_LEFT_PIN_1, speed);
+ analogWrite(MOTOR_LEFT_PIN_1, 255);
+ analogWrite(MOTOR_LEFT_PIN_2, 255-speed);
} else {
// Coast
- analogWrite(MOTOR_LEFT_PIN_2, 0);
- analogWrite(MOTOR_LEFT_PIN_1, 255-speed);
+ analogWrite(MOTOR_LEFT_PIN_1, 0);
+ analogWrite(MOTOR_LEFT_PIN_2, speed);
}
}
-void motorLeftBackwards(int speed, bool mode) {
+void motorLeftBackward(int speed, bool mode) {
if (!mode) {
// Break
- analogWrite(MOTOR_LEFT_PIN_1, 255);
- analogWrite(MOTOR_LEFT_PIN_2, speed);
+ analogWrite(MOTOR_LEFT_PIN_2, 255);
+ analogWrite(MOTOR_LEFT_PIN_1, 255-speed);
} else {
// Coast
- analogWrite(MOTOR_LEFT_PIN_1, 0);
- analogWrite(MOTOR_LEFT_PIN_2, 255-speed);
+ analogWrite(MOTOR_LEFT_PIN_2, 0);
+ analogWrite(MOTOR_LEFT_PIN_1, speed);
}
}
@@ -129,23 +166,23 @@ void motorLeftBackwards(int speed, bool mode) {
void motorRightForward(int speed, bool mode) {
if (!mode) {
// Break
- analogWrite(MOTOR_RIGHT_PIN_2, 255);
- analogWrite(MOTOR_RIGHT_PIN_1, speed);
+ analogWrite(MOTOR_RIGHT_PIN_1, 255);
+ analogWrite(MOTOR_RIGHT_PIN_2, 255-speed);
} else {
// Coast
- analogWrite(MOTOR_RIGHT_PIN_2, 0);
- analogWrite(MOTOR_RIGHT_PIN_1, 255-speed);
+ analogWrite(MOTOR_RIGHT_PIN_1, 0);
+ analogWrite(MOTOR_RIGHT_PIN_2, speed);
}
}
-void motorRightBackwards(int speed, bool mode) {
+void motorRightBackward(int speed, bool mode) {
if (!mode) {
// Break
- analogWrite(MOTOR_RIGHT_PIN_1, 255);
- analogWrite(MOTOR_RIGHT_PIN_2, speed);
+ analogWrite(MOTOR_RIGHT_PIN_2, 255);
+ analogWrite(MOTOR_RIGHT_PIN_1, 255-speed);
} else {
// Coast
- analogWrite(MOTOR_RIGHT_PIN_1, 0);
- analogWrite(MOTOR_RIGHT_PIN_2, 255-speed);
+ analogWrite(MOTOR_RIGHT_PIN_2, 0);
+ analogWrite(MOTOR_RIGHT_PIN_1, speed);
}
}
\ No newline at end of file