two color sensors test

include/settings.h

 // From top to bottom of motor driver (bottom is big capacitor)
-#define MOTOR_LEFT_PIN_2 D5
+#define MOTOR_LEFT_PIN_2 D3 // Not using D5 because it gets pulled high briefly on startup
 #define MOTOR_LEFT_PIN_1 D6
 #define MOTOR_RIGHT_PIN_2 D9
 #define MOTOR_RIGHT_PIN_1 D10
 
-
-#define IR_SENSOR_LEFT_PIN A0
-#define IR_SENSOR_RIGHT_PIN A1
-
 #define DEBOUNCE_TIME_MS 50
 
 #define LEFT_MOTOR_SPEED 50
 #define RIGHT_MOTOR_SPEED 50
-#define LEFT_IR_THRESHOLD 500
-#define RIGHT_IR_THRESHOLD 500
-
 
 // mode 0(default) - break, 1 - coast
 void motorLeftForward(int speed, bool mode = 0);

src/main.cpp

@@ -4,8 +4,9 @@
 #include "Adafruit_TCS34725.h"
 
 // Color sensor
-// Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_4X);
-
+Adafruit_TCS34725 color_sensor_left = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_24MS, TCS34725_GAIN_4X);
+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
@@ -14,9 +15,6 @@ int prev_button_press_time = 0;
 void setup() {
   Serial.begin(115200);
 
-  pinMode(IR_SENSOR_LEFT_PIN, INPUT_ANALOG);
-  pinMode(IR_SENSOR_RIGHT_PIN, INPUT_ANALOG);
-
   pinMode(MOTOR_LEFT_PIN_1, OUTPUT);
   pinMode(MOTOR_LEFT_PIN_2, OUTPUT);
   pinMode(MOTOR_RIGHT_PIN_1, OUTPUT);
@@ -24,12 +22,21 @@ void setup() {
   pinMode(USER_BTN, INPUT);
 
   // Color sensor setup
-  // if (tcs.begin()) {
-  //   Serial.println("Found sensor");
-  // } else {
-  //   Serial.println("No TCS34725 found ... check your connections");
-  //   while (1); // halt!
-  // }
+  // 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!
+  }
 }
 
 void loop() {
@@ -44,12 +51,6 @@ void loop() {
   }
   prev_button_state = current_button_state;
 
-  // uint16_t ir_left_value = analogRead(IR_SENSOR_LEFT_PIN);
-  // uint16_t ir_right_value = analogRead(IR_SENSOR_RIGHT_PIN);
-  // Serial.print(ir_left_value);
-  // Serial.print(" ");
-  // Serial.println(ir_right_value);
-
 
   // For testing motor
   // int motor_speed = map(ir_left_value, 0, 1023, 0, 255);
@@ -80,16 +81,23 @@ void loop() {
   // }
 
   // Color sensor code
-  // float red, green, blue;
-  // tcs.setInterrupt(false);  // turn on LED
-  // delay(60);  // takes 50ms to read
-  // tcs.getRGB(&red, &green, &blue);
-  // tcs.setInterrupt(true);  // turn off LED
+  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
  
-  // Serial.print("R:"); Serial.print(int(red));
-  // Serial.print("\tG:"); Serial.print(int(green));
-  // Serial.print("\tB:"); Serial.print(int(blue));
-  // Serial.print("\n");
+  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");
 }
 
 // Left motor