diff --git a/src/LmEncoderOnBoardMotor.cpp b/src/LmEncoderOnBoardMotor.cpp index 9170a13..1f0de52 100644 --- a/src/LmEncoderOnBoardMotor.cpp +++ b/src/LmEncoderOnBoardMotor.cpp @@ -12,10 +12,23 @@ #include #include +enum { + /* get new battery level */ + NEW, + /* switch indicator LED on */ + ON, + /* switch indicator LED off */ + OFF, +}; + MeEncoderOnBoard EncoderOnBoardMotor::encoder1(SLOT1); MeEncoderOnBoard EncoderOnBoardMotor::encoder2(SLOT2); bool EncoderOnBoardMotor::pos_reached[] = { false, false }; bool EncoderOnBoardMotor::synced = false; +bool EncoderOnBoardMotor::on = false; +int EncoderOnBoardMotor::state = NEW; +int EncoderOnBoardMotor::counter = 0; +int EncoderOnBoardMotor::level = 0; void EncoderOnBoardMotor::isrProcessEncoder1(void) { @@ -33,6 +46,41 @@ void EncoderOnBoardMotor::isrProcessEncoder2(void) encoder2.pulsePosPlus(); } +void EncoderOnBoardMotor::indicateBatteryPower(void) +{ + + switch (state) { + case NEW: + level = analogRead(A4); + /* dim exponentially */ + level /= 10; + level *= level; + counter = level; + state = ON; + break; + + case ON: + digitalWrite(13, HIGH); + + if (counter <= 0) { + /* fully charged batteries are about ~500 */ + counter = 600 - level; + state = OFF; + } + break; + + case OFF: + digitalWrite(13, LOW); + + if (counter <= 0) { + state = NEW; + } + break; + } + + counter--; +} + EncoderOnBoardMotor::EncoderOnBoardMotor(int slot, float ratio) : slot(slot) { //Set PWM 8KHz @@ -42,6 +90,9 @@ EncoderOnBoardMotor::EncoderOnBoardMotor(int slot, float ratio) : slot(slot) TCCR2A = _BV(WGM21) | _BV(WGM20); TCCR2B = _BV(CS21); + pinMode(13, OUTPUT); + Timer::registerCallback(indicateBatteryPower); + switch (slot) { case SLOT1: encoder1.reset(slot); diff --git a/src/LmEncoderOnBoardMotor.h b/src/LmEncoderOnBoardMotor.h index 73b1bd1..e30fbf5 100644 --- a/src/LmEncoderOnBoardMotor.h +++ b/src/LmEncoderOnBoardMotor.h @@ -21,7 +21,8 @@ class EncoderOnBoardMotor public: /** - * Constructor to assign one of the two onboard slots to a + * Initialize the motor encoder and start a battery indicator + * dimming the blue LED 13 according to battery power. * * @param slot On board encoder slot to use * (SLOT1 is right, SLOT_2 is left) @@ -104,6 +105,11 @@ class EncoderOnBoardMotor static int slot2Index(int slot); static MeEncoderOnBoard *slot2Encoder(int slot); int slot; + static void indicateBatteryPower(void); + static bool on; + static int state; + static int counter; + static int level; }; //! @cond SuppressGuard