Add battery power indicator

src/LmEncoderOnBoardMotor.cpp

@@ -12,10 +12,23 @@
 #include <math.h>
 #include <float.h>
 
+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);

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