50 lines
1.0 KiB
C++
50 lines
1.0 KiB
C++
#include "Gear.h"
|
|
|
|
static const int8_t GEAR_MAP[3][3] = {
|
|
{ 5, 3, 1 },
|
|
{ -2, 0, -2 },
|
|
{ -1, 4, 2 },
|
|
};
|
|
|
|
GearSelector::GearSelector(uint8_t xPin, uint8_t yPin, int threshLow, int threshHigh)
|
|
: _xPin(xPin), _yPin(yPin),
|
|
_threshLow(threshLow), _threshHigh(threshHigh),
|
|
_currentGear(0) {}
|
|
|
|
bool GearSelector::update() {
|
|
AxisPos x = _quantize(analogRead(_xPin));
|
|
AxisPos y = _quantize(analogRead(_yPin));
|
|
|
|
int newGear = _mapToGear(x, y);
|
|
|
|
if (newGear == -2) return false;
|
|
|
|
if (newGear != _currentGear) {
|
|
delay(100);
|
|
_currentGear = newGear;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
int GearSelector::getGear() const {
|
|
return _currentGear;
|
|
}
|
|
|
|
char GearSelector::getGearChar() const {
|
|
if (_currentGear == -1) return 'R';
|
|
return '0' + _currentGear;
|
|
}
|
|
|
|
AxisPos GearSelector::_quantize(int val) const {
|
|
if (val < _threshLow) return POS_LOW;
|
|
if (val > _threshHigh) return POS_HIGH;
|
|
return POS_CENTRE;
|
|
}
|
|
|
|
int GearSelector::_mapToGear(AxisPos x, AxisPos y) const {
|
|
int xi = (int)x + 1;
|
|
int yi = (int)y + 1;
|
|
return GEAR_MAP[xi][yi];
|
|
}
|