Source code

001/*
002 * Copyright (c) 2020-2021 REV Robotics
003 *
004 * Redistribution and use in source and binary forms, with or without
005 * modification, are permitted provided that the following conditions are met:
006 *
007 * 1. Redistributions of source code must retain the above copyright notice,
008 *    this list of conditions and the following disclaimer.
009 * 2. Redistributions in binary form must reproduce the above copyright
010 *    notice, this list of conditions and the following disclaimer in the
011 *    documentation and/or other materials provided with the distribution.
012 * 3. Neither the name of REV Robotics nor the names of its
013 *    contributors may be used to endorse or promote products derived from
014 *    this software without specific prior written permission.
015 *
016 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
017 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
018 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
019 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
020 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
021 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
022 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
023 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
024 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
025 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
026 * POSSIBILITY OF SUCH DAMAGE.
027 */
028
029package com.revrobotics;
030
031import edu.wpi.first.wpilibj.util.Color;
032import java.util.ArrayList;
033
034public class ColorMatch {
035  private static double CalculateDistance(Color color1, Color color2) {
036    double redDiff = color1.red - color2.red;
037    double greenDiff = color1.green - color2.green;
038    double blueDiff = color1.blue - color2.blue;
039
040    return Math.sqrt((redDiff * redDiff + greenDiff * greenDiff + blueDiff * blueDiff) / 2);
041  }
042
043  private static final double kDefaultConfidence = 0.95;
044  private double m_confidenceLevel;
045  private ArrayList<Color> m_colorsToMatch = new ArrayList<Color>();
046
047  public ColorMatch() {
048    m_confidenceLevel = kDefaultConfidence;
049  }
050
051  /**
052   * Add color to match object
053   *
054   * @param color color to add to matching
055   */
056  public void addColorMatch(Color color) {
057    m_colorsToMatch.add(color);
058  }
059
060  /**
061   * Set the confidence interval for determining color. Defaults to 0.95
062   *
063   * @param confidence A value between 0 and 1
064   */
065  public void setConfidenceThreshold(double confidence) {
066    if (confidence < 0) {
067      confidence = 0;
068    } else if (confidence > 1) {
069      confidence = 1;
070    }
071    m_confidenceLevel = confidence;
072  }
073
074  /**
075   * MatchColor uses euclidean distance to compare a given normalized RGB vector against stored
076   * values
077   *
078   * @param colorToMatch color to compare against stored colors
079   * @return Matched color if detected, returns null if no color detected confidence level
080   */
081  public ColorMatchResult matchColor(Color colorToMatch) {
082    ColorMatchResult match = matchClosestColor(colorToMatch);
083    if (match.confidence > m_confidenceLevel) {
084      return match;
085    }
086    return null;
087  }
088
089  /**
090   * MatchColor uses euclidean distance to compare a given normalized RGB vector against stored
091   * values
092   *
093   * @param color color to compare against stored colors
094   * @return Closest color to match
095   */
096  public ColorMatchResult matchClosestColor(Color color) {
097    double magnitude = color.red + color.blue + color.green;
098    if (magnitude > 0 && m_colorsToMatch.size() > 0) {
099      double minDistance = 1.0;
100      int idx = 0;
101
102      for (int i = 0; i < m_colorsToMatch.size(); i++) {
103        double targetDistance = CalculateDistance(m_colorsToMatch.get(i), color);
104
105        if (targetDistance < minDistance) {
106          minDistance = targetDistance;
107          idx = i;
108        }
109      }
110      ColorMatchResult match =
111          new ColorMatchResult(m_colorsToMatch.get(idx), 1.0 - (minDistance / magnitude));
112      return match;
113    } else {
114      // return frc::Color::kBlack;
115      return new ColorMatchResult(Color.kBlack, 0.0);
116    }
117  }
118}