diff --git a/src/avr/src/config.h b/src/avr/src/config.h
index 7b01aac..702fb2a 100644
--- a/src/avr/src/config.h
+++ b/src/avr/src/config.h
@@ -201,7 +201,7 @@ enum {
 #define I2C_DEV                  TWIC
 #define I2C_ISR                  TWIC_TWIS_vect
 #define I2C_ADDR                 0x2b
-#define I2C_MAX_DATA             8
+#define I2C_MAX_DATA             16
 
 
 // Motor
diff --git a/src/avr/src/i2c.c b/src/avr/src/i2c.c
index d115a1b..5832d3c 100644
--- a/src/avr/src/i2c.c
+++ b/src/avr/src/i2c.c
@@ -62,7 +62,9 @@ static void _i2c_end_command() {
 
 
 static void _i2c_command_byte(uint8_t byte) {
-  i2c.data[i2c.length++] = byte;
+  if (i2c.length < I2C_MAX_DATA) {
+    i2c.data[i2c.length++] = byte;
+  }
 }
 
 
diff --git a/src/py/bbctrl/Mach.py b/src/py/bbctrl/Mach.py
index 6e64f2b..a9a6943 100644
--- a/src/py/bbctrl/Mach.py
+++ b/src/py/bbctrl/Mach.py
@@ -123,6 +123,12 @@ class Mach(Comm):
         if log['msg'] == 'Switch not found':
             self.estop()
 
+    def _end_cycle(self):
+        if (self._get_cycle() != 'idle' and self._is_ready()
+                and not self.planner.is_busy() and not super().is_active()):
+            self.planner.position_change()
+            self._set_cycle('idle')
+
     def _update(self, update):
         # Detect motor faults
         for motor in range(4):
@@ -138,11 +144,9 @@ class Mach(Comm):
         if state_changed and state == 'ESTOPPED':
             self.planner.reset(stop=False)
 
-        # Exit cycle if state changed to READY
-        if (state_changed and self._get_cycle() != 'idle' and self._is_ready()
-                and not self.planner.is_busy() and not super().is_active()):
-            self.planner.position_change()
-            self._set_cycle('idle')
+        # Check if cycle has ended
+        if state_changed:
+            self._end_cycle()
 
         # Planner stop
         if state == 'READY' and self.stopping:
@@ -191,8 +195,13 @@ class Mach(Comm):
 
     @overrides(Comm)
     def comm_next(self):
-        if self.planner.is_running() and not self._is_holding():
-            return self.planner.next()
+        shouldFetch = self.planner.is_running() and not self._is_holding()
+        cmd = self.planner.next() if shouldFetch else None
+
+        if cmd is None:
+            self._end_cycle()
+
+        return cmd
 
     @overrides(Comm)
     def comm_error(self):