Skip to content
Merged
19 changes: 19 additions & 0 deletions tests/hsv_tests.cc
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
#include "doctest.h"
#include "util/hsv.hh"

TEST_CASE("HSV to Color") {
// check primary colors
CHECK(Color{HSVColor{0, 255, 255}} == Colors::red);
CHECK(Color{HSVColor{86, 255, 255}} == Colors::green);
CHECK(Color{HSVColor{172, 255, 255}} == Colors::blue);

// check no saturation
const auto ns = Color{HSVColor{172, 0, 255}};
CHECK(ns.red() == ns.green());
CHECK(ns.red() == ns.blue());
CHECK(ns.red() == 255);

// check value
const auto v = Color{HSVColor{0, 255, 0}};
CHECK(v == Colors::black);
}
55 changes: 55 additions & 0 deletions tests/quadrature_encoder_tests.cc
Original file line number Diff line number Diff line change
@@ -0,0 +1,55 @@
#include "doctest.h"
#include "util/quadrature_encoder.hh"

TEST_CASE("Quadrature Encoder") {

using Enc = QuadratureEncoder<>;

Enc encoder{};

// check clockwise
CHECK(encoder.get_motion(true, false) == Enc::None);
CHECK(encoder.get_motion(false, false) == Enc::None);
CHECK(encoder.get_motion(false, true) == Enc::None);
CHECK(encoder.get_motion(true, true) == Enc::CW);

// check clockwise no state change
CHECK(encoder.get_motion(true, false) == Enc::None);
CHECK(encoder.get_motion(true, false) == Enc::None);
CHECK(encoder.get_motion(true, false) == Enc::None);
CHECK(encoder.get_motion(true, false) == Enc::None);

CHECK(encoder.get_motion(false, false) == Enc::None);
CHECK(encoder.get_motion(false, false) == Enc::None);
CHECK(encoder.get_motion(false, false) == Enc::None);
CHECK(encoder.get_motion(false, false) == Enc::None);

CHECK(encoder.get_motion(false, true) == Enc::None);
CHECK(encoder.get_motion(false, true) == Enc::None);
CHECK(encoder.get_motion(false, true) == Enc::None);
CHECK(encoder.get_motion(false, true) == Enc::None);

CHECK(encoder.get_motion(true, true) == Enc::CW);
CHECK(encoder.get_motion(true, true) == Enc::None);
CHECK(encoder.get_motion(true, true) == Enc::None);
CHECK(encoder.get_motion(true, true) == Enc::None);

// check counter-clockwise
CHECK(encoder.get_motion(false, true) == Enc::None);
CHECK(encoder.get_motion(false, false) == Enc::None);
CHECK(encoder.get_motion(true, false) == Enc::None);
CHECK(encoder.get_motion(true, true) == Enc::CCW);

// check with no state change
CHECK(encoder.get_motion(false, true) == Enc::None);
CHECK(encoder.get_motion(false, true) == Enc::None);

CHECK(encoder.get_motion(false, false) == Enc::None);
CHECK(encoder.get_motion(false, false) == Enc::None);

CHECK(encoder.get_motion(true, false) == Enc::None);
CHECK(encoder.get_motion(true, false) == Enc::None);

CHECK(encoder.get_motion(true, true) == Enc::CCW);
CHECK(encoder.get_motion(true, true) == Enc::None);
}
39 changes: 39 additions & 0 deletions util/hsv.hh
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
#pragma once

#include "util/colors.hh"
#include <cstdint>

class HSVColor {
public:
uint8_t h;
uint8_t s;
uint8_t v;

constexpr operator Color() const {
if (!s) {
return Color{v, v, v};
}

const auto region = h / 43u;
const auto remainder = (h - (region * 43u)) * 6u;

const uint8_t p = (v * (255u - s)) >> 8u;
const uint8_t q = (v * (255u - ((s * remainder) >> 8u))) >> 8u;
const uint8_t t = (v * (255u - ((s * (255u - remainder)) >> 8u))) >> 8u;

switch (region) {
case 0:
return Color{v, t, p};
case 1:
return Color{q, v, p};
case 2:
return Color{p, v, t};
case 3:
return Color{p, q, v};
case 4:
return Color{t, p, v};
default:
return Color{v, p, q};
}
}
};
33 changes: 33 additions & 0 deletions util/quadrature_encoder.hh
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
#pragma once

#include <cstdint>

template<bool invert = false>
class QuadratureEncoder {
static constexpr uint8_t valid_ccw = 0b10000111 ^ (invert ? 0xff : 0x00);
static constexpr uint8_t valid_cw = 0b01001011 ^ (invert ? 0xff : 0x00);

uint8_t prev_state{};

public:
enum Direction { None = 0, CW = 1, CCW = -1 };

Direction get_motion(bool state_a, bool state_b) {
const auto cur_state = state_a | (state_b << 1u);

if ((prev_state & 0b11) == cur_state)
return None;

prev_state <<= 2;
prev_state |= cur_state;

switch (prev_state) {
case valid_cw:
return CW;
case valid_ccw:
return CCW;
default:
return None;
}
}
};
Loading