color sensor cleanup and basic line following

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,

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