# Examples

### Use-Case

This example program demonstrates how to use the AndyMark Proximity & Color Sensor in an FTC OpMode. It initializes the sensor from the robot's hardware map and continuously reads data while the OpMode is running. The program collects proximity information (distance to an object), the ambient light level, individual red, green, and blue color values, and a classified color name based on the sensor's readings. These values are then displayed on the Driver Station screen in real time, allowing users to see exactly what the sensor is detecting. This example is useful for testing the sensor, understanding its output, and troubleshooting sensor placement on a robot, and it can be used without needing in-depth programming knowledge.

### Mounting Examples

![](https://content.gitbook.com/content/cQgnvayQud3Xf5sZi0wz/blobs/OG1ogAquE0pJX5AcnUSq/image)

### Control Hub Configuration

![](https://content.gitbook.com/content/cQgnvayQud3Xf5sZi0wz/blobs/qbc8yDtzL8xUoxl0dv9v/image)

![](https://content.gitbook.com/content/cQgnvayQud3Xf5sZi0wz/blobs/B0sTLKHKswk0uHDNxSjw/image)

### Code Example

{% tabs %}
{% tab title="OnBot Java" %}

```java
package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.ColorSensor;
import com.qualcomm.robotcore.hardware.DistanceSensor;

@TeleOp(name = "AMReleaseColorSensor")
public class AMReleaseColorSensor extends LinearOpMode
{
  private ColorSensor sensor_color;

  @Override
  public void runOpMode()
  {
    sensor_color = hardwareMap.get(ColorSensor.class, "sensor_color");

    waitForStart();
    if (opModeIsActive())
    {
      while (opModeIsActive())
      {
        telemetry.addData("RED", sensor_color.red());
        telemetry.addData("GREEN", sensor_color.green());
        telemetry.addData("BLUE", sensor_color.blue());
        /*
        // This measurment is actually a unitless proximity measurement,
        // so larger measurements = farther away, while
        // smaller measurments = closer.
        */
        telemetry.addData("Distance (m)", ((DistanceSensor) sensor_color).getDistance(DistanceUnit.METER));
        telemetry.update();
      }
    }
  }
}
```

[AMReleaseColorSensor.java](https://3879385486-files.gitbook.io/~/files/v0/b/gitbook-x-prod.appspot.com/o/spaces%2F6dFSSchrNzNvNcuFf0H6%2Fuploads%2F7i2v1y9pAOm8QCYQoauo%2FAMReleaseColorSensor.java?alt=media\&token=201ca6f4-072a-4cdf-9f21-1abad731c9c4)
{% endtab %}

{% tab title="Blocks" %}
[AMReleaseColorSensor.blk](https://3879385486-files.gitbook.io/~/files/v0/b/gitbook-x-prod.appspot.com/o/spaces%2F6dFSSchrNzNvNcuFf0H6%2Fuploads%2FUBP1PjHWvIyfizhcBU0N%2FAMReleaseColorSensor.blk?alt=media\&token=25af535e-c091-4d2a-b123-db1563cf88c6)

![](https://content.gitbook.com/content/cQgnvayQud3Xf5sZi0wz/blobs/33KTi3BO64SFirsHkQi1/image)
{% endtab %}

{% tab title="Arduino" %}

```cpp
#include <Wire.h>

#define INT_PIN 4
#define SDA_PIN 6
#define SCL_PIN 10
#define TMD37253M_I2C_ADDR 0x39  // Device I2C address

// Register Definitions
#define ENABLE_REG 0x80          // Enables states and interrupts
#define ATIME_REG 0x81           // ALS integration time
#define WTIME_REG 0x83           // Wait time
#define CONTROL_REG 0x8F         // Gain control
#define CDATA_REG 0x94           // Clear data low byte
#define RDATA_REG 0x96           // Red data low byte
#define GDATA_REG 0x98           // Green data low byte
#define BDATA_REG 0x9A           // Blue data low byte
#define PDATA_REG 0x9C           // Proximity data
#define STATUS_REG 0x93          // Status register

void setup()
{
  // Initialize Serial Monitor
  Serial.begin(115200);
  while (!Serial);
  Serial.println("TMD37253M RGB, Ambient Light, and Proximity Sensor Example");

  // Configure I2C pins and initialize Wire library
  Wire.begin(SDA_PIN, SCL_PIN);

  // Test I2C communication
  Wire.beginTransmission(TMD37253M_I2C_ADDR);
  if (Wire.endTransmission() != 0)
  {
    Serial.println("I2C communication failed! Check connections and I2C address.");
    while (1);
  }
  else
  {
    Serial.println("I2C communication successful.");
  }

  // Initialize the sensor
  if (!initializeSensor())
  {
    Serial.println("Failed to initialize TMD37253M sensor!");
    while (1);
  }
  Serial.println("Sensor initialized successfully.");
}

void loop()
{
  // Read RGB, ambient light, and proximity data
  uint16_t clear, red, green, blue, proximity;
  if (readSensorData(clear, red, green, blue, proximity))
  {
    // Print formatted data
    Serial.printf("%u, %u, %u, %u, %u\n", clear, red, green, blue, proximity);
  }
  else
  {
    Serial.println("0, 0, 0, 0, 0");  // Data read failed
  }

  delay(500); // Delay for readability
}

bool initializeSensor()
{
  Serial.println("Initializing sensor...");

  // Step 1: Power ON (PON = 1)
  writeRegister(ENABLE_REG, 0x01);  // PON
  delay(10);

  // Step 2: Enable ALS (PON | AEN) and Proximity (PEN)
  writeRegister(ENABLE_REG, 0x07);  // PON | AEN | PEN
  Serial.println("ALS and Proximity sensing enabled.");

  // Step 3: Set ALS integration time (e.g., 153.6ms = 0xDB)
  writeRegister(ATIME_REG, 0xDB);  // Adjust for your lighting conditions
  Serial.println("Set ALS integration time.");

  // Step 4: Set wait time (e.g., 2.4ms increments)
  writeRegister(WTIME_REG, 0xFF);  // Longest wait time
  Serial.println("Set wait time.");

  // Step 5: Configure gain control for ALS (e.g., 16x gain)
  writeRegister(CONTROL_REG, 0x02);  // Set gain to 16x
  Serial.println("Set ALS gain to 16x.");

  // Configure Proximity Pulse (e.g., 16 pulses, 16μs each)
  writeRegister(0x8E, 0x11);  // Adjust pulse length and count  // 0x11 - 00010001

// Set LED drive strength
  writeRegister(CONTROL_REG, 0x0F);  // Maximum LED drive

  // Wait to allow the sensor to stabilize and collect data
  delay(200);

  // Debug: Check status register
  uint8_t status = readRegister(STATUS_REG);
  Serial.printf("Status Register: 0x%02X\n", status);

  // Assume initialization success without relying on STATUS_REG
  return true;
}

bool readSensorData(uint16_t &clear, uint16_t &red, uint16_t &green, uint16_t &blue, uint16_t &proximity)
{
  //Serial.println("Reading sensor data...");
  clear = read16(CDATA_REG);
  red   = read16(RDATA_REG);
  green = read16(GDATA_REG);
  blue  = read16(BDATA_REG);
  proximity = read16(PDATA_REG);

  //Serial.printf("Raw Data - C: %u, R: %u, G: %u, B: %u, Proximity: %u\n", clear, red, green, blue, proximity);

  // Verify all reads are non-zero (basic check)
  return (clear | red | green | blue | proximity) != 0;
}

void writeRegister(uint8_t reg, uint8_t value)
{
  Wire.beginTransmission(TMD37253M_I2C_ADDR);
  Wire.write(reg);
  Wire.write(value);
  if (Wire.endTransmission() != 0)
  {
    Serial.printf("Failed to write to register 0x%02X\n", reg);
  }
}

uint8_t readRegister(uint8_t reg)
{
  Wire.beginTransmission(TMD37253M_I2C_ADDR);
  Wire.write(reg);
  if (Wire.endTransmission(false) != 0)
  {
    Serial.printf("Failed to set register 0x%02X for reading.\n", reg);
    return 0;
  }
  Wire.requestFrom(TMD37253M_I2C_ADDR, 1);
  return Wire.available() ? Wire.read() : 0;
}

uint16_t read16(uint8_t reg)
{
  Wire.beginTransmission(TMD37253M_I2C_ADDR);
  Wire.write(reg);
  if (Wire.endTransmission(false) != 0)
  {
    Serial.printf("Failed to set register 0x%02X for 16-bit read.\n", reg);
    return 0;
  }
  Wire.requestFrom(TMD37253M_I2C_ADDR, 2);
  uint16_t value = 0;
  if (Wire.available() == 2)
  {
    value = Wire.read();
    value |= (Wire.read() << 8);
  }
  else
  {
    Serial.printf("Failed to read 2 bytes from 0x%02X\n", reg);
  }
  return value;
}
```

{% endtab %}
{% endtabs %}