 #define I2C_SLAVE_ADDRESS 0x4
 #include <TinyWireS.h>
 
-// The default buffer size, Can't recall the scope of defines right now
-#ifndef TWI_RX_BUFFER_SIZE
-#define TWI_RX_BUFFER_SIZE ( 16 )
+enum I2cRegisters {
+  I2cRegLeftServoLo = 0,
+  I2cRegLeftServoHi,
+  I2cRegRightServoLo,
+  I2cRegRightServoHi,
+  I2cRegBatteryLo,
+  I2cRegBatteryHi,
+  I2cRegCount
+};
+
+volatile uint8_t currentRegister = 0;
+
+uint16_t leftServoMicros = 1500;
+uint16_t rightServoMicros = 1500;
+uint16_t batteryMicroVolts = 0;
+
+#if F_CPU == 8000000L
+    // Divide FCLK by 64 -> 8 MHz/64 = 125 KHz (each tick is 8 uS)
+    #define TCCR1_CS_BITS 0x7
+#else
+    #error Unexpected CPU speed
 #endif
 
-// Tracks the current register pointer position
-volatile byte reg_position;
-const byte reg_count = 3;
-
-/**
- * This is called for each read request we receive, never put more than one byte of data (with TinyWireS.send) to the
- * send-buffer when using this callback
+/*
+ * Handle a 'get' request from the I2C master.
  */
 void requestEvent()
 {
     byte value = 0;
-    switch (reg_position) {
-    case 0:
-       value = OCR1A;
-       break;
-    case 1:
-       value = OCR1B;
-       break;
-    case 2:
-       value = byte(analogRead(3) >> 2);
+    switch (currentRegister) {
+    case I2cRegLeftServoLo:
+       value = lowByte(leftServoMicros);
+       break;
+    case I2cRegLeftServoHi:
+       value = highByte(leftServoMicros);
+       break;
+    case I2cRegRightServoLo:
+       value = lowByte(rightServoMicros);
+       break;
+    case I2cRegRightServoHi:
+       value = highByte(rightServoMicros);
+       break;
+    case I2cRegBatteryLo:
+       value = lowByte(batteryMicroVolts);
+       break;
+    case I2cRegBatteryHi:
+       value = highByte(batteryMicroVolts);
        break;
     }
     TinyWireS.send(value);
-    reg_position++;
-    if (reg_position >= reg_count)
-       reg_position = 0;
-}
-
-/**
- * The I2C data receive handler
- *
- * This needs to complete before the next incoming transaction (start, data, restart/stop) on the bus does
- * so be quick, set flags for long running tasks to be called from the mainloop instead of running them directly,
+    currentRegister++;
+    if (currentRegister >= I2cRegCount)
+       currentRegister = 0;
+}
+
+/*
+ * Handle a 'set' request from the I2C master.
  */
 void receiveEvent(uint8_t howMany)
 {
+   static uint16_t nextLeftServoMicros = 0;
+   static uint16_t nextRightServoMicros = 0;
+
     // Sanity check
-    if (howMany < 1 || howMany > TWI_RX_BUFFER_SIZE)
+    if (howMany < 1 || howMany > I2cRegCount)
       return;
 
     // Read register position
-    reg_position = TinyWireS.receive();
+    currentRegister = TinyWireS.receive();
     howMany--;
 
-    if (reg_position >= reg_count) {
+    if (currentRegister >= I2cRegCount) {
        // If invalid register, ignore the request
-       reg_position = 0;
+       currentRegister = 0;
        return;
     }
     

 * 50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
 * 60: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
 * 70: -- -- -- -- -- -- -- --
 * # i2cget -y 1 4 0  ## get left PWM duration
 * 0x60
 * # i2cget -y 1 4 1  ## get right PWM duration
 * 0x60
 * # i2cget -y 1 4 2  ## get battery voltage/2 (0-255)
 * 0xbe
 * # i2cset -y 1 4 0 160 ## set left PWM duration
 * # i2cset -y 1 4 1 160 ## set right PWM duration
 *
 * Programming an ATtiny85 with an Arduino Uno
 *

 *      Arduino Pin 12 ---> ATtiny Pin 6
 *      Arduino Pin 13 ---> ATtiny Pin 7
 *      Arduino Reset, Arduino group -> connect with 10 uF capacitor
 * 8. Burn bootloader (VERY, VERY IMPORTANT. I learned the hard way.)
 * 9. Compile and upload this program.
 */


