diff --git a/CHANGELOG.md b/CHANGELOG.md
index c939c8ead..ceed3149e 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -8,11 +8,14 @@
 - The `use_gyro` method is added to the normal `DriveBase` class instead of
   having a separate `GyroDriveBase` class. Since the latter was only released
   in beta versions, this is not a breaking change ([support#1054]).
+- New color distance function used by the color sensors that is more
+  consistent when distinguishing user-provided colors ([pybricks-micropython#104]).
 
 ### Fixed
 - Improved external device detection speed ([support#1140]).
 - Fixed Powered Up Tilt Sensor not working  ([support#1189]).
 
+[pybricks-micropython#104]: https://github.com/pybricks/pybricks-micropython/pull/104
 [support#1054]: https://github.com/pybricks/support/issues/1054
 [support#1140]: https://github.com/pybricks/support/issues/1140
 [support#1189]: https://github.com/pybricks/support/issues/1189
diff --git a/bricks/_common/sources.mk b/bricks/_common/sources.mk
index e66367bd2..d6e0e066f 100644
--- a/bricks/_common/sources.mk
+++ b/bricks/_common/sources.mk
@@ -190,6 +190,7 @@ PBIO_SRC_C = $(addprefix lib/pbio/,\
 	src/angle.c \
 	src/battery.c \
 	src/color/conversion.c \
+	src/color/util.c \
 	src/control.c \
 	src/control_settings.c \
 	src/dcmotor.c \
diff --git a/lib/pbio/drv/led/led_pwm.c b/lib/pbio/drv/led/led_pwm.c
index 6f557db4e..e093f31a5 100644
--- a/lib/pbio/drv/led/led_pwm.c
+++ b/lib/pbio/drv/led/led_pwm.c
@@ -48,7 +48,7 @@ static pbio_error_t pbdrv_led_pwm_set_hsv(pbdrv_led_dev_t *dev, const pbio_color
     uint32_t Y = ((color->r_brightness * r + color->g_brightness * g + color->b_brightness * b) >> 12) + 1;
 
     // Reapply V from HSV for brightness. V is squared for gamma correction.
-    uint32_t scale_factor = hsv->v * hsv->v * pdata->scale_factor / 128;
+    uint32_t scale_factor = pbio_color_hsv_get_v(hsv) * pbio_color_hsv_get_v(hsv) * pdata->scale_factor / 128;
 
     r = r * scale_factor / Y;
     g = g * scale_factor / Y;
diff --git a/lib/pbio/drv/led/led_virtual.c b/lib/pbio/drv/led/led_virtual.c
index 74b256a81..6a597a459 100644
--- a/lib/pbio/drv/led/led_virtual.c
+++ b/lib/pbio/drv/led/led_virtual.c
@@ -26,7 +26,7 @@
 static pbio_error_t pbdrv_led_virtual_set_hsv(pbdrv_led_dev_t *dev, const pbio_color_hsv_t *hsv) {
     uint8_t id = (intptr_t)dev->pdata;
 
-    return pbdrv_virtual_call_method("led", id, "on_set_hsv", "IBBB", pbdrv_clock_get_us(), hsv->h, hsv->s, hsv->v);
+    return pbdrv_virtual_call_method("led", id, "on_set_hsv", "IBBB", pbdrv_clock_get_us(), hsv->h, hsv->s, pbio_color_hsv_get_v(hsv));
 }
 
 static const pbdrv_led_funcs_t pbdrv_led_virtual_funcs = {
diff --git a/lib/pbio/include/pbio/color.h b/lib/pbio/include/pbio/color.h
index 0294af8e6..8dc464b2b 100644
--- a/lib/pbio/include/pbio/color.h
+++ b/lib/pbio/include/pbio/color.h
@@ -92,18 +92,24 @@ typedef struct {
     uint16_t h;
     /** The saturation component. 0 to 100 percent. */
     uint8_t s;
-    /** The value component. 0 to 100 percent. */
-    uint8_t v;
+    /** The value component. Normally 0 to 100 percent but allowed to be
+     * negative to provide higher contrast in color scanning applications. */
+    int8_t v;
 } pbio_color_hsv_t;
 
+static inline uint8_t pbio_color_hsv_get_v(const pbio_color_hsv_t *hsv) {
+    return hsv->v < 0 ? 0 : hsv->v;
+}
+
 /** Compressed HSV color. Stores data in 24 bytes instead of 32. */
 typedef struct __attribute__((__packed__)) {
     /** The hue component. 0 to 359 degrees. */
     uint16_t h : 9;
     /** The saturation component. 0 to 100 percent. */
     uint8_t s : 7;
-    /** The value component. 0 to 100 percent. */
-    uint8_t v;
+    /** The value component. Normally 0 to 100 percent but allowed to be
+     * negative to provide higher contrast in color scanning applications. */
+    int8_t v;
 } pbio_color_compressed_hsv_t;
 
 void pbio_color_rgb_to_hsv(const pbio_color_rgb_t *rgb, pbio_color_hsv_t *hsv);
@@ -112,6 +118,7 @@ void pbio_color_to_hsv(pbio_color_t color, pbio_color_hsv_t *hsv);
 void pbio_color_to_rgb(pbio_color_t color, pbio_color_rgb_t *rgb);
 void pbio_color_hsv_compress(const pbio_color_hsv_t *hsv, pbio_color_compressed_hsv_t *compressed);
 void pbio_color_hsv_expand(const pbio_color_compressed_hsv_t *compressed, pbio_color_hsv_t *hsv);
+int32_t pbio_color_get_bicone_squared_distance(const pbio_color_hsv_t *hsv_a, const pbio_color_hsv_t *hsv_b);
 
 #endif // _PBIO_COLOR_H_
 
diff --git a/lib/pbio/include/pbio/int_math.h b/lib/pbio/include/pbio/int_math.h
index c6d355003..430cbf300 100644
--- a/lib/pbio/include/pbio/int_math.h
+++ b/lib/pbio/include/pbio/int_math.h
@@ -35,6 +35,8 @@ int32_t pbio_int_math_sign(int32_t a);
 int32_t pbio_int_math_atan2(int32_t y, int32_t x);
 int32_t pbio_int_math_mult_then_div(int32_t a, int32_t b, int32_t c);
 int32_t pbio_int_math_sqrt(int32_t n);
+int32_t pbio_int_math_sin_deg(int32_t x);
+int32_t pbio_int_math_cos_deg(int32_t x);
 
 #endif // _PBIO_INT_MATH_H_
 
diff --git a/lib/pbio/include/pbio/light.h b/lib/pbio/include/pbio/light.h
index b1eea8d7c..3586632bf 100644
--- a/lib/pbio/include/pbio/light.h
+++ b/lib/pbio/include/pbio/light.h
@@ -29,7 +29,8 @@ typedef struct _pbio_color_light_t pbio_color_light_t;
     { .h = (hue), .s = (saturation), .v = (value) }
 
 /** Sentinel value for a color light animation array. */
-#define PBIO_COLOR_LIGHT_ANIMATION_END { .v = UINT8_MAX }
+#define PBIO_COLOR_LIGHT_ANIMATION_END_V (INT8_MAX)
+#define PBIO_COLOR_LIGHT_ANIMATION_END_HSV { .v = PBIO_COLOR_LIGHT_ANIMATION_END_V }
 
 #if PBIO_CONFIG_LIGHT
 
diff --git a/lib/pbio/src/color/conversion.c b/lib/pbio/src/color/conversion.c
index e7d52e6b2..4888f6548 100644
--- a/lib/pbio/src/color/conversion.c
+++ b/lib/pbio/src/color/conversion.c
@@ -105,7 +105,7 @@ void pbio_color_hsv_to_rgb(const pbio_color_hsv_t *hsv, pbio_color_rgb_t *rgb) {
     // the output more visually linear.
 
     // Scale 0..100 percent to 0..255
-    uint8_t value = 327 * hsv->v / 128;
+    uint8_t value = 327 * pbio_color_hsv_get_v(hsv) / 128;
     uint8_t saturation = 327 * hsv->s / 128;
 
     // The brightness floor is minimum number that all of
diff --git a/lib/pbio/src/color/util.c b/lib/pbio/src/color/util.c
new file mode 100644
index 000000000..b2a2eb00a
--- /dev/null
+++ b/lib/pbio/src/color/util.c
@@ -0,0 +1,38 @@
+// SPDX-License-Identifier: MIT
+// Copyright (c) 2018-2022 The Pybricks Authors
+
+#include <pbio/color.h>
+#include <pbio/int_math.h>
+
+/**
+ * Gets squared Euclidean distance between HSV colors mapped into a
+ * chroma-lightness-bicone. The bicone is 20000 units tall and 20000 units in
+ * diameter.
+ *
+ * @param [in]  hsv_a    The first HSV color.
+ * @param [in]  hsv_b    The second HSV color.
+ * @returns              Squared distance (0 to 400000000).
+ */
+int32_t pbio_color_get_bicone_squared_distance(const pbio_color_hsv_t *hsv_a, const pbio_color_hsv_t *hsv_b) {
+
+    // Chroma (= radial coordinate in bicone) of a and b (0-10000).
+    int32_t radius_a = pbio_color_hsv_get_v(hsv_a) * hsv_a->s;
+    int32_t radius_b = pbio_color_hsv_get_v(hsv_b) * hsv_b->s;
+
+    // Lightness (= z-coordinate in bicone) of a and b (0-20000).
+    // v is allowed to be negative, resulting in negative lightness.
+    // This can be used to create a higher contrast between "none-color" and
+    // normal colors.
+    int32_t lightness_a = (200 - hsv_a->s) * hsv_a->v;
+    int32_t lightness_b = (200 - hsv_b->s) * hsv_b->v;
+
+    // z delta of a and b in HSV bicone (-20000, 20000).
+    int32_t delta_z = (lightness_b - lightness_a);
+
+    // x and y deltas of a and b in HSV bicone (-20000, 20000)
+    int32_t delta_x = (radius_b * pbio_int_math_cos_deg(hsv_b->h) - radius_a * pbio_int_math_cos_deg(hsv_a->h)) / 10000;
+    int32_t delta_y = (radius_b * pbio_int_math_sin_deg(hsv_b->h) - radius_a * pbio_int_math_sin_deg(hsv_a->h)) / 10000;
+
+    // Squared Euclidean distance (0, 400000000)
+    return delta_x * delta_x + delta_y * delta_y + delta_z * delta_z;
+}
diff --git a/lib/pbio/src/int_math.c b/lib/pbio/src/int_math.c
index e525b187c..dc89112bf 100644
--- a/lib/pbio/src/int_math.c
+++ b/lib/pbio/src/int_math.c
@@ -294,3 +294,46 @@ int32_t pbio_int_math_mult_then_div(int32_t a, int32_t b, int32_t c) {
     assert(result == (int64_t)a * (int64_t)b / (int64_t)c);
     return result;
 }
+
+/**
+ * Approximates first 90-degree segment of a sine in degrees, output
+ * upscaled by 10000.
+ *
+ * @param [in]  x        Angle in degrees (0-90).
+ * @returns              Approximately sin(x) * 10000.
+ */
+static int32_t pbio_int_math_sin_deg_branch0(int32_t x) {
+    return (201 - x) * x;
+}
+
+// integer sine approximation from degrees to (-10000, 10000)
+
+/**
+ * Approximates sine of an angle in degrees, output upscaled by 10000.
+ *
+ * @param [in]  x        Angle in degrees.
+ * @returns              Approximately sin(x) * 10000.
+ */
+int32_t pbio_int_math_sin_deg(int32_t x) {
+    x = x % 360;
+    if (x < 90) {
+        return pbio_int_math_sin_deg_branch0(x);
+    }
+    if (x < 180) {
+        return pbio_int_math_sin_deg_branch0(180 - x);
+    }
+    if (x < 270) {
+        return -pbio_int_math_sin_deg_branch0(x - 180);
+    }
+    return -pbio_int_math_sin_deg_branch0(360 - x);
+}
+
+/**
+ * Approximates cosine of an angle in degrees, output upscaled by 10000.
+ *
+ * @param [in]  x        Angle in degrees.
+ * @returns              Approximately cos(x) * 10000.
+ */
+int32_t pbio_int_math_cos_deg(int32_t x) {
+    return pbio_int_math_sin_deg(x + 90);
+}
diff --git a/lib/pbio/src/light/color_light.c b/lib/pbio/src/light/color_light.c
index dca8de476..50cc29a16 100644
--- a/lib/pbio/src/light/color_light.c
+++ b/lib/pbio/src/light/color_light.c
@@ -147,7 +147,7 @@ static uint32_t pbio_color_light_animate_next(pbio_light_animation_t *animation)
     const pbio_color_compressed_hsv_t *cell = &cells[light->current_cell++];
 
     // if we have reached the array terminator, start back at the beginning
-    if (cell->v == UINT8_MAX) {
+    if (cell->v == PBIO_COLOR_LIGHT_ANIMATION_END_V) {
         cell = &cells[0];
         light->current_cell = 1;
     }
@@ -169,7 +169,7 @@ static uint32_t pbio_color_light_animate_next(pbio_light_animation_t *animation)
  *
  * @param [in]  light       The light instance
  * @param [in]  interval    The the time intervale between animation cells in milliseconds
- * @param [in]  cells       Array of up to 65536 animation cells ending with ::PBIO_COLOR_LIGHT_ANIMATION_END
+ * @param [in]  cells       Array of up to 65536 animation cells ending with ::PBIO_COLOR_LIGHT_ANIMATION_END_HSV
  */
 void pbio_color_light_start_animation(pbio_color_light_t *light, uint16_t interval, const pbio_color_compressed_hsv_t *cells) {
     pbio_color_light_stop_animation(light);
diff --git a/lib/pbio/test/src/test_color.c b/lib/pbio/test/src/test_color.c
index f5aa7b067..3ee5df701 100644
--- a/lib/pbio/test/src/test_color.c
+++ b/lib/pbio/test/src/test_color.c
@@ -108,6 +108,15 @@ static void test_hsv_to_rgb(void *env) {
     pbio_color_hsv_t hsv;
     pbio_color_rgb_t rgb;
 
+    // no-color (negative brightness)
+    hsv.h = 0;
+    hsv.s = 0;
+    hsv.v = -50;
+    pbio_color_hsv_to_rgb(&hsv, &rgb);
+    tt_want_int_op(rgb.r, ==, 0);
+    tt_want_int_op(rgb.g, ==, 0);
+    tt_want_int_op(rgb.b, ==, 0);
+
     // black
     hsv.h = 0;
     hsv.s = 0;
@@ -348,11 +357,264 @@ static void test_color_hsv_compression(void *env) {
     tt_want_int_op(hsv.v, ==, expanded.v);
 }
 
+static void test_color_hsv_cost(void *env) {
+    pbio_color_hsv_t color_a;
+    pbio_color_hsv_t color_b;
+    int32_t dist;
+
+    // color compared to itself should give 0
+    color_a.h = 0;
+    color_a.s = 100;
+    color_a.v = 100;
+    tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_a), ==, 0);
+
+    // blacks with different saturations/hues should be the same
+    color_a.h = 230;
+    color_a.s = 23;
+    color_a.v = 0;
+
+    color_b.h = 23;
+    color_b.s = 99;
+    color_b.v = 0;
+    tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), ==, 0);
+
+    // colors with different hues should be different when value>0 and saturation>0
+    color_a.h = 230;
+    color_a.s = 99;
+    color_a.v = 100;
+
+    color_b.h = 23;
+    color_b.s = 99;
+    color_b.v = 100;
+    tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), >, 0);
+
+    // grays with different hues should be the same
+    color_a.h = 230;
+    color_a.s = 0;
+    color_a.v = 50;
+
+    color_b.h = 23;
+    color_b.s = 0;
+    color_b.v = 50;
+    tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), ==, 0);
+
+    // distance should be greater when saturation is greater
+    color_a.h = 30;
+    color_a.s = 20;
+    color_a.v = 70;
+
+    color_b.h = 60;
+    color_b.s = 20;
+    color_b.v = 70;
+
+    dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b);
+
+    color_a.h = 30;
+    color_a.s = 40;
+    color_a.v = 70;
+
+    color_b.h = 60;
+    color_b.s = 40;
+    color_b.v = 70;
+
+    tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), >, dist);
+
+    // resolve colors that are close
+    color_a.h = 30;
+    color_a.s = 20;
+    color_a.v = 70;
+
+    color_b.h = 35;
+    color_b.s = 20;
+    color_b.v = 70;
+
+    tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), >, 0);
+
+    color_a.h = 30;
+    color_a.s = 20;
+    color_a.v = 70;
+
+    color_b.h = 30;
+    color_b.s = 25;
+    color_b.v = 70;
+
+    tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), >, 0);
+
+    color_a.h = 30;
+    color_a.s = 20;
+    color_a.v = 70;
+
+    color_b.h = 30;
+    color_b.s = 20;
+    color_b.v = 75;
+
+    tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), >, 0);
+
+    // hues 360 and 0 should be the same
+    color_a.h = 360;
+    color_a.s = 100;
+    color_a.v = 100;
+
+    color_b.h = 0;
+    color_b.s = 100;
+    color_b.v = 100;
+    tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), ==, 0);
+
+    // distance between hues 359 and 1 should be smaller than hues 1 and 5
+    color_a.h = 359;
+    color_a.s = 100;
+    color_a.v = 100;
+
+    color_b.h = 1;
+    color_b.s = 100;
+    color_b.v = 100;
+    dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b);
+
+    color_a.h = 1;
+    color_a.s = 100;
+    color_a.v = 100;
+
+    color_b.h = 5;
+    color_b.s = 100;
+    color_b.v = 100;
+
+    tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), >, dist);
+
+    // check distance is monotonous along several color paths. This should catch potential int overflows
+    int prev_dist = 0;
+    bool monotone = true;
+
+    // along saturation
+    color_a.h = 180;
+    color_a.s = 0;
+    color_a.v = 100;
+
+    color_b.h = 180;
+    color_b.s = 0;
+    color_b.v = 100;
+
+    while (color_a.s < 100) {
+        color_a.s += 5;
+        dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b);
+
+        if (dist <= prev_dist) {
+            monotone = false;
+            break;
+        }
+        prev_dist = dist;
+    }
+    tt_want(monotone);
+
+    // along value
+
+    prev_dist = 0;
+    monotone = true;
+
+    color_a.h = 180;
+    color_a.s = 100;
+    color_a.v = 0;
+
+    color_b.h = 180;
+    color_b.s = 100;
+    color_b.v = 0;
+
+    while (color_a.v < 100) {
+        color_a.v += 5;
+        dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b);
+
+        if (dist <= prev_dist) {
+            monotone = false;
+            break;
+        }
+        prev_dist = dist;
+    }
+    tt_want(monotone);
+
+    // along value, saturation 0
+
+    prev_dist = 0;
+    monotone = true;
+
+    color_a.h = 180;
+    color_a.s = 0;
+    color_a.v = 0;
+
+    color_b.h = 180;
+    color_b.s = 0;
+    color_b.v = 0;
+
+    while (color_a.v < 100) {
+        color_a.v += 5;
+        dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b);
+
+        if (dist <= prev_dist) {
+            monotone = false;
+            break;
+        }
+        prev_dist = dist;
+    }
+    tt_want(monotone);
+
+    // along chroma
+
+    prev_dist = 0;
+    monotone = true;
+
+    color_a.h = 180;
+    color_a.s = 100;
+    color_a.v = 100;
+
+    color_b.h = 180;
+    color_b.s = 100;
+    color_b.v = 100;
+
+    for (int i = -19; i < 21; i++) {
+        color_a.s = i < 0 ? -i * 5 : i * 5;
+        color_a.h = i < 0 ? 180 : 0;
+        color_a.v = 10000 / (200 - color_a.s); // constant lightness
+
+        dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b);
+
+        if (dist <= prev_dist) {
+            monotone = false;
+        }
+        prev_dist = dist;
+    }
+    tt_want(monotone);
+
+    // check max distances
+
+    color_a.h = 0;
+    color_a.s = 100;
+    color_a.v = 100;
+
+    color_b.h = 180;
+    color_b.s = 100;
+    color_b.v = 100;
+
+    dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b);
+    tt_want_int_op(dist, >, 390000000);
+    tt_want_int_op(dist, <, 410000000);
+
+    color_a.h = 0;
+    color_a.s = 0;
+    color_a.v = 0;
+
+    color_b.h = 0;
+    color_b.s = 0;
+    color_b.v = 100;
+
+    dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b);
+    tt_want_int_op(dist, >, 390000000);
+    tt_want_int_op(dist, <, 410000000);
+}
+
 struct testcase_t pbio_color_tests[] = {
     PBIO_TEST(test_rgb_to_hsv),
     PBIO_TEST(test_hsv_to_rgb),
     PBIO_TEST(test_color_to_hsv),
     PBIO_TEST(test_color_to_rgb),
     PBIO_TEST(test_color_hsv_compression),
+    PBIO_TEST(test_color_hsv_cost),
     END_OF_TESTCASES
 };
diff --git a/lib/pbio/test/src/test_color_light.c b/lib/pbio/test/src/test_color_light.c
index d4f70a1e9..346599b68 100644
--- a/lib/pbio/test/src/test_color_light.c
+++ b/lib/pbio/test/src/test_color_light.c
@@ -31,13 +31,13 @@ static const uint16_t test_blink[] = {
 static const pbio_color_compressed_hsv_t test_animation[] = {
     PBIO_COLOR_LIGHT_ANIMATION_CELL(PBIO_COLOR_HUE_CYAN, 100, 100),
     PBIO_COLOR_LIGHT_ANIMATION_CELL(PBIO_COLOR_HUE_MAGENTA, 100, 100),
-    PBIO_COLOR_LIGHT_ANIMATION_END
+    PBIO_COLOR_LIGHT_ANIMATION_END_HSV
 };
 
 static pbio_error_t test_light_set_hsv(pbio_color_light_t *light, const pbio_color_hsv_t *hsv) {
     test_light_set_hsv_call_count++;
     test_light_set_hsv_last_hue = hsv->h;
-    test_light_set_hsv_last_brightness = hsv->v;
+    test_light_set_hsv_last_brightness = pbio_color_hsv_get_v(hsv);
     return PBIO_SUCCESS;
 }
 
diff --git a/pybricks/common/pb_type_colorlight_internal.c b/pybricks/common/pb_type_colorlight_internal.c
index d6a0e573a..6d93a7664 100644
--- a/pybricks/common/pb_type_colorlight_internal.c
+++ b/pybricks/common/pb_type_colorlight_internal.c
@@ -116,7 +116,7 @@ STATIC mp_obj_t common_ColorLight_internal_animate(size_t n_args, const mp_obj_t
     }
 
     // sentinel value
-    cells[colors_len].v = UINT8_MAX;
+    cells[colors_len].v = PBIO_COLOR_LIGHT_ANIMATION_END_V;
 
     mp_int_t interval = pb_obj_get_int(interval_in);
 
diff --git a/pybricks/parameters/pb_type_color.c b/pybricks/parameters/pb_type_color.c
index 3bf9dd2e5..cc1204aed 100644
--- a/pybricks/parameters/pb_type_color.c
+++ b/pybricks/parameters/pb_type_color.c
@@ -12,6 +12,7 @@
 #endif
 
 #include <pbio/color.h>
+#include <pbio/int_math.h>
 
 #include "py/objstr.h"
 
@@ -67,7 +68,7 @@ const pb_type_Color_obj_t pb_Color_MAGENTA_obj = {
 
 const pb_type_Color_obj_t pb_Color_NONE_obj = {
     {&pb_type_Color},
-    .hsv = {0, 0, 0}
+    .hsv = {0, 0, -40}
 };
 
 const pb_type_Color_obj_t pb_Color_BLACK_obj = {
@@ -112,12 +113,10 @@ static mp_obj_t pb_type_Color_make_new_helper(mp_int_t h, mp_int_t s, mp_int_t v
     self->hsv.h = h < 0 ? h + 360 : h;
 
     // Bind s to 0--100
-    s = s < 0 ? 0 : s;
-    self->hsv.s = s > 100 ? 100 : s;
+    self->hsv.s = pbio_int_math_bind(s, 0, 100);
 
-    // Bind v to 0--100
-    v = v < 0 ? 0 : v;
-    self->hsv.v = v > 100 ? 100 : v;
+    // Bind v to -100 to 100
+    self->hsv.v = pbio_int_math_clamp(v, 100);
 
     return MP_OBJ_FROM_PTR(self);
 }
@@ -161,7 +160,7 @@ void pb_type_Color_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kin
 
     // Otherwise, print hsv representation that can be evaluated
     pb_type_Color_obj_t *self = MP_OBJ_TO_PTR(self_in);
-    mp_printf(print, "Color(h=%u, s=%u, v=%u)", self->hsv.h, self->hsv.s, self->hsv.v);
+    mp_printf(print, "Color(h=%u, s=%u, v=%d)", self->hsv.h, self->hsv.s, self->hsv.v);
 }
 
 STATIC mp_obj_t pb_type_Color_subscr(mp_obj_t self_in, mp_obj_t index, mp_obj_t value) {
diff --git a/pybricks/pupdevices/pb_type_pupdevices_colorlightmatrix.c b/pybricks/pupdevices/pb_type_pupdevices_colorlightmatrix.c
index 15d0219d8..93595d4c5 100644
--- a/pybricks/pupdevices/pb_type_pupdevices_colorlightmatrix.c
+++ b/pybricks/pupdevices/pb_type_pupdevices_colorlightmatrix.c
@@ -37,7 +37,7 @@ STATIC uint8_t get_color_id(mp_obj_t color_in) {
     const pbio_color_hsv_t *hsv = pb_type_Color_get_hsv(color_in);
 
     // Brightness is defined in 10 increments.
-    uint8_t brightness = hsv->v / 10;
+    uint8_t brightness = pbio_color_hsv_get_v(hsv) / 10;
     pb_powered_up_color_id_t color;
 
     // For low saturation, assume grayscale.
diff --git a/pybricks/util_mp/pb_obj_helper.c b/pybricks/util_mp/pb_obj_helper.c
index fb386e8a3..fa70edf33 100644
--- a/pybricks/util_mp/pb_obj_helper.c
+++ b/pybricks/util_mp/pb_obj_helper.c
@@ -3,6 +3,7 @@
 
 #include <pbio/color.h>
 #include <pbio/error.h>
+#include <pbio/int_math.h>
 
 #include "py/builtin.h"
 #include "py/mpconfig.h"
@@ -104,7 +105,7 @@ mp_int_t pb_obj_get_hue(mp_obj_t arg) {
 void pb_obj_get_hsv(mp_obj_t arg, pbio_color_hsv_t *hsv) {
     hsv->h = pb_obj_get_hue(mp_obj_subscr(arg, MP_OBJ_NEW_SMALL_INT(0), MP_OBJ_SENTINEL));
     hsv->s = pb_obj_get_pct(mp_obj_subscr(arg, MP_OBJ_NEW_SMALL_INT(1), MP_OBJ_SENTINEL));
-    hsv->v = pb_obj_get_pct(mp_obj_subscr(arg, MP_OBJ_NEW_SMALL_INT(2), MP_OBJ_SENTINEL));
+    hsv->v = pbio_int_math_clamp(pb_obj_get_int(mp_obj_subscr(arg, MP_OBJ_NEW_SMALL_INT(2), MP_OBJ_SENTINEL)), 100);
 }
 
 mp_obj_t pb_obj_new_fraction(int32_t numerator, int32_t denominator) {
diff --git a/pybricks/util_pb/pb_color_map.c b/pybricks/util_pb/pb_color_map.c
index 23cb3d9b4..6e8b506d2 100644
--- a/pybricks/util_pb/pb_color_map.c
+++ b/pybricks/util_pb/pb_color_map.c
@@ -13,6 +13,7 @@
 #include <stdio.h>
 
 #include <pbio/error.h>
+#include <pbio/color.h>
 
 #include "py/obj.h"
 
@@ -31,16 +32,6 @@ void pb_color_map_rgb_to_hsv(const pbio_color_rgb_t *rgb, pbio_color_hsv_t *hsv)
     // Standard conversion
     pbio_color_rgb_to_hsv(rgb, hsv);
 
-    // For very low values, saturation is not reliable
-    if (hsv->v <= 3) {
-        hsv->s = 0;
-    }
-
-    // For very low values, hue is not reliable
-    if (hsv->s <= 3) {
-        hsv->h = 0;
-    }
-
     // Slight shift for lower hues to make yellow somewhat more accurate
     if (hsv->h < 40) {
         uint8_t offset = ((hsv->h - 20) << 8) / 20;
@@ -71,34 +62,6 @@ void pb_color_map_save_default(mp_obj_t *color_map) {
     *color_map = MP_OBJ_FROM_PTR(&pb_color_map_default);
 }
 
-// Cost function between two colors a and b. The lower, the closer they are.
-static int32_t get_hsv_cost(const pbio_color_hsv_t *x, const pbio_color_hsv_t *c) {
-
-    // Calculate the hue error
-    int32_t hue_error;
-
-    if (c->s <= 5 || x->s <= 5) {
-        // When comparing against unsaturated colors,
-        // the hue error is not so relevant.
-        hue_error = 0;
-    } else {
-        hue_error = c->h > x->h ? c->h - x->h : x->h - c->h;
-        if (hue_error > 180) {
-            hue_error = 360 - hue_error;
-        }
-    }
-
-    // Calculate the value error:
-    int32_t value_error = x->v > c->v ? x->v - c->v : c->v - x->v;
-
-    // Calculate the saturation error, with extra penalty for low saturation
-    int32_t saturation_error = x->s > c->s ? x->s - c->s : c->s - x->s;
-    saturation_error += (100 - c->s) / 2;
-
-    // Total error
-    return hue_error * hue_error + 5 * saturation_error * saturation_error + 2 * value_error * value_error;
-}
-
 // Get a discrete color that matches the given hsv values most closely
 mp_obj_t pb_color_map_get_color(mp_obj_t *color_map, pbio_color_hsv_t *hsv) {
 
@@ -116,7 +79,7 @@ mp_obj_t pb_color_map_get_color(mp_obj_t *color_map, pbio_color_hsv_t *hsv) {
     for (size_t i = 0; i < n; i++) {
 
         // Evaluate the cost function
-        cost_now = get_hsv_cost(hsv, pb_type_Color_get_hsv(colors[i]));
+        cost_now = pbio_color_get_bicone_squared_distance(hsv, pb_type_Color_get_hsv(colors[i]));
 
         // If cost is less than before, update the minimum and the match
         if (cost_now < cost_min) {